diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index abe046f4..53421726 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -1,5 +1,8 @@ -name: Library Compile -on: push + +name: MinimalBuild +on: + push: + branches: [minimal] jobs: build: name: Test compiling examples for UNO @@ -10,4 +13,5 @@ jobs: - name: Compile all examples uses: ArminJo/arduino-test-compile@v1.0.0 with: - libraries: PciManager + libraries: PciManager + examples-exclude: esp32_bldc_magnetic_spi, esp32_stepper_openloop, stm32_bldc_encoder, stm32_bldc_hall diff --git a/README.md b/README.md index f8974bcc..a46577e6 100644 --- a/README.md +++ b/README.md @@ -1,446 +1,281 @@ -# Arduino Field Oriented Control (FOC) library +# Arduino *SimpleFOClibrary* v2.0.1 - minimal project builder - -![Library Compile](https://github.com/askuric/Arduino-FOC/workflows/Library%20Compile/badge.svg) +![MinimalBuild](https://github.com/askuric/Arduino-FOC/workflows/MinimalBuild/badge.svg?branch=minimal) [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) - - -Proper low cost FOC supporting boards are very hard to find these days and even may not exist. The reason may be that the hobby community has not yet dug into it properly. Therefore this is the attempt to demistify the Field Oriented Control (FOC) algorithm and make a robust but simple implementation for usage with Arduino hadrware. - -### This project aims to close the gap in the areas: -- Low cost applications <50$ -- Low current operation < 5A -- Simple usage and scalability (Arduino) - and demistify FOC control in a simple way. - - -#### The closest you can get to FOC support and low cost (I was able to find) is: - -Odroid | Trinamic ------------- | ------------- - | -:heavy_check_mark: Open Source | :x: Open Source -:heavy_check_mark:Simple to use | :heavy_check_mark: Simple to use -:x: Low cost ($100) | :x: Low cost ($100) -:x: Low power (>50A) | :heavy_check_mark: Low power - -Infineon | FOC-Arduino-Brushless ------------- | ------------- -| -:x: Open Source | :heavy_check_mark: Open Source -:heavy_check_mark:Simple to use | :x: Simple to use -:heavy_check_mark:Low cost ($40) | :heavy_check_mark: Low cost -:heavy_check_mark: Low power | :heavy_check_mark: Low power - - - -# Electrical connections - -### All you need for this project is (an exaple in brackets): - - Brushless DC motor - 3 pahse (IPower GBM4198H-120T [Ebay](https://www.ebay.com/itm/iPower-Gimbal-Brushless-Motor-GBM4108H-120T-for-5N-7N-GH2-ILDC-Aerial-photo-FPV/252025852824?hash=item3aade95398:g:q94AAOSwPcVVo571:rk:2:pf:1&frcectupt=true)) - - Encoder - ( Incremental 2400cpr [Ebay](https://www.ebay.com/itm/600P-R-Photoelectric-Incremental-Rotary-Encoder-5V-24V-AB-2-Phases-Shaft-6mm-New/173145939999?epid=19011022356&hash=item28504d601f:g:PZsAAOSwdx1aKQU-:rk:1:pf:1)) -- Arduino + BLDC motor driver ( L6234 driver [Drotek](https://store-drotek.com/212-brushless-gimbal-controller-l6234.html), [Ebay](https://www.ebay.fr/itm/L6234-Breakout-Board-/153204519965)) - -Alternatively the library supports the arduino based gimbal controllers such as: -- HMBGC V2.2 ([Ebay](https://www.ebay.com/itm/HMBGC-V2-0-3-Axle-Gimbal-Controller-Control-Plate-Board-Module-with-Sensor/351497840990?hash=item51d6e7695e:g:BAsAAOSw0QFXBxrZ:rk:1:pf:1)) - - -## Arduino FOC Shield V1.2 - -At this moment we are developing an open source version of Arduin shiled specifically for FOC motor control. -We already have prototypes of the board and we are in the testing phase. We will be coming out with the details very soon! - -### Features -- Plug and play capability with the Arduino FOC library -- Price in the range of \$20-\$40 -- Gerber files and BOM available Open Source - -***Let me know if you are interested! antun.skuric@outlook.com*** -You can explore the [3D model of the board in the PDF form](extras/ArduinoFOCShieldV12.pdf). - - - - -## Arduino UNO + L6234 breakout broad -The code is simple enough to be run on Arudino Uno board. - -

- -

- -### Encoder -- Encoder channels `A` and `B` are connected to the Arduino's external intrrupt pins `2` and `3`. -- Optionally if your encoder has `index` signal you can connect it to any available pin, figure shows pin `4`. - - The library doesnt support the Index pin for now (version v1.1.0) -### L6234 breakout board -- Connected to the arduino pins `9`,`10` and `11`. -- Additionally you can connect the `enable` pin to the any digital pin of the arduino the picture shows pin `8` but this is optional. You can connect the driver enable directly to 5v. -- Make sure you connect the common ground of the power supply and your Arduino -### Motor -- Motor phases `a`, `b` and `c` are connected directly to the driver outputs - -Motor phases `a`,`b`,`c` and encoder channels `A` and `B` have to be oriented right for the algorightm to work. But don't worry about it too much. Connect it in initialy as you wish and then if it doesnt move reverse pahse `a` and `b` of the motor, that should be enogh. - - -## HMBGC V2.2 -To use HMBGC controller for vector control (FOC) you need to connect motor to one of the motor terminals and connect the Encoder. The shema of connection is shown on the figures above, I also took a (very bad) picture of my setup. - -

- - -

- - -### Encoder -Since HMBGC doesn't have acces to the arduinos external interrupt pins `2` and `3` and additionally we only have acces to the analog pins, we need to read the encoder using the software interrupt. To show the functionallity we provide one example of the HMBGC code (`HMBGC_example.ino`) using the [PciManager library](https://github.com/prampec/arduino-pcimanager). - -- Encoder channels `A` and `B` are connected to the pins `A0` and `A1`. -- Optionally if your encoder has `index` signal you can connect it to any available pin, figure shows pin `A2`. - - The library doesnt support the Index pin for now (version v1.1.0) -### Motor -- Motor phases `a`,`b` and `c` are connected directly to the driver outputs - -Motor phases `a`,`b`,`c` and encoder channels `A` and `B` have to be oriented right for the algorightm to work. But don't worry about it too much. Connect it in initialy as you wish and then if it doesnt move reverse pahse `a` and `b` of the motor, that should be enogh. - - - - -# Arduino FOC library code -The code is organised into a librarie. The library contains two classes `BLDCmotor` and `Endcoder`. `BLDCmotor` contains all the necessary FOC algorithm funcitons as well as PI controllers for the velocity and angle control. `Encoder` deals with the encoder interupt funcitons, calcualtes motor angle and velocity( using the [Mixed Time Frequency Method](https://github.com/askuric/Arduino-Mixed-Time-Frequency-Method)). - -## Encoder setup -To initialise the encoder you need to provide the encoder `A` and `B` channel pins, encoder `PPR` and optionally `index` pin. -```cpp -// Encoder(int encA, int encB , int cpr, int index) -// - encA, encB - encoder A and B pins -// - ppr - impulses per rotation (cpr=ppr*4) -// - index pin - (optional input) -Encoder encoder = Encoder(2, 3, 8192, 4); -``` -Additionally the encoder has one more important parameters which is whether you want to use Arduino's internal pullup or you have external one. That is set by changing the value of the `encoder.pullup` variuable. The default value is set to `Pullup::EXTERN` -```cpp -// check if you need internal pullups -// Pullup::EXTERN - external pullup added - dafault -// Pullup::INTERN - needs internal arduino pullup -encoder.pullup = Pullup::EXTERN; +[![arduino-library-badge](https://www.ardu-badge.com/badge/Simple%20FOC.svg?)](https://www.ardu-badge.com/badge/Simple%20FOC.svg) + +This is the branch of the [*SimpleFOClibrary*](https://github.com/askuric/Arduino-FOC) repository intended to be used to simplify the creation of the projects with minimal code possible which is specific for certain **motor+sensor+driver** combination. + +### Repository structure +Library source code structure +```shell +├─── library_source +| | +| ├─ BLDCMotor.cpp/h # BLDC motor handling class +| ├─ StepperMotor.cpp/h # Stepper motor handling class +| | +│ ├─── common # Contains all the common utility classes and functions +| | | +| | ├─ defaults.h # default motion control parameters +| | ├─ foc_utils.cpp./h # utility functions of the FOC algorithm +| | ├─ time_utils.cpp/h # utility functions for dealing with time measurements and delays +| | ├─ pid.cpp./h # class implementing PID controller +| | ├─ lowpass_filter.cpp./h # class implementing Low pass filter +| | | +| | ├─── base_classes +| | | ├─ FOCMotor.cpp./h # common class for all implemented motors +| | | ├─ BLDCDriver.h # common class for all BLDC drivers +| | | ├─ StepperDriver.h # common class for all Stepper drivers +| | | └─ Sensor./h # common class for all implemented sensors +| | +| ├─── drivers +| | ├─ BLDCDriver3PWM.cpp/h # Implementation of generic 3PWM bldc driver +| | ├─ BLDCDriver6PWM.cpp/h # Implementation of generic 6PWM bldc driver +| | ├─ StepperDriver2PWM.cpp/h # Implementation of generic 2PWM stepper driver +| | ├─ StepperDriver4PWM.cpp/h # Implementation of generic 4PWM stepper driver +| | | +| | ├─ hardware_api.h # common mcu specific api handling pwm setting and configuration +| | | +| | ├─── hardware_specific # mcu specific hadrware_api.h implementations +| | | ├─ atmega2560_mcu.cpp # ATMega 2560 implementation +| | | ├─ atmega328_mcu.cpp # ATMega 328 (Arduino UNO) implementation +| | | ├─ esp32_mcu.cpp # esp32 implementation +| | | ├─ stm32_mcu.cpp # stm32 implementation +| | | ├─ teensy_mcu.cpp # teensy implementation +| | | └─ generic_mcu./h # generic implementation - if not nay of above (not complete) +| | +| ├─── sensors +| │ ├─ Encoder.cpp/h # Encoder class implementing the Quadrature encoder operations +| │ ├─ MagneticSensorSPI.cpp/h # class implementing SPI communication for Magnetic sensors +| │ ├─ MagneticSensorI2C.cpp/h # class implementing I2C communication for Magnetic sensors +| │ ├─ MagneticSensorAnalog.cpp/h # class implementing Analog output for Magnetic sensors + └─ HallSensor.cpp/h # class implementing Hall sensor ``` -Finally to start the encoder counter and intialise all the periphery pins you need the call of `encoder.init()` is made. -```cpp -// initialise encoder hardware -encoder.init(doA, doB); -``` -Where the functions `doA()` and `doB()` are buffering functions of encoder callback funcitons `encoder.handleA()` and `encoder.handleB()`. -```cpp -// interrupt ruotine intialisation -void doA(){encoder.handleA();} -void doB(){encoder.handleB();} -``` -You can name the funcitons as you wish. It is just important to supply them to the `encoder.init()` funciton. This procedure is a tradeoff in between scalability and simplicity. This allows you to have more than one encoder connected to the same arduino. All you need to do is to instantiate new `Encoder` class and create new buffer functions. For example: -```cpp -// encoder 1 -Encoder enc1 = Encoder(2, 3, 8192, 4); -void doA1(){enc1.handleA();} -void doB1(){enc1.handleB();} -// encoder 2 -Encoder enc2 = Encoder(5, 6, 8192, 7); -void doA2(){enc2.handleA();} -void doB2(){enc2.handleB();} -void setup(){ -... - enc1.init(doA1,doB1); - enc2.init(doA2,doB2); -... -} +Minimal project examples provided for quick start: +```shell +├─── minimal_project_examples # Project examples +│ ├─ atmega2560_stepper_encoder # ATMega2560 + BLDC motor + 3PWM driver + encoder +| | +│ ├─ atmega328_bldc_encoder # ATMega328 + BLDC motor + 3PWM driver + Encoder +│ ├─ atmega328_bldc_magnetic_i2c # ATMega328 + BLDC motor + 3PWM driver + I2C magnetic sensor +│ ├─ atmega328_bldc_openloop # ATMega328 + BLDC motor + 3PWM driver +│ ├─ atmega328_driver_standalone # ATMega328 + 3PWM driver +│ | +│ ├─ esp32_bldc_magnetic_spi # ESP32 + BLDC motor + 3PWM driver + SPI magnetic sensor +│ ├─ esp32_stepper_openloop # ESP32 + Stepper motor + 4PWM driver +| | +│ ├─ stm32_bldc_encoder # stm32 + BLDC motor + 6PWM driver + encoder + └─ stm32_bldc_hall # stm32 + BLDC motor + 3PWM driver + hall sensors ``` -To explore better the encoder algorithm an example is provided `encoder_example.ino`. -## Motor setup -To intialise the motor you need to input the `pwm` pins, number of `pole pairs` and optionally driver `enable` pin. -```cpp -// BLDCMotor( int phA, int phB, int phC, int pp, int en) -// - phA, phB, phC - motor A,B,C phase pwm pins -// - pp - pole pair number -// - enable pin - (optional input) -BLDCMotor motor = BLDCMotor(9, 10, 11, 11, 8); + +# Creating your own minimal project + +Creating your own minimal project version is very simple and is done in four steps: +- Step 0: Download minimal branch contents to your PC +- Step 1: Create your the arduino project +- Step 2: Add **driver** specific code +- Step 3: Add **motor** specific code +- Step 4: Add **sensor** specific code + +## Step 0. Download the code +#### Github website download +- Make sure you are in [minimal branch](https://github.com/askuric/Arduino-FOC/tree/minimal) +- Download the code by clicking on the `Clone or Download > Download ZIP`. +- Unzip it + +#### Using terminal +- Open the terminal: + ```sh + cd *to you desired directory* + git clone -b minimal https://github.com/askuric/Arduino-FOC.git + ``` + +## Step 1. Creating the Arduino project + +Open a directory you want to use as your arduino project directory `my_arduino_project` and create `my_arduino_project.ino` file. After this you create `src` folder in this directory and copy the folder named `common` from the `library_source` folder. Your project directory should now have structure: + +```shell +├─── my_arduino_project +| ├─ my_arduino_project.ino +| └─── src +│ | ├─── common +| | | ├─ defaults.h # default motion control parameters +| | | ├─ foc_utils.cpp./h # utility functions of the FOC algorithm +| | | ├─ time_utils.cpp/h # utility functions for dealing with time measurements and delays +| | | ├─ pid.cpp./h # class implementing PID controller +| | | ├─ lowpass_filter.cpp./h # class implementing Low pass filter +| | | └─── base_classes # common class for all implemented sensors ``` -To finalise the motor setup the encoder is added to the motor and the `init` function is called. -```cpp -// link the motor to the sensor -motor.linkEncoder(&encoder); -// intialise motor -motor.init(); +## Step 2. Add driver specific code +First create a `drivers` folder in `src` folder. If you wish to use the 3PWM or 6PWM BLDC driver in your project with your setup you will have to copy the `BLDCDriver3PWM.cpp/h` files or `BLDCDriver3PWM.cpp/h` files from the `library_source/drivers` folder in your drivers folder. If you wish to use the 4PWM or 2PWM stepper motor make sure to copy the `StepperDriver4PWM.cpp/h` or `StepperDriver2PWM.cpp/h` files and place them to the `src/drivers` folder. +```shell +├─── my_arduino_project +| ├─ my_arduino_project.ino +| └─── src +| | ├─── common # Common utility classes and functions +| | | +│ └─── drivers +| └─ BLDCDriver3PWM.cpp/h # BLDC motor handling class +``` +Next from the `library_source/drivers` directory copy the `hardware_api.h` file to the `src/drivers` folder as well as the `hardware_specific` folder. Finally in the `hardware_specific` folder leave only the `x_mcu.cpp` file which corresponds to your mcu architecture. For example, for esp32 boards +```shell +├─── my_arduino_project +| ├─ my_arduino_project.ino +| └─── src +| | ├─── common # Common utility classes and functions +| | | +│ └─── drivers +| ├─ BLDCDriver3PWM.cpp/h # BLDC driver handling class +| ├─ hardware_api.h # common mcu specific api handling pwm setting and configuration +| └─── hardware_specific # mcu specific hadrware_api.h implementations +| └─ esp32_mcu.cpp # esp32 implementation ``` - -## BLDC Driver parameters -First thing you can configure is your `power_supply_voltage` value. The default value is set to `12V`. If you set your power supply to some other vlaue, chnage it here for the control loops to adapt. +And in your Arduino code in the `my_arduino_project.ino` file make sure to add the the include: ```cpp -// power supply voltage -motor.power_supply_voltage = 12; +#include "src/drivers/BLDCDriver3PWM.h" ``` -You can also change driver type by changing the value of the variable `motor.driver`. It tells the algorithm to generate unipolar of bipolar FOC voltages. This basically means if your BLDC driver can only output voltages in range `[0,power_supply_voltage]` your driver is `DriverType::unipolar` and if it can output voltage in range `[-power_supply_voltage, power_supply_voltage]` than you driver is `DriverType::bipolar` what is case in most of the drivers and what is default value as well. -```cpp -// set driver type -// DriverType::unipolar // HMBGC -// DriverType::bipolar // L6234 (default) -motor.driver = DriverType::bipolar; +For the combination of stepper driver 4pwm and stm32 board the structure will be: +```shell +├─── my_arduino_project +| ├─ my_arduino_project.ino +| └─── src +| | ├─── common # Common utility classes and functions +| | | +│ └─── drivers +| ├─ StepperDriver4PWM.cpp/h # Stepper driver handling class +| ├─ hardware_api.h # common mcu specific api handling pwm setting and configuration +| └─── hardware_specific # mcu specific hadrware_api.h implementations +| └─ stm32_mcu.cpp # stm32 implementation ``` -## Control loop setup -First parameter you can change is the variable you want to control. You set it by changing the `motor.controller` variable. If you want to control the motor angle you will set the `controller` to `ControlType::angle`, if youy seek the DC motor behavior behaviour by controlling the voltage use `ControlType::voltage`, if you wish to control motor angular velocity `ControlType::velocity`. If you wish to control velocities which are very very slow, typically around ~0.01 rad/s you can use the `ControlType::velocity_ultra_slow` controller. +And the include: ```cpp -// set FOC loop to be used -// ControlType::voltage -// ControlType::velocity -// ControlType::velocity_ultra_slow -// ControlType::angle -motor.controller = ControlType::angle; +#include "src/drivers/StepperDriver4PWM.h" ``` -### Voltage control loop -This control loop allows you to run the BLDC motor as it is simple DC motor using Park transformation. This mode is enabled by: -```cpp -// voltage control loop -motor.controller = ControlType::voltage; +If you wish to run your drivers in the standalone mode these are all the files that you will need. See the `atmega328_driver_standalone` project example. + +## Step 3. Add motor specific code +If you wish to use the BLDC motor with your setup you will have to copy the `BLDCMotor.cpp/h` from the `library_source` folder, and if you wish to use the stepper motor make sure to copy the `StepperMotor.cpp/h` files and place them to the `src` folder +```shell +├─── my_arduino_project +| ├─ my_arduino_project.ino +| └─── src +| | ├─── common # Common utility classes and functions +| | ├─── drivers # Driver handling software +| | | +| └─ BLDCMotor.cpp/h # BLDC motor handling class ``` - - -You rcan test this algoithm by running the example `voltage_control.ino`. -The FOC algorithm reads the angle $\textsf{a}$ from the motor and sets appropriate $\textsf{u}_a$, $\textsf{u}_b$ and $\textsf{u}_c$ voltages such to always have $90\degree$ angle in between the magnetic fields of the permanent magents in rotor and the stator. What is exaclty the principle of the DC motor. -> This control loop will give you the motor which spins freely with the velocity depending on the voltage $U_q$ you set and the disturbance it is facing. *It will turn slower if you try to hold it*. - - -### Velocity control loop -This control loop allows you to spin your BLDC motor with desired velocity. This mode is enabled by: +And in your Arduino code in the `my_arduino_project.ino` file make sure to add the the include: ```cpp -// velocity control loop -motor.controller = ControlType::velocity; +#include "src/BLDCMotor.h" ``` - - - -You can test this algorithm by running the example `velocity_control.ino` and `velocity_control_serial.ino` . -The velocity control is created by adding a PI velocity controller. This controller reads the motor velocity $\textsf{v}$ and sets the $\textsf{u}_q$ voltage to the motor in a such maner that it reaches and maintains the target velocity $\textsf{v}_d$, set by the user. -#### PI controller parameters -To change the parameters of your PI controller to reach desired behaiour you can change `motor.PI_velocity` structure: -```cpp -// velocity PI controller parameters -// default K=1.0 Ti = 0.003 -motor.PI_velocity.K = 1; -motor.PI_velocity.Ti = 0.003; -motor.PI_velocity.u_limit = 12; +For stepper motors the procedure is equivalent: + +```shell +├─── my_arduino_project +| ├─ my_arduino_project.ino +| └─── src +| | ├─── common # Common utility classes and functions +| | ├─── drivers # Driver handling software +| | | +| └─ StepperMotor.cpp/h # Stepper motor handling class ``` -The parameters of the PI controller are proportional gain `K`, integral time constant `Ti` and voltage limit `u_limit` which is by default set to the `power_supply_voltage`. -- The `u_limit` parameter is intended if some reason you wish to limit the voltage that can be sent to your motor. -- In general by raising the proportional constant `K` your motor controller will be more reactive, but too much will make it unstable. -- The same goes for integral time constant `Ti` the smaller it is the faster motors reaction to disturbance will be, but too small value will make it unstable. - -So in order to get optimal performance you will have to fiddle a bit with with the parameters. :) - -### Angle control loop -This control loop allows you to move your BLDC motor to the desired angle in real time. This mode is enabled by: +And the include: ```cpp -// angle control loop -motor.controller = ControlType::angle; +#include "src/StepperMotor.h" ``` - - - -You can test this algorithm by running the example `angle_control.ino` and `angle_control_serial.ino` . -The angle control loop is done by adding one more control loop in cascade on the velocity control loop like showed on the figure above. The loop is closed by using simple P controller. The controller reads the angle $\textsf{a}$ from the motor and determins which velocity $\textsf{v}_d$ the motor should move to reach desire angle $\textsf{a}_d$ set by the user. And then the velocity controller reads the current velocity from the motor $\textsf{v}$ and sets the voltage $\textsf{u}_q$ that is neaded to reach the velocity $\textsf{v}_d$, set by the angle loop. - -#### Controller parameters -To tune this control loop you can set the parameters to both angle P controller and velocity PI controller. -```cpp -// velocity PI controller parameters -// default K=1.0 Ti = 0.003 -motor.PI_velocity.K = 0.5; -motor.PI_velocity.Ti = 0.01; -motor.PI_velocity.u_limit = 12; -// angle P controller -// default K=70 -motor.P_angle.K = 20; -// maximal velocity of the poisition control -// default 20 -motor.P_angle.velocity_limit = 10; +If you wish to run your motor in the open loop mode these are all the files that you will need. See the `esp32_stepper_openloop` and `atmega328_bldc_openloop` project examples. + +## Step 4. Add sensor specific code +In order to support the different position sensors you will first have to create the `sensors` folder in your `src` folder. And then copy their `*.cpp` and `*.h` files which correspond to the sensor into your `src/sensors` directory. You can find the sensor implementations in the `library_source/sensors` directory. + + +### Example: Encoder sensor +For example if you wish to use BLDC motor and encoder as a sensor, your arduino project will have structure: +```shell +├─── my_arduino_project +| ├─ my_arduino_project.ino +| └─── src +| | ├─── common # Common utility classes and functions +| | ├─── drivers # Driver handling software +│ ├─── sensors +| | └─ Encoder.cpp/h # Encoder class implementing the Quadrature encoder operations +| | +| └─ BLDCMotor.cpp/h # BLDC motor handling class ``` -It is important to paramter both velocity PI and angle P controller to have the optimal performance. -The velocity PI controller is parametrisized by updating the `motor.PI_velcity` structure as expalined before. -- Rough rule should be to lower the proportional gain `K` and raise time constant `Ti` in order to achieve less vibrations. - -The angle P controller can be updated by changign the `motor.P_angle` structure. -- Roughly proportional gain `K` will make it more responsive, but too high value will make it unstable. - -Additionally you can configure the `velocity_limit` value of the controller. This value prevents the contorller to set too high velocities $v_d$ to the motor. -- If you make your `velocity_limit` very low your motor will be moving in between desired positions with exactly this velocity. If you keep it high, you will not notice that this variable even exists. :D - -Finally, each application is a bit different and the chances are you will have to tune the controller values a bit to reach desired behaviour. - - -### Ultra slow velocity control loop -This control loop allows you to spin your BLDC motor with desired velocity as well as the [velocity loop](#velocity-control-loop) but it is intended for very smooth operation in very low velocityes (< 0.1 rad/s). This mode is enabled by: +And in your in your arduino project `my_arduino_project.ino` add the line: ```cpp -// velocity ultra slow control loop -motor.controller = ControlType::velocity_ultra_slow; +#include "src/sensors/Encoder.h" ``` - - - -You can test this algorithm by running the example `velocity_ultrasloaw_control_serial.ino` . -This type of the velocity control is nothing more but motor angle control. It works particularly well for the purposes of very slow movements because regular velocity calculation techniques are not vel suited for this application and regular [velocity control loop](#velocity-control-loop) would not work well. -The behavior is achieved by integrating the user set target velocity $\textsf{v}_d$ to get the necessary angle $\textsf{a}_d$. And then controlling the motor angle $\textsf{a}$ with high-gain PI controller. This controller reads the motor angle $\textsf{a}$ and sets the $\textsf{u}_q$ voltage to the motor in a such maner that it closely follows the target angle $\textsf{a}_d$, to achieve the velocity profile $\textsf{v}_d$, set by the user. -#### PI controller parameters -To change the parameters of your PI controller to reach desired behaiour you can change `motor.PI_velocity` structure: -```cpp -// velocity PI controller parameters - // default K=120.0 Ti = 100.0 -motor.PI_velocity_ultra_slow.K = 120; -motor.PI_velocity_ultra_slow.Ti = 100; -motor.PI_velocity_ultra_slow.u_limit = 12; +See `atmega328_bldc_encoder` and `stm32_bldc_encoder` project example for BLDC motors or `atmega2560_stepper_encoder` for stepper equivalent. + +### Example: SPI Magnetic sensor +If you wish to use Stepper motor and SPI magnetic sensor in your project, your folder structure will be: + +```shell +├─── my_arduino_project +| ├─ my_arduino_project.ino +| └─── src +| | ├─── common # Common utility classes and functions +| | ├─── drivers # Driver handling software +│ ├─── sensors +| | └─ MagneticSensorSPI.cpp/h # class implementing SPI communication for Magnetic sensors +| | +| └─ StepperMotor.cpp/h # Stepper motor handling class ``` -The parameters of the PI controller are proportional gain `K`, integral time constant `Ti` and voltage limit `u_limit` which is by default set to the `power_supply_voltage`. -- The `u_limit` parameter is intended if some reason you wish to limit the voltage that can be sent to your motor. -- In general by raising the proportional constant `K` your motor controller will be more reactive, but too much will make it unstable. -- The same goes for integral time constant `Ti` the smaller it is the faster motors reaction to disturbance will be, but too small value will make it unstable. By defaualt the integral time constant `Ti` is set `100s`. Which means that it is extreamply slow, meaning that it is not effecting the behvior of the controlle, making it basically a P controller. - -From the PI controller parameters you can see that the values are much higher than in the [velocity control loop](#velocity-control-loop). The reason is because the angle control loop is not the main loop and we need it to follow the profile as good as possible as fast as possible. Therefore we need much higher gain than before. - -## FOC routine -### Intialisation - `setup()` -After the motor and encoder are intialised and the driver and control loops are configured you intialise the FOC algorithm. +And in your in your arduino project `my_arduino_project.ino` add the line: ```cpp -// align encoder and start FOC -motor.initFOC(); +#include "src/sensors/MagneticSensorSPI.h" ``` -This function aligns encoder and motor zero positions and intialises FOC variables. It is intended to be run in the `setup` function of the Arudino. After the call of this funciton FOC is ready to start following your instructions. - -### Real-time execution `loop()` - -The real time execution of the Arduino FOC library is govenred by two funcitons `motor.loopFOC()` and `motor.move(float target)`. -```cpp -// iterative state calculation calculating angle -// and setting FOC pahse voltage -// the faster you run this funciton the better -// in arduino loop it should have ~1kHz -// the best would be to be in ~10kHz range -motor.loopFOC(); +See `esp32_bldc_magnetic_spi` project example or `atmega328_bldc_magnetic_i2c` for I2C magnetic sensors equivalent. + + +### Example: Multiple sensors: analog magnetic sensor and encoder +For example if you wish to use magnetic sensor with SPI communication, your arduino project will have structure: + +```shell +├─── my_arduino_project +| ├─ my_arduino_project.ino +| └─── src +| | ├─── common # Common utility classes and functions +| | ├─── drivers # Driver handling software +│ ├─── sensors +| | ├─ Encoder.cpp/h # Encoder class implementing the Quadrature encoder operations +| | └─ MagneticSensorAnalog.cpp/h # class implementing Analog output for Magnetic sensors +| | +| └─ StepperMotor.cpp/h # Stepper motor handling class ``` -The funciton `loopFOC()` gets the current motor angle from the encoder, turns in into the electrical angle and computes Clarke transfrom to set the desired $U_q$ voltage to the motor. Basically it implements the funcitonality of the [velocity control loop](#voltage-control-loop). -- The faster you can run this funciton the better -- In the empty arduino loop it runs at ~1kHz but idealy it would be around ~10kHz - - +And added includes should be: ```cpp -// iterative function setting the outter loop target -// velocity, position or voltage -// this funciton can be run at much lower frequency than loopFOC funciton -// it can go as low as ~50Hz -motor.move(target); +#include "src/sensors/MagneticSensorAnalog.h" +#include "src/sensors/Encoder.h" ``` -The `move()` method executes the control loops of the algorihtm. If is governed by the `motor.controller` variable. It executes eigther pure voltage loop, velocity loop, angle loop or ultra slow velocity loop. - -It receives one parameter `BLDCMotor::move(float target)` which is current user define target value. -- If the user runs [velocity loop](#velocity-control-loop), `move` funciton will interpret `target` as the target velocity $\textsf{v}_d$. -- If the user runs [angle loop](#angle-control-loop), `move` will interpret `target` parameter as the target angle $\textsf{a}_d$. -- If the user runs the [voltage loop](#voltage-control-loop), `move` funciton will interpret the `target` parameter as voltage $\textbf{u}_q$. - - -> At this point because we are oriented to simplicity we did not implement synchornious version of this code. Uing timer interrupt. The main reason for the moment is that Arduino UNO doesn't have enough timers to run it. -> *But in future we are planning to include this functionality.* - -## Examples -Examples folder structure +### Example: Sensors standalone - *without motor/driver* +It is possible to use the sensors developed in this library as standalone sensors. For that you can need to do steps 0. and 1. and then just add the sensor specific code. This is one possible project structure if you wish to use an encoder as a standalone sensor: +```shell +├─── my_arduino_project +| ├─ my_arduino_project.ino +| └─── src +| | ├─── common # Common utility classes and functions +│ └─── sensors +| └─ Encoder.cpp/h # Encoder class implementing the Quadrature encoder operations ``` -├───examples -│ ├───voltage_control # example of the voltage control loop with configuraiton -│ ├───angle_control # example of angle control loop with configuraiton -│ ├───angle_control_serial # example of angle control using serial port with configuraiton -│ ├───velocity_control # example of velocity control loop with configuraiton -│ ├───velocity_control_serial # example of velocity control using serial port with configuraiton -│ ├───velocity_ultraslow_control_serial # example of ultra slow velocity control using serial port with configuraiton -│ ├───encoder_example # simple example of encoder usage -│ ├───minimal_example # example of code without using configuration -│ └───HMBGC_example # example of code to be used with HMBGC controller with configuraiton -``` - - -# Debugging -To debug control loop exection in the examples we added a funciton `motor_monitor()` which log the motor variables to the serial port. The funciton logs different variables based for differenc control loops. -```cpp -// utility function intended to be used with serial plotter to monitor motor variables -// significantly slowing the execution down!!!! -void motor_monitor() { - switch (motor.controller) { - case ControlType::velocity_ultra_slow: - case ControlType::velocity: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.print(motor.shaft_velocity_sp); - Serial.print("\t"); - Serial.println(motor.shaft_velocity); - break; - case ControlType::angle: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.print(motor.shaft_angle_sp); - Serial.print("\t"); - Serial.println(motor.shaft_angle); - break; - case ControlType::voltage: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.println(motor.shaft_velocity); - break; - } -} -``` -This is just a template funciton to help you debug and create your own functions in future. -The funciton accesses the motor variables: -```cpp -class BLDCMotor -{ - public: - ... - // current elelctrical angle - float elctric_angle; - // current motor angle - float shaft_angle; - // current motor velocity - float shaft_velocity; - // current target velocity - float shaft_velocity_sp; - // current target angle - float shaft_angle_sp; - // current voltage u_q set - float voltage_q; -... -} -``` -Additionally it is possible to use encoder api directly to get the encoder angle and velocity. +And you can include it directly to the arduino project: ```cpp - -class Encoder{ - public: - // shaft velocity getter - float getVelocity(); - // shaft angle getter - float getAngle(); -} +#include "src/sensors/Encoder.h" ``` +## Documentation +Find out more information about the Arduino *Simple**FOC**library* and *Simple**FOC**project* in [docs website](https://docs.simplefoc.com/) -# Future Work Roadmap -#### Library maintenance -- [ ] Proper introduction of the **Arudino FOC Shield V1.2** -- [ ] Make the library accesible in the Arduino Library Manager -- [ ] Publish a video utilising the library and the samples -- [ ] Make minimal version of the arduino code - all in one arduino file - -#### Code developement -- [ ] Encoder index proper implementation -- [ ] Enable more dirver types -- [ ] Timer interrupt execution rather than in the `loop()` -- [ ] Make support for magnetic encoder AS5048 and similar - +## Arduino FOC repo structure +Branch | Description | Status +------------ | ------------- | ------------ +[master](https://github.com/simplefoc/Arduino-FOC) | Stable and tested library version | ![Library Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Compile/badge.svg) +[dev](https://github.com/simplefoc/Arduino-FOC/tree/dev) | Development library version | ![Library Dev Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Dev%20Compile/badge.svg?branch=dev) +[minimal](https://github.com/simplefoc/Arduino-FOC/tree/minimal) | Minimal Arduino example with integrated library | ![MinimalBuild](https://github.com/simplefoc/Arduino-FOC/workflows/MinimalBuild/badge.svg?branch=minimal) diff --git a/examples/HMBGC_example/HMBGC_example.ino b/examples/HMBGC_example/HMBGC_example.ino deleted file mode 100644 index 483514da..00000000 --- a/examples/HMBGC_example/HMBGC_example.ino +++ /dev/null @@ -1,123 +0,0 @@ -#include -#include -#include - - -// BLDCMotor( int phA, int phB, int phC, int pp, int en) -// - phA, phB, phC - motor A,B,C phase pwm pins -// - pp - pole pair number -// - enable pin - (optional input) -BLDCMotor motor = BLDCMotor(9, 10, 11, 11); -// Encoder(int encA, int encB , int cpr, int index) -// - encA, encB - encoder A and B pins -// - ppr - impulses per rotation (cpr=ppr*4) -// - index pin - (optional input) -Encoder encoder = Encoder(A1, A0, 600); -// interrupt ruotine intialisation -void doA(){encoder.handleA();} -void doB(){encoder.handleB();} - -// encoder interrupt init -PciListenerImp listenerA(encoder.pinA, doA); -PciListenerImp listenerB(encoder.pinB, doB); - -void setup() { - // debugging port - Serial.begin(115200); - - // check if you need internal pullups - // Pullup::EXTERN - external pullup added - dafault - // Pullup::INTERN - needs internal arduino pullup - encoder.pullup = Pullup::INTERN; - // initialise encoder hardware - encoder.init(); - - // interrupt intitialisation - PciManager.registerListener(&listenerA); - PciManager.registerListener(&listenerB); - - // set driver type - // DriverType::unipolar - // DriverType::bipolar - default - motor.driver = DriverType::unipolar; - - // power supply voltage - // default 12V - motor.power_supply_voltage = 12; - - // set FOC loop to be used - // ControlType::voltage - // ControlType::velocity - // ControlType::velocity_ultra_slow - // ControlType::angle - motor.controller = ControlType::velocity; - - // velocity PI controller parameters - // default K=1.0 Ti = 0.003 - motor.PI_velocity.K = 1; - motor.PI_velocity.Ti = 0.003; - - // link the motor to the sensor - motor.linkEncoder(&encoder); - - // intialise motor - motor.init(); - // align encoder and start FOC - motor.initFOC(); - - Serial.println("Motor ready."); - delay(1000); -} - -float velocity_sp; - -void loop() { - // iterative state calculation calculating angle - // and setting FOC pahse voltage - // the faster you run this funciton the better - // in arduino loop it should have ~1kHz - // the best would be to be in ~10kHz range - motor.loopFOC(); - - // 0.5 hertz sine weve - velocity_sp = sin( micros()*1e-6 *2*M_PI * 0.5 ); - - // iterative function setting the outter loop target - // velocity, position or voltage - // this funciton can be run at much lower frequency than loopFOC funciton - // it can go as low as ~50Hz - motor.move(velocity_sp); - - - // function intended to be used with serial plotter to monitor motor variables - // significantly slowing the execution down!!!! - motor_monitor(); -} - -// utility function intended to be used with serial plotter to monitor motor variables -// significantly slowing the execution down!!!! -void motor_monitor() { - switch (motor.controller) { - case ControlType::velocity_ultra_slow: - case ControlType::velocity: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.print(motor.shaft_velocity_sp); - Serial.print("\t"); - Serial.println(motor.shaft_velocity); - break; - case ControlType::angle: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.print(motor.shaft_angle_sp); - Serial.print("\t"); - Serial.println(motor.shaft_angle); - break; - case ControlType::voltage: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.println(motor.shaft_velocity); - break; - } -} - diff --git a/examples/angle_control/angle_control.ino b/examples/angle_control/angle_control.ino deleted file mode 100644 index 3394bb7b..00000000 --- a/examples/angle_control/angle_control.ino +++ /dev/null @@ -1,127 +0,0 @@ -#include - -// Only pins 2 and 3 are supported -#define arduinoInt1 2 // Arduino UNO interrupt 0 -#define arduinoInt2 3 // Arduino UNO interrupt 1 - -// BLDCMotor( int phA, int phB, int phC, int pp, int en) -// - phA, phB, phC - motor A,B,C phase pwm pins -// - pp - pole pair number -// - enable pin - (optional input) -BLDCMotor motor = BLDCMotor(9, 10, 11, 11, 8); -// Encoder(int encA, int encB , int cpr, int index) -// - encA, encB - encoder A and B pins -// - ppr - impulses per rotation (cpr=ppr*4) -// - index pin - (optional input) -Encoder encoder = Encoder(arduinoInt1, arduinoInt2, 8192, 4); -// interrupt ruotine intialisation -void doA(){encoder.handleA();} -void doB(){encoder.handleB();} - -void setup() { - // debugging port - Serial.begin(115200); - - // check if you need internal pullups - // Pullup::EXTERN - external pullup added - dafault - // Pullup::INTERN - needs internal arduino pullup - encoder.pullup = Pullup::EXTERN; - - // initialise encoder hardware - encoder.init(doA, doB); - - // set driver type - // DriverType::unipolar - // DriverType::bipolar - default - motor.driver = DriverType::bipolar; - - // power supply voltage - // default 12V - motor.power_supply_voltage = 12; - - // set FOC loop to be used - // ControlType::voltage - // ControlType::velocity - // ControlType::velocity_ultra_slow - // ControlType::angle - motor.controller = ControlType::angle; - - // velocity PI controller parameters - // default K=1.0 Ti = 0.003 - motor.PI_velocity.K = 0.5; - motor.PI_velocity.Ti = 0.01; - // angle P controller - // default K=70 - motor.P_angle.K = 20; - // maximal velocity of the poisiiton control - // default 20 - motor.P_angle.velocity_limit = 10; - - // link the motor to the sensor - motor.linkEncoder(&encoder); - - // intialise motor - motor.init(); - // align encoder and start FOC - motor.initFOC(); - - - Serial.println("Motor ready."); - delay(1000); -} - -// angle target variable -float target_angle; - -void loop() { - - // iterative state calculation calculating angle - // and setting FOC pahse voltage - // the faster you run this funciton the better - // in arduino loop it should have ~1kHz - // the best would be to be in ~10kHz range - motor.loopFOC(); - - - // 0.5 hertz sine wave - target_angle = sin( micros()*1e-6 *2*M_PI * 0.5 ); - - // iterative function setting the outter loop target - // velocity, position or voltage - // this funciton can be run at much lower frequency than loopFOC funciton - // it can go as low as ~50Hz - motor.move(target_angle); - - - // function intended to be used with serial plotter to monitor motor variables - // significantly slowing the execution down!!!! - motor_monitor(); -} - -// utility function intended to be used with serial plotter to monitor motor variables -// significantly slowing the execution down!!!! -void motor_monitor() { - switch (motor.controller) { - case ControlType::velocity_ultra_slow: - case ControlType::velocity: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.print(motor.shaft_velocity_sp); - Serial.print("\t"); - Serial.println(motor.shaft_velocity); - break; - case ControlType::angle: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.print(motor.shaft_angle_sp); - Serial.print("\t"); - Serial.println(motor.shaft_angle); - break; - case ControlType::voltage: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.println(motor.shaft_velocity); - break; - } -} - diff --git a/examples/angle_control_serial/angle_control_serial.ino b/examples/angle_control_serial/angle_control_serial.ino deleted file mode 100644 index 394b5d36..00000000 --- a/examples/angle_control_serial/angle_control_serial.ino +++ /dev/null @@ -1,142 +0,0 @@ -#include - -// Only pins 2 and 3 are supported -#define arduinoInt1 2 // Arduino UNO interrupt 0 -#define arduinoInt2 3 // Arduino UNO interrupt 1 - -// angle set point variable -float target_angle = 0; - -// BLDCMotor( int phA, int phB, int phC, int pp, int en) -// - phA, phB, phC - motor A,B,C phase pwm pins -// - pp - pole pair number -// - enable pin - (optional input) -BLDCMotor motor = BLDCMotor(9, 10, 11, 11, 8); -// Encoder(int encA, int encB , int cpr, int index) -// - encA, encB - encoder A and B pins -// - ppr - impulses per rotation (cpr=ppr*4) -// - index pin - (optional input) -Encoder encoder = Encoder(arduinoInt1, arduinoInt2, 8192, 4); -// interrupt ruotine intialisation -void doA(){encoder.handleA();} -void doB(){encoder.handleB();} - -void setup() { - // debugging port - Serial.begin(115200); - - // check if you need internal pullups - // Pullup::EXTERN - external pullup added - dafault - // Pullup::INTERN - needs internal arduino pullup - encoder.pullup = Pullup::EXTERN; - - // initialise encoder hardware - encoder.init(doA, doB); - // set driver type - // DriverType::unipolar - // DriverType::bipolar - default - motor.driver = DriverType::bipolar; - - // power supply voltage - // default 12V - motor.power_supply_voltage = 12; - - // set FOC loop to be used - // ControlType::voltage - // ControlType::velocity - // ControlType::velocity_ultra_slow - // ControlType::angle - motor.controller = ControlType::angle; - - // velocity PI controller parameters - // default K=1.0 Ti = 0.003 - motor.PI_velocity.K = 0.5; - motor.PI_velocity.Ti = 0.01; - // angle P controller - // default K=70 - motor.P_angle.K = 20; - // maximal velocity of the poisiiton control - // default 20 - motor.P_angle.velocity_limit = 10; - - // link the motor to the sensor - motor.linkEncoder(&encoder); - - // intialise motor - motor.init(); - // align encoder and start FOC - motor.initFOC(); - - Serial.println("Motor ready."); - Serial.println("Set the target angle using serial terminal:"); - delay(1000); -} - -void loop() { - // iterative state calculation calculating angle - // and setting FOC pahse voltage - // the faster you run this funciton the better - // in arduino loop it should have ~1kHz - // the best would be to be in ~10kHz range - motor.loopFOC(); - - // iterative function setting the outter loop target - // velocity, position or voltage - // this funciton can be run at much lower frequency than loopFOC funciton - // it can go as low as ~50Hz - motor.move(target_angle); - - - // function intended to be used with serial plotter to monitor motor variables - // significantly slowing the execution down!!!! - // motor_monitor(); -} - -// utility function intended to be used with serial plotter to monitor motor variables -// significantly slowing the execution down!!!! -void motor_monitor() { - switch (motor.controller) { - case ControlType::velocity_ultra_slow: - case ControlType::velocity: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.print(motor.shaft_velocity_sp); - Serial.print("\t"); - Serial.println(motor.shaft_velocity); - break; - case ControlType::angle: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.print(motor.shaft_angle_sp); - Serial.print("\t"); - Serial.println(motor.shaft_angle); - break; - case ControlType::voltage: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.println(motor.shaft_velocity); - break; - } -} - -// Serial communication callback function -// gets the target value from the user -void serialEvent() { - // a string to hold incoming data - static String inputString; - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the inputString: - inputString += inChar; - // if the incoming character is a newline - // end of input - if (inChar == '\n') { - target_angle = inputString.toFloat(); - Serial.print("Tagret angle: "); - Serial.println(target_angle); - inputString = ""; - } - } -} - diff --git a/examples/change_direction/change_direction.ino b/examples/change_direction/change_direction.ino deleted file mode 100644 index e5ab1d57..00000000 --- a/examples/change_direction/change_direction.ino +++ /dev/null @@ -1,123 +0,0 @@ -#include - -// Only pins 2 and 3 are supported -#define arduinoInt1 2 // Arduino UNO interrupt 0 -#define arduinoInt2 3 // Arduino UNO interrupt 1 - -// BLDCMotor( int phA, int phB, int phC, int pp, int en) -// - phA, phB, phC - motor A,B,C phase pwm pins -// - pp - pole pair number -// - enable pin - (optional input) -BLDCMotor motor = BLDCMotor(9, 10, 11, 11, 8); -// Encoder(int encA, int encB , int cpr, int index) -// - encA, encB - encoder A and B pins -// - ppr - impulses per rotation (cpr=ppr*4) -// - index pin - (optional input) -Encoder encoder = Encoder(arduinoInt1, arduinoInt2, 8192, 4); -// interrupt ruotine intialisation -void doA(){encoder.handleA();} -void doB(){encoder.handleB();} - -void setup() { - // debugging port - Serial.begin(115200); - - // check if you need internal pullups - // Pullup::EXTERN - external pullup added - dafault - // Pullup::INTERN - needs internal arduino pullup - encoder.pullup = Pullup::EXTERN; - - // initialise encoder hardware - encoder.init(doA, doB); - - // set driver type - // DriverType::unipolar - // DriverType::bipolar - default - motor.driver = DriverType::bipolar; - - // power supply voltage - // default 12V - motor.power_supply_voltage = 12; - - // set FOC loop to be used - // ControlType::voltage - // ControlType::velocity - // ControlType::velocity_ultra_slow - // ControlType::angle - motor.controller = ControlType::velocity; - - // velocity PI controller parameters - // default K=1.0 Ti = 0.003 - motor.PI_velocity.K = 1; - motor.PI_velocity.Ti = 0.003; - - // link the motor to the sensor - motor.linkEncoder(&encoder); - // intialise motor - motor.init(); - // align encoder and start FOC - motor.initFOC(); - - - Serial.println("Motor ready."); - delay(1000); -} - -// target velocity variable -float target_velocity = 3; -int t = 0; - -void loop() { - // iterative state calculation calculating angle - // and setting FOC pahse voltage - // the faster you run this funciton the better - // in arduino loop it should have ~1kHz - // the best would be to be in ~10kHz range - motor.loopFOC(); - - // direction chnaging logic - // change direction each 1000 loop passes - target_velocity *= (t >= 1000) ? -1 : 1; - // loop passes counter - t = (t >= 1000) ? 0 : t+1; - - - // iterative function setting the outter loop target - // velocity, position or voltage - // this funciton can be run at much lower frequency than loopFOC funciton - // it can go as low as ~50Hz - motor.move(target_velocity); - - - // function intended to be used with serial plotter to monitor motor variables - // significantly slowing the execution down!!!! - //motor_monitor(); -} - -// utility function intended to be used with serial plotter to monitor motor variables -// significantly slowing the execution down!!!! -void motor_monitor() { - switch (motor.controller) { - case ControlType::velocity_ultra_slow: - case ControlType::velocity: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.print(motor.shaft_velocity_sp); - Serial.print("\t"); - Serial.println(motor.shaft_velocity); - break; - case ControlType::angle: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.print(motor.shaft_angle_sp); - Serial.print("\t"); - Serial.println(motor.shaft_angle); - break; - case ControlType::voltage: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.println(motor.shaft_velocity); - break; - } -} - diff --git a/examples/encoder_example/encoder_example.ino b/examples/encoder_example/encoder_example.ino deleted file mode 100644 index 10de5d06..00000000 --- a/examples/encoder_example/encoder_example.ino +++ /dev/null @@ -1,33 +0,0 @@ -#include - -// Only pins 2 and 3 are supported -#define arduinoInt1 2 // Arduino UNO interrupt 0 -#define arduinoInt2 3 // Arduino UNO interrupt 1 - -Encoder encoder = Encoder(arduinoInt1, arduinoInt2, 8192); -// interrupt ruotine intialisation -void doA(){encoder.handleA();} -void doB(){encoder.handleB();} - -void setup() { - // debugging port - Serial.begin(115200); - - // check if you need internal pullups - // Pullup::EXTERN - external pullup added - dafault - // Pullup::INTERN - needs internal arduino pullup - encoder.pullup = Pullup::EXTERN; - - // initialise encoder hardware - encoder.init(doA, doB); - - Serial.println("Encoder ready"); - delay(1000); -} - -void loop() { - // display the angle and the angular velocity to the terminal - Serial.print(encoder.getAngle()); - Serial.print("\t"); - Serial.println(encoder.getVelocity()); -} diff --git a/examples/minimal_example/minimal_example.ino b/examples/minimal_example/minimal_example.ino deleted file mode 100644 index fd225765..00000000 --- a/examples/minimal_example/minimal_example.ino +++ /dev/null @@ -1,48 +0,0 @@ -#include - -// This example gives you a minimal code needed to run the FOC algorithm -// All the configuration is set to defualt values -// motor.power_supply_voltage= 12V -// motor.driver = DriverType::bipolar -// encoder.pullup = Pullup::EXTERN -// motor.PI_velocity.K = 1 -// motor.PI_velocity.Ti = 0.003 - -// BLDCMotor( phA, phB, phC, pole_pairs, enable) -BLDCMotor motor = BLDCMotor(9, 10, 11, 11, 8); -// Encoder(encA, encB , ppr, index) -Encoder encoder = Encoder(2, 3, 8192, 4); - -// interrupt ruotine intialisation -void doA(){encoder.handleA();} -void doB(){encoder.handleB();} - -void setup() { - // debugging port - Serial.begin(115200); - - // initialise encoder hardware - encoder.init(doA, doB); - // link the motor to the sensor - motor.linkEncoder(&encoder); - // velocity control - motor.controller = ControlType::velocity; - - // intialise motor - motor.init(); - // align encoder and start FOC - motor.initFOC(); - - Serial.println("Motor ready."); - delay(1000); -} - -// target velocity variable -float target_velocity = 2; - -void loop() { - // foc loop - motor.loopFOC(); - // control loop - motor.move(target_velocity); -} diff --git a/examples/velocity_control/velocity_control.ino b/examples/velocity_control/velocity_control.ino deleted file mode 100644 index c1bf8914..00000000 --- a/examples/velocity_control/velocity_control.ino +++ /dev/null @@ -1,118 +0,0 @@ -#include - -// Only pins 2 and 3 are supported -#define arduinoInt1 2 // Arduino UNO interrupt 0 -#define arduinoInt2 3 // Arduino UNO interrupt 1 - -// BLDCMotor( int phA, int phB, int phC, int pp, int en) -// - phA, phB, phC - motor A,B,C phase pwm pins -// - pp - pole pair number -// - enable pin - (optional input) -BLDCMotor motor = BLDCMotor(9, 10, 11, 11, 8); -// Encoder(int encA, int encB , int cpr, int index) -// - encA, encB - encoder A and B pins -// - ppr - impulses per rotation (cpr=ppr*4) -// - index pin - (optional input) -Encoder encoder = Encoder(arduinoInt1, arduinoInt2, 8192, 4); -// interrupt ruotine intialisation -void doA(){encoder.handleA();} -void doB(){encoder.handleB();} - -void setup() { - // debugging port - Serial.begin(115200); - - // check if you need internal pullups - // Pullup::EXTERN - external pullup added - dafault - // Pullup::INTERN - needs internal arduino pullup - encoder.pullup = Pullup::EXTERN; - - // initialise encoder hardware - encoder.init(doA, doB); - - // set driver type - // DriverType::unipolar - // DriverType::bipolar - default - motor.driver = DriverType::bipolar; - - // power supply voltage - // default 12V - motor.power_supply_voltage = 12; - - // set FOC loop to be used - // ControlType::voltage - // ControlType::velocity - // ControlType::velocity_ultra_slow - // ControlType::angle - motor.controller = ControlType::velocity; - - // velocity PI controller parameters - // default K=1.0 Ti = 0.003 - motor.PI_velocity.K = 1; - motor.PI_velocity.Ti = 0.003; - - // link the motor to the sensor - motor.linkEncoder(&encoder); - // intialise motor - motor.init(); - // align encoder and start FOC - motor.initFOC(); - - - Serial.println("Motor ready."); - delay(1000); -} - -// target velocity variable -float target_velocity; - -void loop() { - // iterative state calculation calculating angle - // and setting FOC pahse voltage - // the faster you run this funciton the better - // in arduino loop it should have ~1kHz - // the best would be to be in ~10kHz range - motor.loopFOC(); - - // 0.5 hertz sine wave - target_velocity = sin( micros()*1e-6 *2*M_PI * 0.5 ); - - // iterative function setting the outter loop target - // velocity, position or voltage - // this funciton can be run at much lower frequency than loopFOC funciton - // it can go as low as ~50Hz - motor.move(target_velocity); - - - // function intended to be used with serial plotter to monitor motor variables - // significantly slowing the execution down!!!! - motor_monitor(); -} - -// utility function intended to be used with serial plotter to monitor motor variables -// significantly slowing the execution down!!!! -void motor_monitor() { - switch (motor.controller) { - case ControlType::velocity_ultra_slow: - case ControlType::velocity: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.print(motor.shaft_velocity_sp); - Serial.print("\t"); - Serial.println(motor.shaft_velocity); - break; - case ControlType::angle: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.print(motor.shaft_angle_sp); - Serial.print("\t"); - Serial.println(motor.shaft_angle); - break; - case ControlType::voltage: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.println(motor.shaft_velocity); - break; - } -} - diff --git a/examples/velocity_control_serial/velocity_control_serial.ino b/examples/velocity_control_serial/velocity_control_serial.ino deleted file mode 100644 index ef0848af..00000000 --- a/examples/velocity_control_serial/velocity_control_serial.ino +++ /dev/null @@ -1,137 +0,0 @@ -#include - -// Only pins 2 and 3 are supported -#define arduinoInt1 2 // Arduino UNO interrupt 0 -#define arduinoInt2 3 // Arduino UNO interrupt 1 - -// velocity set point variable -float target_velocity = 0; - - -// BLDCMotor( int phA, int phB, int phC, int pp, int en) -// - phA, phB, phC - motor A,B,C phase pwm pins -// - pp - pole pair number -// - enable pin - (optional input) -BLDCMotor motor = BLDCMotor(9, 10, 11, 11, 8); -// Encoder(int encA, int encB , int cpr, int index) -// - encA, encB - encoder A and B pins -// - ppr - impulses per rotation (cpr=ppr*4) -// - index pin - (optional input) -Encoder encoder = Encoder(arduinoInt1, arduinoInt2, 8192, 4); -// interrupt ruotine intialisation -void doA(){encoder.handleA();} -void doB(){encoder.handleB();} - -void setup() { - // debugging port - Serial.begin(115200); - - // check if you need internal pullups - // Pullup::EXTERN - external pullup added - // Pullup::INTERN - needs internal arduino pullup - encoder.pullup = Pullup::EXTERN; - - // initialise encoder hardware - encoder.init(doA, doB); - - // set driver type - // DriverType::unipolar - // DriverType::bipolar - default - motor.driver = DriverType::bipolar; - - // power supply voltage - // default 12V - motor.power_supply_voltage = 12; - - // set FOC loop to be used - // ControlType::voltage - // ControlType::velocity - // ControlType::velocity_ultra_slow - // ControlType::angle - motor.controller = ControlType::velocity; - - // velocity PI controller parameters - // default K=1.0 Ti = 0.003 - motor.PI_velocity.K = 1; - motor.PI_velocity.Ti = 0.003; - - // link the motor to the sensor - motor.linkEncoder(&encoder); - - // intialise motor - motor.init(); - // align encoder and start FOC - motor.initFOC(); - - Serial.println("Motor ready."); - Serial.println("Set the target velocity using serial terminal:"); - delay(1000); -} - -void loop() { - // iterative state calculation calculating angle - // and setting FOC pahse voltage - // the faster you run this funciton the better - // in arduino loop it should have ~1kHz - // the best would be to be in ~10kHz range - motor.loopFOC(); - - // iterative function setting the outter loop target - // velocity, position or voltage - // this funciton can be run at much lower frequency than loopFOC funciton - // it can go as low as ~50Hz - motor.move(target_velocity); - - - // function intended to be used with serial plotter to monitor motor variables - // significantly slowing the execution down!!!! - // motor_monitor(); -} - -// utility function intended to be used with serial plotter to monitor motor variables -// significantly slowing the execution down!!!! -void motor_monitor() { - switch (motor.controller) { - case ControlType::velocity_ultra_slow: - case ControlType::velocity: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.print(motor.shaft_velocity_sp); - Serial.print("\t"); - Serial.println(motor.shaft_velocity); - break; - case ControlType::angle: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.print(motor.shaft_angle_sp); - Serial.print("\t"); - Serial.println(motor.shaft_angle); - break; - case ControlType::voltage: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.println(motor.shaft_velocity); - break; - } -} - -// Serial communication callback function -// gets the target value from the user -void serialEvent() { - // a string to hold incoming data - static String inputString; - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the inputString: - inputString += inChar; - // if the incoming character is a newline - // end of input - if (inChar == '\n') { - target_velocity = inputString.toFloat(); - Serial.print("Tagret velocity: "); - Serial.println(target_velocity); - inputString = ""; - } - } -} diff --git a/examples/velocity_ultraslow_control_serial/velocity_ultraslow_control_serial.ino b/examples/velocity_ultraslow_control_serial/velocity_ultraslow_control_serial.ino deleted file mode 100644 index 15349486..00000000 --- a/examples/velocity_ultraslow_control_serial/velocity_ultraslow_control_serial.ino +++ /dev/null @@ -1,137 +0,0 @@ -#include - -// Only pins 2 and 3 are supported -#define arduinoInt1 2 // Arduino UNO interrupt 0 -#define arduinoInt2 3 // Arduino UNO interrupt 1 - -// velocity set point variable -float target_velocity = 0; - - -// BLDCMotor( int phA, int phB, int phC, int pp, int en) -// - phA, phB, phC - motor A,B,C phase pwm pins -// - pp - pole pair number -// - enable pin - (optional input) -BLDCMotor motor = BLDCMotor(9, 10, 11, 11, 8); -// Encoder(int encA, int encB , int cpr, int index) -// - encA, encB - encoder A and B pins -// - ppr - impulses per rotation (cpr=ppr*4) -// - index pin - (optional input) -Encoder encoder = Encoder(arduinoInt1, arduinoInt2, 8192, 4); -// interrupt ruotine intialisation -void doA(){encoder.handleA();} -void doB(){encoder.handleB();} - -void setup() { - // debugging port - Serial.begin(115200); - - // check if you need internal pullups - // Pullup::EXTERN - external pullup added - dafault - // Pullup::INTERN - needs internal arduino pullup - encoder.pullup = Pullup::EXTERN; - - // initialise encoder hardware - encoder.init(doA, doB); - - // set driver type - // DriverType::unipolar - // DriverType::bipolar - default - motor.driver = DriverType::bipolar; - - // power supply voltage - // default 12V - motor.power_supply_voltage = 12; - - // set FOC loop to be used - // ControlType::voltage - // ControlType::velocity - // ControlType::velocity_ultra_slow - // ControlType::angle - motor.controller = ControlType::velocity_ultra_slow; - - // velocity PI controller parameters - // default K=120.0 Ti = 100.0 - motor.PI_velocity_ultra_slow.K = 120; - motor.PI_velocity_ultra_slow.Ti = 100; - - // link the motor to the sensor - motor.linkEncoder(&encoder); - - // intialise motor - motor.init(); - // align encoder and start FOC - motor.initFOC(); - - Serial.println("Motor ready."); - Serial.println("Set the target velocity using serial terminal:"); - delay(1000); -} - -void loop() { - // iterative state calculation calculating angle - // and setting FOC pahse voltage - // the faster you run this funciton the better - // in arduino loop it should have ~1kHz - // the best would be to be in ~10kHz range - motor.loopFOC(); - - // iterative function setting the outter loop target - // velocity, position or voltage - // this funciton can be run at much lower frequency than loopFOC funciton - // it can go as low as ~50Hz - motor.move(target_velocity); - - - // function intended to be used with serial plotter to monitor motor variables - // significantly slowing the execution down!!!! - // motor_monitor(); -} - -// utility function intended to be used with serial plotter to monitor motor variables -// significantly slowing the execution down!!!! -void motor_monitor() { - switch (motor.controller) { - case ControlType::velocity_ultra_slow: - case ControlType::velocity: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.print(motor.shaft_velocity_sp); - Serial.print("\t"); - Serial.println(motor.shaft_velocity); - break; - case ControlType::angle: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.print(motor.shaft_angle_sp); - Serial.print("\t"); - Serial.println(motor.shaft_angle); - break; - case ControlType::voltage: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.println(motor.shaft_velocity); - break; - } -} - -// Serial communication callback function -// gets the target value from the user -void serialEvent() { - // a string to hold incoming data - static String inputString; - while (Serial.available()) { - // get the new byte: - char inChar = (char)Serial.read(); - // add it to the inputString: - inputString += inChar; - // if the incoming character is a newline - // end of input - if (inChar == '\n') { - target_velocity = inputString.toFloat(); - Serial.print("Tagret velocity: "); - Serial.println(target_velocity); - inputString = ""; - } - } -} \ No newline at end of file diff --git a/examples/voltage_control/voltage_control.ino b/examples/voltage_control/voltage_control.ino deleted file mode 100644 index e097f9cc..00000000 --- a/examples/voltage_control/voltage_control.ino +++ /dev/null @@ -1,112 +0,0 @@ -#include - -// Only pins 2 and 3 are supported -#define arduinoInt1 2 // Arduino UNO interrupt 0 -#define arduinoInt2 3 // Arduino UNO interrupt 1 - -// BLDCMotor( int phA, int phB, int phC, int pp, int en) -// - phA, phB, phC - motor A,B,C phase pwm pins -// - pp - pole pair number -// - enable pin - (optional input) -BLDCMotor motor = BLDCMotor(9, 10, 11, 11, 8); -// Encoder(int encA, int encB , int cpr, int index) -// - encA, encB - encoder A and B pins -// - ppr - impulses per rotation (cpr=ppr*4) -// - index pin - (optional input) -Encoder encoder = Encoder(arduinoInt1, arduinoInt2, 8192, 4); -// interrupt ruotine intialisation -void doA(){encoder.handleA();} -void doB(){encoder.handleB();} - -void setup() { - // debugging port - Serial.begin(115200); - - // check if you need internal pullups - // Pullup::EXTERN - external pullup added - dafault - // Pullup::INTERN - needs internal arduino pullup - encoder.pullup = Pullup::EXTERN; - - // initialise encoder hardware - encoder.init(doA, doB); - - // set driver type - // DriverType::unipolar - // DriverType::bipolar - default - motor.driver = DriverType::bipolar; - - // power supply voltage - // default 12V - motor.power_supply_voltage = 12; - - // set FOC loop to be used - // ControlType::voltage - // ControlType::velocity - // ControlType::velocity_ultra_slow - // ControlType::angle - motor.controller = ControlType::voltage; - - // link the motor to the sensor - motor.linkEncoder(&encoder); - - // intialise motor - motor.init(); - // align encoder and start FOC - motor.initFOC(); - - - Serial.println("Motor ready."); - delay(1000); -} - -// uq voltage -float voltage_Uq = 5; - -void loop() { - - // iterative state calculation calculating angle - // and setting FOC pahse voltage - // the faster you run this funciton the better - // in arduino loop it should have ~1kHz - // the best would be to be in ~10kHz range - motor.loopFOC(); - - // iterative function setting the outter loop target - // velocity, position or voltage - // this funciton can be run at much lower frequency than loopFOC funciton - // it can go as low as ~50Hz - motor.move(voltage_Uq); - - - // function intended to be used with serial plotter to monitor motor variables - // significantly slowing the execution down!!!! - motor_monitor(); -} - -// utility function intended to be used with serial plotter to monitor motor variables -// significantly slowing the execution down!!!! -void motor_monitor() { - switch (motor.controller) { - case ControlType::velocity_ultra_slow: - case ControlType::velocity: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.print(motor.shaft_velocity_sp); - Serial.print("\t"); - Serial.println(motor.shaft_velocity); - break; - case ControlType::angle: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.print(motor.shaft_angle_sp); - Serial.print("\t"); - Serial.println(motor.shaft_angle); - break; - case ControlType::voltage: - Serial.print(motor.voltage_q); - Serial.print("\t"); - Serial.println(motor.shaft_velocity); - break; - } -} - diff --git a/extras/ArduinoFOCShieldV12.pdf b/extras/ArduinoFOCShieldV12.pdf deleted file mode 100644 index 2039b2e4..00000000 Binary files a/extras/ArduinoFOCShieldV12.pdf and /dev/null differ diff --git a/extras/Images/AFSV11_bottom.png b/extras/Images/AFSV11_bottom.png deleted file mode 100644 index 71d5d46f..00000000 Binary files a/extras/Images/AFSV11_bottom.png and /dev/null differ diff --git a/extras/Images/AFSV11_side.png b/extras/Images/AFSV11_side.png deleted file mode 100644 index d7a8a232..00000000 Binary files a/extras/Images/AFSV11_side.png and /dev/null differ diff --git a/extras/Images/AFSV11_top.png b/extras/Images/AFSV11_top.png deleted file mode 100644 index b76e1317..00000000 Binary files a/extras/Images/AFSV11_top.png and /dev/null differ diff --git a/extras/Images/arduino_connection.png b/extras/Images/arduino_connection.png deleted file mode 100644 index 04ac9945..00000000 Binary files a/extras/Images/arduino_connection.png and /dev/null differ diff --git a/extras/Images/ebay.jpg b/extras/Images/ebay.jpg deleted file mode 100644 index 1189dcb0..00000000 Binary files a/extras/Images/ebay.jpg and /dev/null differ diff --git a/extras/Images/ebay2.jpg b/extras/Images/ebay2.jpg deleted file mode 100644 index 87ed238c..00000000 Binary files a/extras/Images/ebay2.jpg and /dev/null differ diff --git a/extras/Images/hmbgc_connection.png b/extras/Images/hmbgc_connection.png deleted file mode 100644 index e1195e29..00000000 Binary files a/extras/Images/hmbgc_connection.png and /dev/null differ diff --git a/extras/Images/pinout.jpg b/extras/Images/pinout.jpg deleted file mode 100644 index 3b4fc366..00000000 Binary files a/extras/Images/pinout.jpg and /dev/null differ diff --git a/extras/Images/position.png b/extras/Images/position.png deleted file mode 100644 index 94ca79b6..00000000 Binary files a/extras/Images/position.png and /dev/null differ diff --git a/extras/Images/setup1.jpg b/extras/Images/setup1.jpg deleted file mode 100644 index 0105e4f0..00000000 Binary files a/extras/Images/setup1.jpg and /dev/null differ diff --git a/extras/Images/setup2.jpg b/extras/Images/setup2.jpg deleted file mode 100644 index 202927f8..00000000 Binary files a/extras/Images/setup2.jpg and /dev/null differ diff --git a/extras/Images/velocity.png b/extras/Images/velocity.png deleted file mode 100644 index 98359704..00000000 Binary files a/extras/Images/velocity.png and /dev/null differ diff --git a/extras/Images/velocity_ultraslow_loop.png b/extras/Images/velocity_ultraslow_loop.png deleted file mode 100644 index b3a74476..00000000 Binary files a/extras/Images/velocity_ultraslow_loop.png and /dev/null differ diff --git a/extras/Images/voltage.png b/extras/Images/voltage.png deleted file mode 100644 index 3cd15377..00000000 Binary files a/extras/Images/voltage.png and /dev/null differ diff --git a/keywords.txt b/keywords.txt deleted file mode 100644 index c79a2e98..00000000 --- a/keywords.txt +++ /dev/null @@ -1,60 +0,0 @@ -ArduinoFOC KEYWORD1 -BLDCMotor KEYWORD1 -Encoder KEYWORD1 - -initFOC KEYWORD2 -loopFOC KEYWORD2 -disable KEYWORD2 - -PI_velocity KEYWIORD2 -PI_velocity_ultra_slow KEYWIORD2 -P_angle KEYWIORD2 - -ControlType LITERAL1 -DriverType LITERAL1 -Pullup LITERAL1 - -linkEncoder KEYWIORD2 -handleA KEYWIORD2 -handleB KEYWIORD2 -getVelocity KEYWIORD2 -getAngle KEYWIORD2 -setCounterZero KEYWIORD2 - -voltage_q KEYWIORD2 -shaft_angle_sp KEYWIORD2 -shaft_velocity_sp KEYWIORD2 -shaft_angle KEYWIORD2 -shaft_velocity KEYWIORD2 -controller KEYWIORD2 -driver KEYWIORD2 -pullup KEYWIORD2 - - -voltage KEYWIORD2 -velocity KEYWIORD2 -velocity_ultra_slow KEYWIORD2 -angle KEYWIORD2 - -K KEYWIORD2 -Ti KEYWIORD2 -u_limit KEYWIORD2 -velocity_limit KEYWIORD2 - -pinA LITERAL1 -pinB LITERAL1 -index LITERAL1 - -INTERN KEYWORD2 -EXTERN KEYWORD2 - -unipolar KEYWIORD2 -bipolar KEYWIORD2 - -pwmA LITERAL1 -pwmB LITERAL1 -pwmC LITERAL1 -enable_pin LITERAL1 -pole_pairs LITERAL1 - -power_supply_voltage LITERAL1 \ No newline at end of file diff --git a/library.properties b/library.properties deleted file mode 100644 index 075e71bf..00000000 --- a/library.properties +++ /dev/null @@ -1,10 +0,0 @@ -name=Arduino FOC -version=1.1.0 -author=Antun Skuric -maintainer=Antun Skuric -sentence=A library demistifying FOC for BLDC motors -paragraph=Simple library intended for hobby comunity to run the gimbal motor using FOC algorithm -category=Device Control -url=http://github.com/askuric/Arduino-FOC/ -architectures=* -includes=ArduinoFOC.h \ No newline at end of file diff --git a/library_source/BLDCMotor.cpp b/library_source/BLDCMotor.cpp new file mode 100644 index 00000000..5d36ce35 --- /dev/null +++ b/library_source/BLDCMotor.cpp @@ -0,0 +1,421 @@ +#include "BLDCMotor.h" + +// // BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en) +// // - phA, phB, phC - motor A,B,C phase pwm pins +// // - pp - pole pair number +// // - cpr - counts per rotation number (cpm=ppm*4) +// // - enable pin - (optional input) +// BLDCMotor::BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en){ +// noop; +// } + +// BLDCMotor( int pp) +// - phA, phB, phC - motor A,B,C phase pwm pins +// - pp - pole pair number +// - cpr - counts per rotation number (cpm=ppm*4) +// - enable pin - (optional input) +BLDCMotor::BLDCMotor(int pp) +: FOCMotor() +{ + // save pole pairs number + pole_pairs = pp; +} + + +/** + Link the driver which controls the motor +*/ +void BLDCMotor::linkDriver(BLDCDriver* _driver) { + driver = _driver; +} + +// init hardware pins +void BLDCMotor::init() { + if(monitor_port) monitor_port->println("MOT: Initialise variables."); + // sanity check for the voltage limit configuration + if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; + // constrain voltage for sensor alignment + if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; + // update the controller limits + PID_velocity.limit = voltage_limit; + P_angle.limit = velocity_limit; + + _delay(500); + // enable motor + if(monitor_port) monitor_port->println("MOT: Enable driver."); + enable(); + _delay(500); +} + + +// disable motor driver +void BLDCMotor::disable() +{ + // set zero to PWM + driver->setPwm(0, 0, 0); + // disable the driver + driver->disable(); +} +// enable motor driver +void BLDCMotor::enable() +{ + // enable the driver + driver->enable(); + // set zero to PWM + driver->setPwm(0, 0, 0); + +} + +/** + FOC functions +*/ +// FOC initialization function +int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction ) { + int exit_flag = 1; + // align motor if necessary + // alignment necessary for encoders! + if(zero_electric_offset != NOT_SET){ + // abosolute zero offset provided - no need to align + zero_electric_angle = zero_electric_offset; + // set the sensor direction - default CW + sensor->natural_direction = sensor_direction; + }else{ + // sensor and motor alignment + _delay(500); + exit_flag = alignSensor(); + _delay(500); + } + if(monitor_port) monitor_port->println("MOT: Motor ready."); + + return exit_flag; +} +// Encoder alignment to electrical 0 angle +int BLDCMotor::alignSensor() { + if(monitor_port) monitor_port->println("MOT: Align sensor."); + // align the electrical phases of the motor and sensor + // set angle -90 degrees + + float start_angle = shaftAngle(); + for (int i = 0; i <=5; i++ ) { + float angle = _3PI_2 + _2PI * i / 6.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(200); + } + float mid_angle = shaftAngle(); + for (int i = 5; i >=0; i-- ) { + float angle = _3PI_2 + _2PI * i / 6.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(200); + } + if (mid_angle < start_angle) { + if(monitor_port) monitor_port->println("MOT: natural_direction==CCW"); + sensor->natural_direction = Direction::CCW; + } else if (mid_angle == start_angle) { + if(monitor_port) monitor_port->println("MOT: Sensor failed to notice movement"); + } else{ + if(monitor_port) monitor_port->println("MOT: natural_direction==CW"); + } + + // let the motor stabilize for 2 sec + _delay(2000); + // set sensor to zero + sensor->initRelativeZero(); + _delay(500); + setPhaseVoltage(0, 0, 0); + _delay(200); + + // find the index if available + int exit_flag = absoluteZeroAlign(); + _delay(500); + if(monitor_port){ + if(exit_flag< 0 ) monitor_port->println("MOT: Error: Not found!"); + if(exit_flag> 0 ) monitor_port->println("MOT: Success!"); + else monitor_port->println("MOT: Not available!"); + } + return exit_flag; +} + + +// Encoder alignment the absolute zero angle +// - to the index +int BLDCMotor::absoluteZeroAlign() { + + if(monitor_port) monitor_port->println("MOT: Absolute zero align."); + // if no absolute zero return + if(!sensor->hasAbsoluteZero()) return 0; + + + if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println("MOT: Searching..."); + // search the absolute zero with small velocity + while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){ + loopFOC(); + voltage_q = PID_velocity(velocity_index_search - shaftVelocity()); + } + voltage_q = 0; + // disable motor + setPhaseVoltage(0, 0, 0); + + // align absolute zero if it has been found + if(!sensor->needsAbsoluteZeroSearch()){ + // align the sensor with the absolute zero + float zero_offset = sensor->initAbsoluteZero(); + // remember zero electric angle + zero_electric_angle = _normalizeAngle(_electricalAngle(zero_offset, pole_pairs)); + } + // return bool if zero found + return !sensor->needsAbsoluteZeroSearch() ? 1 : -1; +} + +// Iterative function looping FOC algorithm, setting Uq on the Motor +// The faster it can be run the better +void BLDCMotor::loopFOC() { + // shaft angle + shaft_angle = shaftAngle(); + // set the phase voltage - FOC heart function :) + setPhaseVoltage(voltage_q, voltage_d, _electricalAngle(shaft_angle,pole_pairs)); +} + +// Iterative function running outer loop of the FOC algorithm +// Behavior of this function is determined by the motor.controller variable +// It runs either angle, velocity or voltage loop +// - needs to be called iteratively it is asynchronous function +// - if target is not set it uses motor.target value +void BLDCMotor::move(float new_target) { + // set internal target variable + if( new_target != NOT_SET ) target = new_target; + // get angular velocity + shaft_velocity = shaftVelocity(); + // choose control loop + switch (controller) { + case ControlType::voltage: + voltage_q = target; + break; + case ControlType::angle: + // angle set point + // include angle loop + shaft_angle_sp = target; + shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); + voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + break; + case ControlType::velocity: + // velocity set point + // include velocity loop + shaft_velocity_sp = target; + voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + break; + case ControlType::velocity_openloop: + // velocity control in open loop + // loopFOC should not be called + shaft_velocity_sp = target; + velocityOpenloop(shaft_velocity_sp); + break; + case ControlType::angle_openloop: + // angle control in open loop + // loopFOC should not be called + shaft_angle_sp = target; + angleOpenloop(shaft_angle_sp); + break; + } +} + + +// Method using FOC to set Uq and Ud to the motor at the optimal angle +// Function implementing Space Vector PWM and Sine PWM algorithms +// +// Function using sine approximation +// regular sin + cos ~300us (no memory usaage) +// approx _sin + _cos ~110us (400Byte ~ 20% of memory) +void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { + + const bool centered = true; + int sector; + float _ca,_sa; + + switch (foc_modulation) + { + case FOCModulationType::Trapezoid_120 : + // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5 + static int trap_120_map[6][3] = { + {0,1,-1},{-1,1,0},{-1,0,1},{0,-1,1},{1,-1,0},{1,0,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + }; + // static int trap_120_state = 0; + sector = 6 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + + Ua = Uq + trap_120_map[sector][0] * Uq; + Ub = Uq + trap_120_map[sector][1] * Uq; + Uc = Uq + trap_120_map[sector][2] * Uq; + + if (centered) { + Ua += (driver->voltage_limit)/2 -Uq; + Ub += (driver->voltage_limit)/2 -Uq; + Uc += (driver->voltage_limit)/2 -Uq; + } + break; + + case FOCModulationType::Trapezoid_150 : + // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8 + static int trap_150_map[12][3] = { + {0,1,-1},{-1,1,-1},{-1,1,0},{-1,1,1},{-1,0,1},{-1,-1,1},{0,-1,1},{1,-1,1},{1,-1,0},{1,-1,-1},{1,0,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + }; + // static int trap_150_state = 0; + sector = 12 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + + Ua = Uq + trap_150_map[sector][0] * Uq; + Ub = Uq + trap_150_map[sector][1] * Uq; + Uc = Uq + trap_150_map[sector][2] * Uq; + + //center + if (centered) { + Ua += (driver->voltage_limit)/2 -Uq; + Ub += (driver->voltage_limit)/2 -Uq; + Uc += (driver->voltage_limit)/2 -Uq; + } + + break; + + case FOCModulationType::SinePWM : + // Sinusoidal PWM modulation + // Inverse Park + Clarke transformation + + // angle normalization in between 0 and 2pi + // only necessary if using _sin and _cos - approximation functions + angle_el = _normalizeAngle(angle_el + zero_electric_angle); + _ca = _cos(angle_el); + _sa = _sin(angle_el); + // Inverse park transform + Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; + Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; + + // Clarke transform + Ua = Ualpha + driver->voltage_limit/2; + Ub = -0.5 * Ualpha + _SQRT3_2 * Ubeta + driver->voltage_limit/2; + Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + driver->voltage_limit/2; + + if (!centered) { + float Umin = min(Ua, min(Ub, Uc)); + Ua -= Umin; + Ub -= Umin; + Uc -= Umin; + } + + break; + + case FOCModulationType::SpaceVectorPWM : + // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm + // https://www.youtube.com/watch?v=QMSWUMEAejg + + // if negative voltages change inverse the phase + // angle + 180degrees + if(Uq < 0) angle_el += _PI; + Uq = abs(Uq); + + // angle normalisation in between 0 and 2pi + // only necessary if using _sin and _cos - approximation functions + angle_el = _normalizeAngle(angle_el + zero_electric_angle + _PI_2); + + // find the sector we are in currently + sector = floor(angle_el / _PI_3) + 1; + // calculate the duty cycles + float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_limit; + float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_limit; + // two versions possible + float T0 = 0; // pulled to 0 - better for low power supply voltage + if (centered) { + T0 = 1 - T1 - T2; //centered around driver->voltage_limit/2 + } + + // calculate the duty cycles(times) + float Ta,Tb,Tc; + switch(sector){ + case 1: + Ta = T1 + T2 + T0/2; + Tb = T2 + T0/2; + Tc = T0/2; + break; + case 2: + Ta = T1 + T0/2; + Tb = T1 + T2 + T0/2; + Tc = T0/2; + break; + case 3: + Ta = T0/2; + Tb = T1 + T2 + T0/2; + Tc = T2 + T0/2; + break; + case 4: + Ta = T0/2; + Tb = T1+ T0/2; + Tc = T1 + T2 + T0/2; + break; + case 5: + Ta = T2 + T0/2; + Tb = T0/2; + Tc = T1 + T2 + T0/2; + break; + case 6: + Ta = T1 + T2 + T0/2; + Tb = T0/2; + Tc = T1 + T0/2; + break; + default: + // possible error state + Ta = 0; + Tb = 0; + Tc = 0; + } + + // calculate the phase voltages and center + Ua = Ta*driver->voltage_limit; + Ub = Tb*driver->voltage_limit; + Uc = Tc*driver->voltage_limit; + break; + + } + + // set the voltages in driver + driver->setPwm(Ua, Ub, Uc); +} + + + +// Function (iterative) generating open loop movement for target velocity +// - target_velocity - rad/s +// it uses voltage_limit variable +void BLDCMotor::velocityOpenloop(float target_velocity){ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6; + + // calculate the necessary angle to achieve target velocity + shaft_angle += target_velocity*Ts; + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; +} + +// Function (iterative) generating open loop movement towards the target angle +// - target_angle - rad +// it uses voltage_limit and velocity_limit variables +void BLDCMotor::angleOpenloop(float target_angle){ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6; + + // calculate the necessary angle to move from current position towards target angle + // with maximal velocity (velocity_limit) + if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)) + shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; + else + shaft_angle = target_angle; + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; +} diff --git a/library_source/BLDCMotor.h b/library_source/BLDCMotor.h new file mode 100644 index 00000000..164cc108 --- /dev/null +++ b/library_source/BLDCMotor.h @@ -0,0 +1,109 @@ +#ifndef BLDCMotor_h +#define BLDCMotor_h + +#include "Arduino.h" +#include "common/base_classes/FOCMotor.h" +#include "common/base_classes/Sensor.h" +#include "common/base_classes/BLDCDriver.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" +#include "common/defaults.h" + +/** + BLDC motor class +*/ +class BLDCMotor: public FOCMotor +{ + public: + /** + BLDCMotor class constructor + @param pp pole pairs number + */ + BLDCMotor(int pp); + + /** + * Function linking a motor and a foc driver + * + * @param driver BLDCDriver class implementing all the hardware specific functions necessary PWM setting + */ + void linkDriver(BLDCDriver* driver); + + /** + * BLDCDriver link: + * - 3PWM + * - 6PWM + */ + BLDCDriver* driver; + + /** Motor hardware init function */ + void init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + */ + int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override; + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + void loopFOC() override; + + /** + * Function executing the control loops set by the controller parameter of the BLDCMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + void move(float target = NOT_SET) override; + + float Ua,Ub,Uc;//!< Current phase voltages Ua,Ub and Uc set to motor + float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform + + + private: + // FOC methods + /** + * Method using FOC to set Uq to the motor at the optimal angle + * Heart of the FOC algorithm + * + * @param Uq Current voltage in q axis to set to the motor + * @param Ud Current voltage in d axis to set to the motor + * @param angle_el current electrical angle of the motor + */ + void setPhaseVoltage(float Uq, float Ud, float angle_el); + /** Sensor alignment to electrical 0 angle of the motor */ + int alignSensor(); + /** Motor and sensor alignment to the sensors absolute 0 angle */ + int absoluteZeroAlign(); + + + // Open loop motion control + /** + * Function (iterative) generating open loop movement for target velocity + * it uses voltage_limit variable + * + * @param target_velocity - rad/s + */ + void velocityOpenloop(float target_velocity); + /** + * Function (iterative) generating open loop movement towards the target angle + * it uses voltage_limit and velocity_limit variables + * + * @param target_angle - rad + */ + void angleOpenloop(float target_angle); + // open loop variables + long open_loop_timestamp; +}; + + +#endif diff --git a/library_source/SimpleFOC.h b/library_source/SimpleFOC.h new file mode 100644 index 00000000..bb963ef1 --- /dev/null +++ b/library_source/SimpleFOC.h @@ -0,0 +1,111 @@ +/*! + * @file SimpleFOC.h + * + * @mainpage Simple Field Oriented Control BLDC motor control library + * + * @section intro_sec Introduction + * + * Proper low-cost and low-power FOC supporting boards are very hard to find these days and even may not exist.
Even harder to find is a stable and simple FOC algorithm code capable of running on Arduino devices. Therefore this is an attempt to: + * - Demystify FOC algorithm and make a robust but simple Arduino library: Arduino SimpleFOC library + * - Develop a modular BLDC driver board: Arduino SimpleFOC shield. + * + * @section features Features + * - Arduino compatible: Arduino library code + * - Easy to setup and configure: + * - Easy hardware configuration + * - Easy tuning the control loops + * - Modular: + * - Supports as many sensors , BLDC motors and driver boards as possible + * - Supports as many application requirements as possible + * - Plug & play: Arduino SimpleFOC shield + * + * @section dependencies Supported Hardware + * - Motors + * - BLDC motors + * - Stepper motors + * - Drivers + * - BLDC drivers + * - Gimbal drivers + * - Stepper drivers + * - Position sensors + * - Encoders + * - Magnetic sensors + * - Hall sensors + * - Open-loop control + * - Microcontrollers + * - Arduino + * - STM32 + * - ESP32 + * - Teensy + * + * @section example_code Example code + * @code +#include + +// BLDCMotor( pole_pairs ) +BLDCMotor motor = BLDCMotor(11); +// BLDCDriver( pin_pwmA, pin_pwmB, pin_pwmC, enable (optional) ) +BLDCDriver3PWM motor = BLDCDriver3PWM(9, 10, 11, 8); +// Encoder(pin_A, pin_B, CPR) +Encoder encoder = Encoder(2, 3, 2048); +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + + +void setup() { + // initialize encoder hardware + encoder.init(); + // hardware interrupt enable + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // power supply voltage [V] + driver.voltage_power_supply = 12; + // initialise driver hardware + driver.init(); + // link driver + motor.linkDriver(&driver); + + // set control loop type to be used + motor.controller = ControlType::velocity; + // initialize motor + motor.init(); + + // align encoder and start FOC + motor.initFOC(); +} + +void loop() { + // FOC algorithm function + motor.loopFOC(); + + // velocity control loop function + // setting the target velocity or 2rad/s + motor.move(2); +} + * @endcode + * + * @section license License + * + * MIT license, all text here must be included in any redistribution. + * + */ + +#ifndef SIMPLEFOC_H +#define SIMPLEFOC_H + +#include "BLDCMotor.h" +#include "StepperMotor.h" +#include "sensors/Encoder.h" +#include "sensors/MagneticSensorSPI.h" +#include "sensors/MagneticSensorI2C.h" +#include "sensors/MagneticSensorAnalog.h" +#include "sensors/HallSensor.h" +#include "drivers/BLDCDriver3PWM.h" +#include "drivers/BLDCDriver6PWM.h" +#include "drivers/StepperDriver4PWM.h" +#include "drivers/StepperDriver2PWM.h" + +#endif diff --git a/library_source/StepperMotor.cpp b/library_source/StepperMotor.cpp new file mode 100644 index 00000000..31c81eeb --- /dev/null +++ b/library_source/StepperMotor.cpp @@ -0,0 +1,280 @@ +#include "StepperMotor.h" + +// StepperMotor( int phA, int phB, int phC, int pp, int cpr, int en) +// - ph1A, ph1B - motor phase 1 pwm pins +// - ph2A, ph2B - motor phase 2 pwm pins +// - pp - pole pair number +// - enable pin - (optional input) +StepperMotor::StepperMotor(int pp) +: FOCMotor() +{ + // number od pole pairs + pole_pairs = pp; +} + +/** + Link the driver which controls the motor +*/ +void StepperMotor::linkDriver(StepperDriver* _driver) { + driver = _driver; +} + +// init hardware pins +void StepperMotor::init() { + if(monitor_port) monitor_port->println("MOT: Init variables."); + + // sanity check for the voltage limit configuration + if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; + // constrain voltage for sensor alignment + if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; + + // update the controller limits + PID_velocity.limit = voltage_limit; + P_angle.limit = velocity_limit; + + _delay(500); + // enable motor + if(monitor_port) monitor_port->println("MOT: Enable."); + enable(); + _delay(500); + +} + + +// disable motor driver +void StepperMotor::disable() +{ + // set zero to PWM + driver->setPwm(0, 0); + // disable driver + driver->disable(); +} +// enable motor driver +void StepperMotor::enable() +{ + // disable enable + driver->enable(); + // set zero to PWM + driver->setPwm(0, 0); +} + + +/** + FOC functions +*/ +// FOC initialization function +int StepperMotor::initFOC( float zero_electric_offset, Direction sensor_direction ) { + int exit_flag = 1; + // align motor if necessary + // alignment necessary for encoders! + if(zero_electric_offset != NOT_SET){ + // abosolute zero offset provided - no need to align + zero_electric_angle = zero_electric_offset; + // set the sensor direction - default CW + sensor->natural_direction = sensor_direction; + }else{ + // sensor and motor alignment + _delay(500); + exit_flag = alignSensor(); + _delay(500); + } + if(monitor_port) monitor_port->println("MOT: Motor ready."); + + return exit_flag; +} +// Encoder alignment to electrical 0 angle +int StepperMotor::alignSensor() { + if(monitor_port) monitor_port->println("MOT: Align sensor."); + // align the electrical phases of the motor and sensor + // set angle -90 degrees + + float start_angle = shaftAngle(); + for (int i = 0; i <=5; i++ ) { + float angle = _3PI_2 + _2PI * i / 6.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(200); + } + float mid_angle = shaftAngle(); + for (int i = 5; i >=0; i-- ) { + float angle = _3PI_2 + _2PI * i / 6.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(200); + } + if (mid_angle < start_angle) { + if(monitor_port) monitor_port->println("MOT: natural_direction==CCW"); + sensor->natural_direction = Direction::CCW; + } else if (mid_angle == start_angle) { + if(monitor_port) monitor_port->println("MOT: Sensor failed to notice movement"); + } else{ + if(monitor_port) monitor_port->println("MOT: natural_direction==CW"); + } + + // let the motor stabilize for 2 sec + _delay(2000); + // set sensor to zero + sensor->initRelativeZero(); + _delay(500); + setPhaseVoltage(0, 0, 0); + _delay(200); + + // find the index if available + int exit_flag = absoluteZeroAlign(); + _delay(500); + if(monitor_port){ + if(exit_flag< 0 ) monitor_port->println("MOT: Error: Not found!"); + if(exit_flag> 0 ) monitor_port->println("MOT: Success!"); + else monitor_port->println("MOT: Not available!"); + } + return exit_flag; +} + + +// Encoder alignment the absolute zero angle +// - to the index +int StepperMotor::absoluteZeroAlign() { + + if(monitor_port) monitor_port->println("MOT: Absolute zero align."); + // if no absolute zero return + if(!sensor->hasAbsoluteZero()) return 0; + + + if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println("MOT: Searching..."); + // search the absolute zero with small velocity + while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){ + loopFOC(); + voltage_q = PID_velocity(velocity_index_search - shaftVelocity()); + } + voltage_q = 0; + voltage_d = 0; + // disable motor + setPhaseVoltage(0, 0, 0); + + // align absolute zero if it has been found + if(!sensor->needsAbsoluteZeroSearch()){ + // align the sensor with the absolute zero + float zero_offset = sensor->initAbsoluteZero(); + // remember zero electric angle + zero_electric_angle = _normalizeAngle(_electricalAngle(zero_offset, pole_pairs)); + } + // return bool if zero found + return !sensor->needsAbsoluteZeroSearch() ? 1 : -1; +} + +// Iterative function looping FOC algorithm, setting Uq on the Motor +// The faster it can be run the better +void StepperMotor::loopFOC() { + // shaft angle + shaft_angle = shaftAngle(); + // set the phase voltage - FOC heart function :) + setPhaseVoltage(voltage_q, voltage_d, _electricalAngle(shaft_angle, pole_pairs)); +} + +// Iterative function running outer loop of the FOC algorithm +// Behavior of this function is determined by the motor.controller variable +// It runs either angle, velocity or voltage loop +// - needs to be called iteratively it is asynchronous function +// - if target is not set it uses motor.target value +void StepperMotor::move(float new_target) { + // set internal target variable + if( new_target != NOT_SET ) target = new_target; + // get angular velocity + shaft_velocity = shaftVelocity(); + // choose control loop + switch (controller) { + case ControlType::voltage: + voltage_q = target; + break; + case ControlType::angle: + // angle set point + // include angle loop + shaft_angle_sp = target; + shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); + voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + break; + case ControlType::velocity: + // velocity set point + // include velocity loop + shaft_velocity_sp = target; + voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + break; + case ControlType::velocity_openloop: + // velocity control in open loop + // loopFOC should not be called + shaft_velocity_sp = target; + velocityOpenloop(shaft_velocity_sp); + break; + case ControlType::angle_openloop: + // angle control in open loop + // loopFOC should not be called + shaft_angle_sp = target; + angleOpenloop(shaft_angle_sp); + break; + } +} + + +// Method using FOC to set Uq and Ud to the motor at the optimal angle +// Function implementing Sine PWM algorithms +// - space vector not implemented yet +// +// Function using sine approximation +// regular sin + cos ~300us (no memory usaage) +// approx _sin + _cos ~110us (400Byte ~ 20% of memory) +void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { + // Sinusoidal PWM modulation + // Inverse Park transformation + + // angle normalization in between 0 and 2pi + // only necessary if using _sin and _cos - approximation functions + angle_el = _normalizeAngle(angle_el + zero_electric_angle); + float _ca = _cos(angle_el); + float _sa = _sin(angle_el); + // Inverse park transform + Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; + Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; + + // set the voltages in hardware + driver->setPwm(Ualpha, Ubeta); +} + +// Function (iterative) generating open loop movement for target velocity +// - target_velocity - rad/s +// it uses voltage_limit variable +void StepperMotor::velocityOpenloop(float target_velocity){ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6; + + // calculate the necessary angle to achieve target velocity + shaft_angle += target_velocity*Ts; + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle,pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; +} + +// Function (iterative) generating open loop movement towards the target angle +// - target_angle - rad +// it uses voltage_limit and velocity_limit variables +void StepperMotor::angleOpenloop(float target_angle){ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6; + + // calculate the necessary angle to move from current position towards target angle + // with maximal velocity (velocity_limit) + if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)) + shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; + else + shaft_angle = target_angle; + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle,pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; +} \ No newline at end of file diff --git a/library_source/StepperMotor.h b/library_source/StepperMotor.h new file mode 100644 index 00000000..af4783bf --- /dev/null +++ b/library_source/StepperMotor.h @@ -0,0 +1,117 @@ +/** + * @file StepperMotor.h + * + */ + +#ifndef StepperMotor_h +#define StepperMotor_h + +#include "Arduino.h" +#include "common/base_classes/FOCMotor.h" +#include "common/base_classes/StepperDriver.h" +#include "common/base_classes/Sensor.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" +#include "common/defaults.h" + +/** + Stepper Motor class +*/ +class StepperMotor: public FOCMotor +{ + public: + /** + StepperMotor class constructor + @param pp pole pair number - cpr counts per rotation number (cpm=ppm*4) + */ + StepperMotor(int pp); + + /** + * Function linking a motor and a foc driver + * + * @param driver StepperDriver class implementing all the hardware specific functions necessary PWM setting + */ + void linkDriver(StepperDriver* driver); + + /** + * StepperDriver link: + * - 4PWM - L298N for example + */ + StepperDriver* driver; + + /** Motor hardware init function */ + void init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + * + * - If zero_electric_offset parameter is set the alignment procedure is skipped + * + * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. + * @param sensor_direction sensor natural direction - default is CW + * + */ + int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override; + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + void loopFOC() override; + /** + * Function executing the control loops set by the controller parameter of the BLDCMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + void move(float target = NOT_SET) override; + + float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform + + private: + + // FOC methods + /** + * Method using FOC to set Uq to the motor at the optimal angle + * Heart of the FOC algorithm + * + * @param Uq Current voltage in q axis to set to the motor + * @param Ud Current voltage in d axis to set to the motor + * @param angle_el current electrical angle of the motor + */ + void setPhaseVoltage(float Uq, float Ud , float angle_el); + + /** Sensor alignment to electrical 0 angle of the motor */ + int alignSensor(); + /** Motor and sensor alignment to the sensors absolute 0 angle */ + int absoluteZeroAlign(); + + // Open loop motion control + /** + * Function (iterative) generating open loop movement for target velocity + * it uses voltage_limit variable + * + * @param target_velocity - rad/s + */ + void velocityOpenloop(float target_velocity); + /** + * Function (iterative) generating open loop movement towards the target angle + * it uses voltage_limit and velocity_limit variables + * + * @param target_angle - rad + */ + void angleOpenloop(float target_angle); + // open loop variables + long open_loop_timestamp; +}; + + +#endif diff --git a/library_source/common/base_classes/BLDCDriver.h b/library_source/common/base_classes/BLDCDriver.h new file mode 100644 index 00000000..708323fb --- /dev/null +++ b/library_source/common/base_classes/BLDCDriver.h @@ -0,0 +1,28 @@ +#ifndef BLDCDRIVER_H +#define BLDCDRIVER_H + +class BLDCDriver{ + public: + + /** Initialise hardware */ + virtual int init(); + /** Enable hardware */ + virtual void enable(); + /** Disable hardware */ + virtual void disable(); + + long pwm_frequency; //!< pwm frequency value in hertz + float voltage_power_supply; //!< power supply voltage + float voltage_limit; //!< limiting voltage set to the motor + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + virtual void setPwm(float Ua, float Ub, float Uc); +}; + +#endif \ No newline at end of file diff --git a/library_source/common/base_classes/FOCMotor.cpp b/library_source/common/base_classes/FOCMotor.cpp new file mode 100644 index 00000000..1d1a0f43 --- /dev/null +++ b/library_source/common/base_classes/FOCMotor.cpp @@ -0,0 +1,241 @@ +#include "FOCMotor.h" + +/** + * Default constructor - setting all variabels to default values + */ +FOCMotor::FOCMotor() +{ + // maximum angular velocity to be used for positioning + velocity_limit = DEF_VEL_LIM; + // maximum voltage to be set to the motor + voltage_limit = DEF_POWER_SUPPLY; + + // index search velocity + velocity_index_search = DEF_INDEX_SEARCH_TARGET_VELOCITY; + // sensor and motor align voltage + voltage_sensor_align = DEF_VOLTAGE_SENSOR_ALIGN; + + // electric angle of comthe zero angle + zero_electric_angle = 0; + + // default modulation is SinePWM + foc_modulation = FOCModulationType::SinePWM; + + // default target value + target = 0; + voltage_d = 0; + voltage_q = 0; + + //monitor_port + monitor_port = nullptr; + //sensor + sensor = nullptr; +} + + +/** + Sensor communication methods +*/ +void FOCMotor::linkSensor(Sensor* _sensor) { + sensor = _sensor; +} +// shaft angle calculation +float FOCMotor::shaftAngle() { + // if no sensor linked return 0 + if(!sensor) return shaft_angle; + return sensor->getAngle(); +} +// shaft velocity calculation +float FOCMotor::shaftVelocity() { + // if no sensor linked return 0 + if(!sensor) return 0; + return LPF_velocity(sensor->getVelocity()); +} + +/** + * Monitoring functions + */ +// function implementing the monitor_port setter +void FOCMotor::useMonitoring(Print &print){ + monitor_port = &print; //operate on the address of print + if(monitor_port ) monitor_port->println("MOT: Monitor enabled!"); +} +// utility function intended to be used with serial plotter to monitor motor variables +// significantly slowing the execution down!!!! +void FOCMotor::monitor() { + if(!monitor_port) return; + switch (controller) { + case ControlType::velocity_openloop: + case ControlType::velocity: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_velocity_sp); + monitor_port->print("\t"); + monitor_port->println(shaft_velocity); + break; + case ControlType::angle_openloop: + case ControlType::angle: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_angle_sp); + monitor_port->print("\t"); + monitor_port->println(shaft_angle); + break; + case ControlType::voltage: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_angle); + monitor_port->print("\t"); + monitor_port->println(shaft_velocity); + break; + } +} + +int FOCMotor::command(String user_command) { + // error flag + int errorFlag = 1; + // if empty string + if(user_command.length() < 1) return errorFlag; + + // parse command letter + char cmd = user_command.charAt(0); + // check if get command + char GET = user_command.charAt(1) == '\n'; + // parse command values + float value = user_command.substring(1).toFloat(); + + // a bit of optimisation of variable memory for Arduino UNO (atmega328) + switch(cmd){ + case 'P': // velocity P gain change + case 'I': // velocity I gain change + case 'D': // velocity D gain change + case 'R': // velocity voltage ramp change + if(monitor_port) monitor_port->print(" PID velocity| "); + break; + case 'F': // velocity Tf low pass filter change + if(monitor_port) monitor_port->print(" LPF velocity| "); + break; + case 'K': // angle loop gain P change + if(monitor_port) monitor_port->print(" P angle| "); + break; + case 'L': // velocity voltage limit change + case 'N': // angle loop gain velocity_limit change + if(monitor_port) monitor_port->print(" Limits| "); + break; + } + + // apply the the command + switch(cmd){ + case 'P': // velocity P gain change + if(monitor_port) monitor_port->print("P: "); + if(!GET) PID_velocity.P = value; + if(monitor_port) monitor_port->println(PID_velocity.P); + break; + case 'I': // velocity I gain change + if(monitor_port) monitor_port->print("I: "); + if(!GET) PID_velocity.I = value; + if(monitor_port) monitor_port->println(PID_velocity.I); + break; + case 'D': // velocity D gain change + if(monitor_port) monitor_port->print("D: "); + if(!GET) PID_velocity.D = value; + if(monitor_port) monitor_port->println(PID_velocity.D); + break; + case 'R': // velocity voltage ramp change + if(monitor_port) monitor_port->print("volt_ramp: "); + if(!GET) PID_velocity.output_ramp = value; + if(monitor_port) monitor_port->println(PID_velocity.output_ramp); + break; + case 'L': // velocity voltage limit change + if(monitor_port) monitor_port->print("volt_limit: "); + if(!GET) { + voltage_limit = value; + PID_velocity.limit = value; + } + if(monitor_port) monitor_port->println(voltage_limit); + break; + case 'F': // velocity Tf low pass filter change + if(monitor_port) monitor_port->print("Tf: "); + if(!GET) LPF_velocity.Tf = value; + if(monitor_port) monitor_port->println(LPF_velocity.Tf); + break; + case 'K': // angle loop gain P change + if(monitor_port) monitor_port->print(" P: "); + if(!GET) P_angle.P = value; + if(monitor_port) monitor_port->println(P_angle.P); + break; + case 'N': // angle loop gain velocity_limit change + if(monitor_port) monitor_port->print("vel_limit: "); + if(!GET){ + velocity_limit = value; + P_angle.limit = value; + } + if(monitor_port) monitor_port->println(velocity_limit); + break; + case 'C': + // change control type + if(monitor_port) monitor_port->print("Control: "); + + if(GET){ // if get command + switch(controller){ + case ControlType::voltage: + if(monitor_port) monitor_port->println("voltage"); + break; + case ControlType::velocity: + if(monitor_port) monitor_port->println("velocity"); + break; + case ControlType::angle: + if(monitor_port) monitor_port->println("angle"); + break; + default: + if(monitor_port) monitor_port->println("open loop"); + } + }else{ // if set command + switch((int)value){ + case 0: + if(monitor_port) monitor_port->println("voltage"); + controller = ControlType::voltage; + break; + case 1: + if(monitor_port) monitor_port->println("velocity"); + controller = ControlType::velocity; + break; + case 2: + if(monitor_port) monitor_port->println("angle"); + controller = ControlType::angle; + break; + default: // not valid command + errorFlag = 0; + } + } + break; + case 'V': // get current values of the state variables + switch((int)value){ + case 0: // get voltage + if(monitor_port) monitor_port->print("Uq: "); + if(monitor_port) monitor_port->println(voltage_q); + break; + case 1: // get velocity + if(monitor_port) monitor_port->print("Velocity: "); + if(monitor_port) monitor_port->println(shaft_velocity); + break; + case 2: // get angle + if(monitor_port) monitor_port->print("Angle: "); + if(monitor_port) monitor_port->println(shaft_angle); + break; + case 3: // get angle + if(monitor_port) monitor_port->print("Target: "); + if(monitor_port) monitor_port->println(target); + break; + default: // not valid command + errorFlag = 0; + } + break; + default: // target change + if(monitor_port) monitor_port->print("Target : "); + target = user_command.toFloat(); + if(monitor_port) monitor_port->println(target); + } + // return 0 if error and 1 if ok + return errorFlag; +} \ No newline at end of file diff --git a/library_source/common/base_classes/FOCMotor.h b/library_source/common/base_classes/FOCMotor.h new file mode 100644 index 00000000..986e1ab2 --- /dev/null +++ b/library_source/common/base_classes/FOCMotor.h @@ -0,0 +1,197 @@ +#ifndef FOCMOTOR_H +#define FOCMOTOR_H + +#include "Arduino.h" +#include "Sensor.h" +#include "BLDCDriver.h" + +#include "../time_utils.h" +#include "../foc_utils.h" +#include "../defaults.h" +#include "../pid.h" +#include "../lowpass_filter.h" + + +/** + * Motiron control type + */ +enum ControlType{ + voltage,//!< Torque control using voltage + velocity,//!< Velocity motion control + angle,//!< Position/angle motion control + velocity_openloop, + angle_openloop +}; + +/** + * FOC modulation type + */ +enum FOCModulationType{ + SinePWM, //!< Sinusoidal PWM modulation + SpaceVectorPWM, //!< Space vector modulation method + Trapezoid_120, + Trapezoid_150 +}; + +/** + Generic motor class +*/ +class FOCMotor +{ + public: + /** + * Default constructor - setting all variabels to default values + */ + FOCMotor(); + + /** Motor hardware init function */ + virtual void init()=0; + /** Motor disable function */ + virtual void disable()=0; + /** Motor enable function */ + virtual void enable()=0; + + /** + * Function linking a motor and a sensor + * + * @param sensor Sensor class wrapper for the FOC algorihtm to read the motor angle and velocity + */ + void linkSensor(Sensor* sensor); + + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + * + * - If zero_electric_offset parameter is set the alignment procedure is skipped + * + * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. + * @param sensor_direction sensor natural direction - default is CW + * + */ + virtual int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW)=0; + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + virtual void loopFOC()=0; + /** + * Function executing the control loops set by the controller parameter of the BLDCMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + virtual void move(float target = NOT_SET)=0; + + // State calculation methods + /** Shaft angle calculation in radians [rad] */ + float shaftAngle(); + /** + * Shaft angle calculation function in radian per second [rad/s] + * It implements low pass filtering + */ + float shaftVelocity(); + + // state variables + float target; //!< current target value - depends of the controller + float shaft_angle;//!< current motor angle + float shaft_velocity;//!< current motor velocity + float shaft_velocity_sp;//!< current target velocity + float shaft_angle_sp;//!< current target angle + float voltage_q;//!< current voltage u_q set + float voltage_d;//!< current voltage u_d set + + // motor configuration parameters + float voltage_sensor_align;//!< sensor and motor align voltage parameter + float velocity_index_search;//!< target velocity for index search + int pole_pairs;//!< Motor pole pairs number + + // limiting variables + float voltage_limit; //!< Voltage limitting variable - global limit + float velocity_limit; //!< Velocity limitting variable - global limit + + float zero_electric_angle;//! _2PI ? a_sin - _2PI : a_sin; + return _sin(a_sin); +} + + +// normalizing radian angle to [0,2PI] +float _normalizeAngle(float angle){ + float a = fmod(angle, _2PI); + return a >= 0 ? a : (a + _2PI); +} + +// Electrical angle calculation +float _electricalAngle(float shaft_angle, int pole_pairs) { + return (shaft_angle * pole_pairs); +} \ No newline at end of file diff --git a/library_source/common/foc_utils.h b/library_source/common/foc_utils.h new file mode 100644 index 00000000..7da3f7bc --- /dev/null +++ b/library_source/common/foc_utils.h @@ -0,0 +1,55 @@ +#ifndef FOCUTILS_LIB_H +#define FOCUTILS_LIB_H + +#include "Arduino.h" + +// sign function +#define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) ) +#define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) +#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) + +// utility defines +#define _2_SQRT3 1.15470053838 +#define _SQRT3 1.73205080757 +#define _1_SQRT3 0.57735026919 +#define _SQRT3_2 0.86602540378 +#define _SQRT2 1.41421356237 +#define _120_D2R 2.09439510239 +#define _PI 3.14159265359 +#define _PI_2 1.57079632679 +#define _PI_3 1.0471975512 +#define _2PI 6.28318530718 +#define _3PI_2 4.71238898038 + +#define NOT_SET -12345.0 + +/** + * Function approximating the sine calculation by using fixed size array + * - execution time ~40us (Arduino UNO) + * + * @param a angle in between 0 and 2PI + */ +float _sin(float a); +/** + * Function approximating cosine calculation by using fixed size array + * - execution time ~50us (Arduino UNO) + * + * @param a angle in between 0 and 2PI + */ +float _cos(float a); + +/** + * normalizing radian angle to [0,2PI] + * @param angle - angle to be normalized + */ +float _normalizeAngle(float angle); + + +/** + * Electrical angle calculation + * + * @param shaft_angle - shaft angle of the motor + * @param pole_pairs - number of pole pairs + */ +float _electricalAngle(float shaft_angle, int pole_pairs); +#endif \ No newline at end of file diff --git a/library_source/common/lowpass_filter.cpp b/library_source/common/lowpass_filter.cpp new file mode 100644 index 00000000..9bf4ab7a --- /dev/null +++ b/library_source/common/lowpass_filter.cpp @@ -0,0 +1,25 @@ +#include "lowpass_filter.h" + +LowPassFilter::LowPassFilter(float time_constant) + : Tf(time_constant) + , y_prev(0.0f) +{ + timestamp_prev = _micros(); +} + + +float LowPassFilter::operator() (float x) +{ + unsigned long timestamp = _micros(); + float dt = (timestamp - timestamp_prev)*1e-6f; + + if (dt < 0.0f || dt > 0.5f) + dt = 1e-3f; + + float alpha = Tf/(Tf + dt); + float y = alpha*y_prev + (1.0f - alpha)*x; + + y_prev = y; + timestamp_prev = timestamp; + return y; +} \ No newline at end of file diff --git a/library_source/common/lowpass_filter.h b/library_source/common/lowpass_filter.h new file mode 100644 index 00000000..7c51be54 --- /dev/null +++ b/library_source/common/lowpass_filter.h @@ -0,0 +1,29 @@ +#ifndef LOWPASS_FILTER_H +#define LOWPASS_FILTER_H + + +#include "time_utils.h" +#include "foc_utils.h" + + +/** + * Low pass filter class + */ +class LowPassFilter +{ +public: + /** + * @param Tf - Low pass filter time constant + */ + LowPassFilter(float Tf); + ~LowPassFilter() = default; + + float operator() (float x); + float Tf; //!< Low pass filter time constant + +protected: + unsigned long timestamp_prev; //!< Last execution timestamp + float y_prev; //!< filtered value in previous execution step +}; + +#endif // LOWPASS_FILTER_H \ No newline at end of file diff --git a/library_source/common/pid.cpp b/library_source/common/pid.cpp new file mode 100644 index 00000000..1a0a9828 --- /dev/null +++ b/library_source/common/pid.cpp @@ -0,0 +1,56 @@ +#include "pid.h" + +PIDController::PIDController(float P, float I, float D, float ramp, float limit) + : P(P) + , I(I) + , D(D) + , output_ramp(ramp) // output derivative limit [volts/second] + , limit(limit) // output supply limit [volts] + , integral_prev(0.0) + , error_prev(0.0) + , output_prev(0.0) +{ + timestamp_prev = _micros()*1e-6; +} + +// PID controller function +float PIDController::operator() (float error){ + // calculate the time from the last call + unsigned long timestamp_now = _micros(); + float Ts = (timestamp_now - timestamp_prev) * 1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + + // u(s) = (P + I/s + Ds)e(s) + // Discrete implementations + // proportional part + // u_p = P *e(k) + float proportional = P * error; + // Tustin transform of the integral part + // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) + float integral = integral_prev + I*Ts*0.5*(error + error_prev); + // antiwindup - limit the output voltage_q + integral = _constrain(integral, -limit, limit); + // Discrete derivation + // u_dk = D(ek - ek_1)/Ts + float derivative = D*(error - error_prev)/Ts; + + // sum all the components + float output = proportional + integral + derivative; + // antiwindup - limit the output variable + output = _constrain(output, -limit, limit); + + // limit the acceleration by ramping the output + float output_rate = (output - output_prev)/Ts; + if (output_rate > output_ramp) + output = output_prev + output_ramp*Ts; + else if (output_rate < -output_ramp) + output = output_prev - output_ramp*Ts; + + // saving for the next pass + integral_prev = integral; + output_prev = output; + error_prev = error; + timestamp_prev = timestamp_now; + return output; +} \ No newline at end of file diff --git a/library_source/common/pid.h b/library_source/common/pid.h new file mode 100644 index 00000000..0e9ddf54 --- /dev/null +++ b/library_source/common/pid.h @@ -0,0 +1,40 @@ +#ifndef PID_H +#define PID_H + + +#include "time_utils.h" +#include "foc_utils.h" + +/** + * PID controller class + */ +class PIDController +{ +public: + /** + * + * @param P - Proportional gain + * @param I - Integral gain + * @param D - Derivative gain + * @param ramp - Maximum speed of change of the output value + * @param limit - Maximum output value + */ + PIDController(float P, float I, float D, float ramp, float limit); + ~PIDController() = default; + + float operator() (float error); + + float P; //!< Proportional gain + float I; //!< Integral gain + float D; //!< Derivative gain + float output_ramp; //!< Maximum speed of change of the output value + float limit; //!< Maximum output value + +protected: + float integral_prev; //!< last integral component value + float error_prev; //!< last tracking error value + float timestamp_prev; //!< Last execution timestamp + float output_prev; //!< last pid output value +}; + +#endif // PID_H \ No newline at end of file diff --git a/library_source/common/time_utils.cpp b/library_source/common/time_utils.cpp new file mode 100644 index 00000000..b00a8c78 --- /dev/null +++ b/library_source/common/time_utils.cpp @@ -0,0 +1,31 @@ +#include "time_utils.h" + +// function buffering delay() +// arduino uno function doesn't work well with interrupts +void _delay(unsigned long ms){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega2560__) + // if arduino uno and other atmega328p chips + // use while instad of delay, + // due to wrong measurement based on changed timer0 + unsigned long t = _micros() + ms*1000; + while( _micros() < t ){}; +#else + // regular micros + delay(ms); +#endif +} + + +// function buffering _micros() +// arduino function doesn't work well with interrupts +unsigned long _micros(){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega2560__) +// if arduino uno and other atmega328p chips + //return the value based on the prescaler + if((TCCR0B & 0b00000111) == 0x01) return (micros()/32); + else return (micros()); +#else + // regular micros + return micros(); +#endif +} diff --git a/library_source/common/time_utils.h b/library_source/common/time_utils.h new file mode 100644 index 00000000..143d4853 --- /dev/null +++ b/library_source/common/time_utils.h @@ -0,0 +1,22 @@ +#ifndef TIME_UTILS_H +#define TIME_UTILS_H + +#include "foc_utils.h" + +/** + * Function implementing delay() function in milliseconds + * - blocking function + * - hardware specific + + * @param ms number of milliseconds to wait + */ +void _delay(unsigned long ms); + +/** + * Function implementing timestamp getting function in microseconds + * hardware specific + */ +unsigned long _micros(); + + +#endif \ No newline at end of file diff --git a/library_source/drivers/BLDCDriver3PWM.cpp b/library_source/drivers/BLDCDriver3PWM.cpp new file mode 100644 index 00000000..a3fc052d --- /dev/null +++ b/library_source/drivers/BLDCDriver3PWM.cpp @@ -0,0 +1,72 @@ +#include "BLDCDriver3PWM.h" + +BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en){ + // Pin initialization + pwmA = phA; + pwmB = phB; + pwmC = phC; + + // enable_pin pin + enable_pin = en; + + // default power-supply value + voltage_power_supply = DEF_POWER_SUPPLY; + voltage_limit = NOT_SET; + +} + +// enable motor driver +void BLDCDriver3PWM::enable(){ + // enable_pin the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, HIGH); + // set zero to PWM + setPwm(0,0,0); +} + +// disable motor driver +void BLDCDriver3PWM::disable() +{ + // set zero to PWM + setPwm(0, 0, 0); + // disable the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, LOW); + +} + +// init hardware pins +int BLDCDriver3PWM::init() { + // a bit of separation + _delay(1000); + + // PWM pins + pinMode(pwmA, OUTPUT); + pinMode(pwmB, OUTPUT); + pinMode(pwmC, OUTPUT); + if(enable_pin != NOT_SET) pinMode(enable_pin, OUTPUT); + + + // sanity check for the voltage limit configuration + if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + + // Set the pwm frequency to the pins + // hardware specific function - depending on driver and mcu + _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC); + return 0; +} + + +// Set voltage to the pwm pin +void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { + // limit the voltage in driver + Ua = _constrain(Ua, 0.0, voltage_limit); + Ub = _constrain(Ub, 0.0, voltage_limit); + Uc = _constrain(Uc, 0.0, voltage_limit); + // calculate duty cycle + // limited in [0,1] + float dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 ); + float dc_b = _constrain(Ub / voltage_power_supply, 0.0 , 1.0 ); + float dc_c = _constrain(Uc / voltage_power_supply, 0.0 , 1.0 ); + // hardware specific writing + // hardware specific function - depending on driver and mcu + _writeDutyCycle3PWM(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC); +} \ No newline at end of file diff --git a/library_source/drivers/BLDCDriver3PWM.h b/library_source/drivers/BLDCDriver3PWM.h new file mode 100644 index 00000000..b6f7d7f8 --- /dev/null +++ b/library_source/drivers/BLDCDriver3PWM.h @@ -0,0 +1,52 @@ +#ifndef BLDCDriver3PWM_h +#define BLDCDriver3PWM_h + +#include "../common/base_classes/BLDCDriver.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "hardware_api.h" + +/** + 3 pwm bldc driver class +*/ +class BLDCDriver3PWM: public BLDCDriver +{ + public: + /** + BLDCDriver class constructor + @param phA A phase pwm pin + @param phB B phase pwm pin + @param phC C phase pwm pin + @param en enable pin (optional input) + */ + BLDCDriver3PWM(int phA,int phB,int phC, int en = NOT_SET); + + /** Motor hardware init function */ + int init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + // hardware variables + int pwmA; //!< phase A pwm pin number + int pwmB; //!< phase B pwm pin number + int pwmC; //!< phase C pwm pin number + int enable_pin; //!< enable pin number + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + void setPwm(float Ua, float Ub, float Uc) override; + + private: + +}; + + +#endif diff --git a/library_source/drivers/BLDCDriver6PWM.cpp b/library_source/drivers/BLDCDriver6PWM.cpp new file mode 100644 index 00000000..266a5412 --- /dev/null +++ b/library_source/drivers/BLDCDriver6PWM.cpp @@ -0,0 +1,80 @@ +#include "BLDCDriver6PWM.h" + +BLDCDriver6PWM::BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h,int phC_l, int en){ + // Pin initialization + pwmA_h = phA_h; + pwmB_h = phB_h; + pwmC_h = phC_h; + pwmA_l = phA_l; + pwmB_l = phB_l; + pwmC_l = phC_l; + + // enable_pin pin + enable_pin = en; + + // default power-supply value + voltage_power_supply = DEF_POWER_SUPPLY; + voltage_limit = NOT_SET; + + // dead zone initial - 2% + dead_zone = 0.02; + +} + +// enable motor driver +void BLDCDriver6PWM::enable(){ + // enable_pin the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, HIGH); + // set zero to PWM + setPwm(0, 0, 0); +} + +// disable motor driver +void BLDCDriver6PWM::disable() +{ + // set zero to PWM + setPwm(0, 0, 0); + // disable the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, LOW); + +} + +// init hardware pins +int BLDCDriver6PWM::init() { + // a bit of separation + _delay(1000); + + // PWM pins + pinMode(pwmA_l, OUTPUT); + pinMode(pwmB_h, OUTPUT); + pinMode(pwmC_h, OUTPUT); + pinMode(pwmA_l, OUTPUT); + pinMode(pwmB_l, OUTPUT); + pinMode(pwmC_l, OUTPUT); + if(enable_pin != NOT_SET) pinMode(enable_pin, OUTPUT); + + + // sanity check for the voltage limit configuration + if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + + // configure 6pwm + // hardware specific function - depending on driver and mcu + return _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); +} + + +// Set voltage to the pwm pin +void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { + // limit the voltage in driver + Ua = _constrain(Ua, 0, voltage_limit); + Ub = _constrain(Ub, 0, voltage_limit); + Uc = _constrain(Uc, 0, voltage_limit); + // calculate duty cycle + // limited in [0,1] + float dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 ); + float dc_b = _constrain(Ub / voltage_power_supply, 0.0 , 1.0 ); + float dc_c = _constrain(Uc / voltage_power_supply, 0.0 , 1.0 ); + // hardware specific writing + // hardware specific function - depending on driver and mcu + _writeDutyCycle6PWM(dc_a, dc_b, dc_c, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); +} \ No newline at end of file diff --git a/library_source/drivers/BLDCDriver6PWM.h b/library_source/drivers/BLDCDriver6PWM.h new file mode 100644 index 00000000..d13b69fd --- /dev/null +++ b/library_source/drivers/BLDCDriver6PWM.h @@ -0,0 +1,57 @@ +#ifndef BLDCDriver6PWM_h +#define BLDCDriver6PWM_h + +#include "../common/base_classes/BLDCDriver.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "hardware_api.h" + +/** + 6 pwm bldc driver class +*/ +class BLDCDriver6PWM: public BLDCDriver +{ + public: + /** + BLDCDriver class constructor + @param phA_h A phase pwm pin + @param phA_l A phase pwm pin + @param phB_h B phase pwm pin + @param phB_l A phase pwm pin + @param phC_h C phase pwm pin + @param phC_l A phase pwm pin + @param en enable pin (optional input) + */ + BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h,int phC_l, int en = NOT_SET); + + /** Motor hardware init function */ + int init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + // hardware variables + int pwmA_h,pwmA_l; //!< phase A pwm pin number + int pwmB_h,pwmB_l; //!< phase B pwm pin number + int pwmC_h,pwmC_l; //!< phase C pwm pin number + int enable_pin; //!< enable pin number + + float dead_zone; //!< a percentage of dead-time(zone) (both high and low side in low) for each pwm cycle [0,1] + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + void setPwm(float Ua, float Ub, float Uc) override; + + private: + +}; + + +#endif diff --git a/library_source/drivers/StepperDriver2PWM.cpp b/library_source/drivers/StepperDriver2PWM.cpp new file mode 100644 index 00000000..72182648 --- /dev/null +++ b/library_source/drivers/StepperDriver2PWM.cpp @@ -0,0 +1,98 @@ +#include "StepperDriver2PWM.h" + +StepperDriver2PWM::StepperDriver2PWM(int ph1PWM, int ph1INA, int ph1INB, int ph2PWM, int ph2INA, int ph2INB, int en1, int en2){ + // Pin initialization + pwm1 = ph1PWM; //!< phase 1 pwm pin number + ina1 = ph1INA; //!< phase 1 INA pin number + inb1 = ph1INB; //!< phase 1 INB pin number + pwm2 = ph2PWM; //!< phase 2 pwm pin number + ina2 = ph2INA; //!< phase 2 INA pin number + inb2 = ph2INB; //!< phase 2 INB pin number + + // enable_pin pin + enable_pin1 = en1; + enable_pin2 = en2; + + // default power-supply value + voltage_power_supply = DEF_POWER_SUPPLY; + voltage_limit = NOT_SET; + +} + +// enable motor driver +void StepperDriver2PWM::enable(){ + // enable_pin the driver - if enable_pin pin available + if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, HIGH); + if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, HIGH); + // set zero to PWM + setPwm(0,0); +} + +// disable motor driver +void StepperDriver2PWM::disable() +{ + // set zero to PWM + setPwm(0, 0); + // disable the driver - if enable_pin pin available + if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, LOW); + if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, LOW); + +} + +// init hardware pins +int StepperDriver2PWM::init() { + // a bit of separation + _delay(1000); + + // PWM pins + pinMode(pwm1, OUTPUT); + pinMode(pwm2, OUTPUT); + pinMode(ina1, OUTPUT); + pinMode(ina2, OUTPUT); + pinMode(inb1, OUTPUT); + pinMode(inb2, OUTPUT); + + if(enable_pin1 != NOT_SET) pinMode(enable_pin1, OUTPUT); + if(enable_pin2 != NOT_SET) pinMode(enable_pin2, OUTPUT); + + // sanity check for the voltage limit configuration + if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + + // Set the pwm frequency to the pins + // hardware specific function - depending on driver and mcu + _configure2PWM(pwm_frequency, pwm1, pwm2); + return 0; +} + + +// Set voltage to the pwm pin +void StepperDriver2PWM::setPwm(float Ualpha, float Ubeta) { + float duty_cycle1(0.0),duty_cycle2(0.0); + // limit the voltage in driver + Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit); + Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit); + // hardware specific writing + duty_cycle1 = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); + duty_cycle2 = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0); + // set direction + if( Ualpha > 0 ){ + digitalWrite(inb1, LOW); + digitalWrite(ina1, HIGH); + } + else{ + digitalWrite(ina1, LOW); + digitalWrite(inb1, HIGH); + } + + if( Ubeta > 0 ){ + digitalWrite(ina2, LOW); + digitalWrite(inb2, HIGH); + } + else{ + digitalWrite(inb2, LOW); + digitalWrite(ina2, HIGH); + } + + // write to hardware + _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, pwm1, pwm2); +} \ No newline at end of file diff --git a/library_source/drivers/StepperDriver2PWM.h b/library_source/drivers/StepperDriver2PWM.h new file mode 100644 index 00000000..ef8797ee --- /dev/null +++ b/library_source/drivers/StepperDriver2PWM.h @@ -0,0 +1,59 @@ +#ifndef STEPPER_DRIVER_2PWM_h +#define STEPPER_DRIVER_2PWM_h + +#include "../common/base_classes/StepperDriver.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "hardware_api.h" + +/** + 2 pwm stepper driver class +*/ +class StepperDriver2PWM: public StepperDriver +{ + public: + /** + StepperMotor class constructor + @param ph1PWM PWM1 phase pwm pin + @param ph1INA IN1A phase dir pin + @param ph1INB IN1B phase dir pin + @param ph2PWM PWM2 phase pwm pin + @param ph2INA IN2A phase dir pin + @param ph2INB IN2B phase dir pin + @param en1 enable pin phase 1 (optional input) + @param en2 enable pin phase 2 (optional input) + */ + StepperDriver2PWM(int ph1PWM,int ph1INA,int ph1INB,int ph2PWM,int ph2INA,int ph2INB, int en1 = NOT_SET, int en2 = NOT_SET); + + /** Motor hardware init function */ + int init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + // hardware variables + int pwm1; //!< phase 1 pwm pin number + int ina1; //!< phase 1 INA pin number + int inb1; //!< phase 1 INB pin number + int pwm2; //!< phase 2 pwm pin number + int ina2; //!< phase 2 INA pin number + int inb2; //!< phase 2 INB pin number + int enable_pin1; //!< enable pin number phase 1 + int enable_pin2; //!< enable pin number phase 2 + + /** + * Set phase voltages to the harware + * + * @param Ua phase A voltage + * @param Ub phase B voltage + */ + void setPwm(float Ua, float Ub) override; + + private: + +}; + + +#endif diff --git a/library_source/drivers/StepperDriver4PWM.cpp b/library_source/drivers/StepperDriver4PWM.cpp new file mode 100644 index 00000000..28e2dcae --- /dev/null +++ b/library_source/drivers/StepperDriver4PWM.cpp @@ -0,0 +1,81 @@ +#include "StepperDriver4PWM.h" + +StepperDriver4PWM::StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B,int en1, int en2){ + // Pin initialization + pwm1A = ph1A; + pwm1B = ph1B; + pwm2A = ph2A; + pwm2B = ph2B; + + // enable_pin pin + enable_pin1 = en1; + enable_pin2 = en2; + + // default power-supply value + voltage_power_supply = DEF_POWER_SUPPLY; + voltage_limit = NOT_SET; + +} + +// enable motor driver +void StepperDriver4PWM::enable(){ + // enable_pin the driver - if enable_pin pin available + if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, HIGH); + if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, HIGH); + // set zero to PWM + setPwm(0,0); +} + +// disable motor driver +void StepperDriver4PWM::disable() +{ + // set zero to PWM + setPwm(0, 0); + // disable the driver - if enable_pin pin available + if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, LOW); + if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, LOW); + +} + +// init hardware pins +int StepperDriver4PWM::init() { + // a bit of separation + _delay(1000); + + // PWM pins + pinMode(pwm1A, OUTPUT); + pinMode(pwm1B, OUTPUT); + pinMode(pwm2A, OUTPUT); + pinMode(pwm2B, OUTPUT); + if(enable_pin1 != NOT_SET) pinMode(enable_pin1, OUTPUT); + if(enable_pin2 != NOT_SET) pinMode(enable_pin2, OUTPUT); + + // sanity check for the voltage limit configuration + if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + + // Set the pwm frequency to the pins + // hardware specific function - depending on driver and mcu + _configure4PWM(pwm_frequency, pwm1A, pwm1B, pwm2A, pwm2B); + return 0; +} + + +// Set voltage to the pwm pin +void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { + float duty_cycle1A(0.0),duty_cycle1B(0.0),duty_cycle2A(0.0),duty_cycle2B(0.0); + // limit the voltage in driver + Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit); + Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit); + // hardware specific writing + if( Ualpha > 0 ) + duty_cycle1B = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); + else + duty_cycle1A = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); + + if( Ubeta > 0 ) + duty_cycle2B = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0); + else + duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0); + // write to hardware + _writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, pwm1A, pwm1B, pwm2A, pwm2B); +} \ No newline at end of file diff --git a/library_source/drivers/StepperDriver4PWM.h b/library_source/drivers/StepperDriver4PWM.h new file mode 100644 index 00000000..e4b2ee42 --- /dev/null +++ b/library_source/drivers/StepperDriver4PWM.h @@ -0,0 +1,55 @@ +#ifndef STEPPER_DRIVER_4PWM_h +#define STEPPER_DRIVER_4PWM_h + +#include "../common/base_classes/StepperDriver.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "hardware_api.h" + +/** + 4 pwm stepper driver class +*/ +class StepperDriver4PWM: public StepperDriver +{ + public: + /** + StepperMotor class constructor + @param ph1A 1A phase pwm pin + @param ph1B 1B phase pwm pin + @param ph2A 2A phase pwm pin + @param ph2B 2B phase pwm pin + @param en1 enable pin phase 1 (optional input) + @param en2 enable pin phase 2 (optional input) + */ + StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B, int en1 = NOT_SET, int en2 = NOT_SET); + + /** Motor hardware init function */ + int init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + // hardware variables + int pwm1A; //!< phase 1A pwm pin number + int pwm1B; //!< phase 1B pwm pin number + int pwm2A; //!< phase 2A pwm pin number + int pwm2B; //!< phase 2B pwm pin number + int enable_pin1; //!< enable pin number phase 1 + int enable_pin2; //!< enable pin number phase 2 + + /** + * Set phase voltages to the harware + * + * @param Ua phase A voltage + * @param Ub phase B voltage + */ + void setPwm(float Ua, float Ub) override; + + private: + +}; + + +#endif diff --git a/library_source/drivers/hardware_api.h b/library_source/drivers/hardware_api.h new file mode 100644 index 00000000..01c03867 --- /dev/null +++ b/library_source/drivers/hardware_api.h @@ -0,0 +1,123 @@ +#ifndef HARDWARE_UTILS_H +#define HARDWARE_UTILS_H + +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA bldc driver + * @param pinB pinB bldc driver + */ +void _configure2PWM(long pwm_frequency, const int pinA, const int pinB); + +/** + * Configuring PWM frequency, resolution and alignment + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA bldc driver + * @param pinB pinB bldc driver + * @param pinC pinC bldc driver + */ +void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC); + +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 4PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pin1A pin1A stepper driver + * @param pin1B pin1B stepper driver + * @param pin2A pin2A stepper driver + * @param pin2B pin2B stepper driver + */ +void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B); + +/** + * Configuring PWM frequency, resolution and alignment + * - BLDC driver - 6PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low - if applicable + * @param pinA_h pinA high-side bldc driver + * @param pinA_l pinA low-side bldc driver + * @param pinB_h pinA high-side bldc driver + * @param pinB_l pinA low-side bldc driver + * @param pinC_h pinA high-side bldc driver + * @param pinC_l pinA low-side bldc driver + * + * @return 0 if config good, -1 if failed + */ +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param pinA phase A hardware pin number + * @param pinB phase B hardware pin number + */ +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param pinA phase A hardware pin number + * @param pinB phase B hardware pin number + * @param pinC phase C hardware pin number + */ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 4PWM setting + * - hardware specific + * + * @param dc_1a duty cycle phase 1A [0, 1] + * @param dc_1b duty cycle phase 1B [0, 1] + * @param dc_2a duty cycle phase 2A [0, 1] + * @param dc_2b duty cycle phase 2B [0, 1] + * @param pin1A phase 1A hardware pin number + * @param pin1B phase 1B hardware pin number + * @param pin2A phase 2A hardware pin number + * @param pin2B phase 2B hardware pin number + */ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B); + + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 6PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low + * @param pinA_h phase A high-side hardware pin number + * @param pinA_l phase A low-side hardware pin number + * @param pinB_h phase B high-side hardware pin number + * @param pinB_l phase B low-side hardware pin number + * @param pinC_h phase C high-side hardware pin number + * @param pinC_l phase C low-side hardware pin number + * + */ +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l); + +#endif \ No newline at end of file diff --git a/library_source/drivers/hardware_specific/atmega2560_mcu.cpp b/library_source/drivers/hardware_specific/atmega2560_mcu.cpp new file mode 100644 index 00000000..5236fb03 --- /dev/null +++ b/library_source/drivers/hardware_specific/atmega2560_mcu.cpp @@ -0,0 +1,158 @@ +#include "../hardware_api.h" + +#if defined(__AVR_ATmega2560__) + +// set pwm frequency to 32KHz +void _pinHighFrequency(const int pin){ + // High PWM frequency + // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328 + // https://forum.arduino.cc/index.php?topic=72092.0 + if (pin == 13 || pin == 4 ) { + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 + } + else if (pin == 12 || pin == 11 ) + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 + else if (pin == 10 || pin == 9 ) + TCCR2B = ((TCCR2B & 0b11111000) | 0x01); // set prescaler to 1 + else if (pin == 5 || pin == 3 || pin == 2) + TCCR3B = ((TCCR2B & 0b11111000) | 0x01); // set prescaler to 1 + else if (pin == 8 || pin == 7 || pin == 6) + TCCR4B = ((TCCR4B & 0b11111000) | 0x01); // set prescaler to 1 + else if (pin == 44 || pin == 45 || pin == 46) + TCCR5B = ((TCCR5B & 0b11111000) | 0x01); // set prescaler to 1 + +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); +} + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); + _pinHighFrequency(pinC); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); + analogWrite(pinC, 255.0*dc_c); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pin1A); + _pinHighFrequency(pin1B); + _pinHighFrequency(pin2A); + _pinHighFrequency(pin2B); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0*dc_1a); + analogWrite(pin1B, 255.0*dc_1b); + analogWrite(pin2A, 255.0*dc_2a); + analogWrite(pin2B, 255.0*dc_2b); +} + + +// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm +// supports Arudino/ATmega2560 +int _configureComplementaryPair(int pinH, int pinL) { + if( (pinH == 4 && pinL == 13 ) || (pinH == 13 && pinL == 4 ) ){ + // configure the pwm phase-corrected mode + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); + // configure complementary pwm on low side + if(pinH == 13 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ; + else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ; + // set prescaler to 1 - 32kHz freq + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); + }else if( (pinH == 11 && pinL == 12 ) || (pinH == 12 && pinL == 11 ) ){ + // set prescaler to 1 - 32kHz freq + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); + // configure complementary pwm on low side + if(pinH == 11 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ; + else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ; + }else if((pinH == 10 && pinL == 9 ) || (pinH == 9 && pinL == 10 ) ){ + // set prescaler to 1 - 32kHz freq + TCCR2B = ((TCCR2B & 0b11111000) | 0x01); + // configure complementary pwm on low side + if(pinH == 10 ) TCCR2A = 0b10110000 | (TCCR2A & 0b00001111) ; + else TCCR2A = 0b11100000 | (TCCR2A & 0b00001111) ; + }else{ + return -1; + } + return 0; +} + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arudino/ATmega328 +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + // High PWM frequency + // - always max 32kHz + int ret_flag = 0; + ret_flag += _configureComplementaryPair(pinA_h, pinA_l); + ret_flag += _configureComplementaryPair(pinB_h, pinB_l); + ret_flag += _configureComplementaryPair(pinC_h, pinC_l); + return ret_flag; // returns -1 if not well configured +} + +// function setting the +void _setPwmPair(int pinH, int pinL, float val, int dead_time) +{ + int pwm_h = _constrain(val-dead_time/2,0,255); + int pwm_l = _constrain(val+dead_time/2,0,255); + + analogWrite(pinH, pwm_h); + if(pwm_l == 255 || pwm_l == 0) + digitalWrite(pinL, pwm_l ? LOW : HIGH); + else + analogWrite(pinL, pwm_l); +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arudino/ATmega328 +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); + _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); + _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0); +} + +#endif \ No newline at end of file diff --git a/library_source/drivers/hardware_specific/atmega328_mcu.cpp b/library_source/drivers/hardware_specific/atmega328_mcu.cpp new file mode 100644 index 00000000..24fe563e --- /dev/null +++ b/library_source/drivers/hardware_specific/atmega328_mcu.cpp @@ -0,0 +1,153 @@ +#include "../hardware_api.h" + +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) + +// set pwm frequency to 32KHz +void _pinHighFrequency(const int pin){ + // High PWM frequency + // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328 + if (pin == 5 || pin == 6 ) { + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 + } + if (pin == 9 || pin == 10 ) + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 + if (pin == 3 || pin == 11) + TCCR2B = ((TCCR2B & 0b11111000) | 0x01);// set prescaler to 1 + +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); +} + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); + _pinHighFrequency(pinC); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); + analogWrite(pinC, 255.0*dc_c); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pin1A); + _pinHighFrequency(pin1B); + _pinHighFrequency(pin2A); + _pinHighFrequency(pin2B); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0*dc_1a); + analogWrite(pin1B, 255.0*dc_1b); + analogWrite(pin2A, 255.0*dc_2a); + analogWrite(pin2B, 255.0*dc_2b); +} + + +// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm +int _configureComplementaryPair(int pinH, int pinL) { + if( (pinH == 5 && pinL == 6 ) || (pinH == 6 && pinL == 5 ) ){ + // configure the pwm phase-corrected mode + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); + // configure complementary pwm on low side + if(pinH == 6 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ; + else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ; + // set prescaler to 1 - 32kHz freq + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); + }else if( (pinH == 9 && pinL == 10 ) || (pinH == 10 && pinL == 9 ) ){ + // set prescaler to 1 - 32kHz freq + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); + // configure complementary pwm on low side + if(pinH == 9 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ; + else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ; + }else if((pinH == 3 && pinL == 11 ) || (pinH == 11 && pinL == 3 ) ){ + // set prescaler to 1 - 32kHz freq + TCCR2B = ((TCCR2B & 0b11111000) | 0x01); + // configure complementary pwm on low side + if(pinH == 11 ) TCCR2A = 0b10110000 | (TCCR2A & 0b00001111) ; + else TCCR2A = 0b11100000 | (TCCR2A & 0b00001111) ; + }else{ + return -1; + } + return 0; +} + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arudino/ATmega328 +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + // High PWM frequency + // - always max 32kHz + int ret_flag = 0; + ret_flag += _configureComplementaryPair(pinA_h, pinA_l); + ret_flag += _configureComplementaryPair(pinB_h, pinB_l); + ret_flag += _configureComplementaryPair(pinC_h, pinC_l); + return ret_flag; // returns -1 if not well configured +} + +// function setting the +void _setPwmPair(int pinH, int pinL, float val, int dead_time) +{ + int pwm_h = _constrain(val-dead_time/2,0,255); + int pwm_l = _constrain(val+dead_time/2,0,255); + + analogWrite(pinH, pwm_h); + if(pwm_l == 255 || pwm_l == 0) + digitalWrite(pinL, pwm_l ? LOW : HIGH); + else + analogWrite(pinL, pwm_l); +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arudino/ATmega328 +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); + _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); + _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0); +} + +#endif \ No newline at end of file diff --git a/library_source/drivers/hardware_specific/esp32_mcu.cpp b/library_source/drivers/hardware_specific/esp32_mcu.cpp new file mode 100644 index 00000000..983d4d54 --- /dev/null +++ b/library_source/drivers/hardware_specific/esp32_mcu.cpp @@ -0,0 +1,431 @@ +#include "../hardware_api.h" + +#if defined(ESP_H) + +#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 160e6 + +// preferred pwm resolution default +#define _PWM_RES_DEF 2048 +// min resolution +#define _PWM_RES_MIN 1500 +// max resolution +#define _PWM_RES_MAX 3000 + +// structure containing motor slot configuration +// this library supports up to 4 motors +typedef struct { + int pinA; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator; + mcpwm_io_signals_t mcpwm_a; + mcpwm_io_signals_t mcpwm_b; + mcpwm_io_signals_t mcpwm_c; +} bldc_3pwm_motor_slots_t; +typedef struct { + int pin1A; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator1; + mcpwm_operator_t mcpwm_operator2; + mcpwm_io_signals_t mcpwm_1a; + mcpwm_io_signals_t mcpwm_1b; + mcpwm_io_signals_t mcpwm_2a; + mcpwm_io_signals_t mcpwm_2b; +} stepper_4pwm_motor_slots_t; +typedef struct { + int pin1pwm; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator; + mcpwm_io_signals_t mcpwm_a; + mcpwm_io_signals_t mcpwm_b; +} stepper_2pwm_motor_slots_t; + +typedef struct { + int pinAH; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator1; + mcpwm_operator_t mcpwm_operator2; + mcpwm_io_signals_t mcpwm_ah; + mcpwm_io_signals_t mcpwm_bh; + mcpwm_io_signals_t mcpwm_ch; + mcpwm_io_signals_t mcpwm_al; + mcpwm_io_signals_t mcpwm_bl; + mcpwm_io_signals_t mcpwm_cl; +} bldc_6pwm_motor_slots_t; + +// define bldc motor slots array +bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}, // 2nd motor will be MCPWM0 channel B + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 3rd motor will be MCPWM1 channel A + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B} // 4th motor will be MCPWM1 channel B + }; + +// define stepper motor slots array +bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 + }; + +// define 4pwm stepper motor slots array +stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[2] = { + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0 + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1 + }; + +// define 2pwm stepper motor slots array +stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = { + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 1st motor will be MCPWM0 channel A + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B}, // 2nd motor will be MCPWM0 channel B + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 3rd motor will be MCPWM1 channel A + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B} // 4th motor will be MCPWM1 channel B + }; + +// configuring high frequency pwm timer +// a lot of help from this post from Paul Gauld +// https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller +void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit, float dead_zone = NOT_SET){ + + mcpwm_config_t pwm_config; + pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave) + pwm_config.duty_mode = MCPWM_DUTY_MODE_0; // Active HIGH + mcpwm_init(mcpwm_unit, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings + mcpwm_init(mcpwm_unit, MCPWM_TIMER_1, &pwm_config); //Configure PWM0A & PWM0B with above settings + mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &pwm_config); //Configure PWM0A & PWM0B with above settings + + if (dead_zone != NOT_SET){ + // dead zone is configured + float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + } + _delay(100); + + mcpwm_stop(mcpwm_unit, MCPWM_TIMER_0); + mcpwm_stop(mcpwm_unit, MCPWM_TIMER_1); + mcpwm_stop(mcpwm_unit, MCPWM_TIMER_2); + + // manual configuration due to the lack of config flexibility in mcpwm_init() + mcpwm_num->clk_cfg.prescale = 0; + // calculate prescaler and period + // step 1: calculate the prescaler using the default pwm resolution + // prescaler = bus_freq / (pwm_freq * default_pwm_res) - 1 + int prescaler = ceil((double)_MCPWM_FREQ / (double)_PWM_RES_DEF / 2.0 / (double)pwm_frequency) - 1; + // constrain prescaler + prescaler = _constrain(prescaler, 0, 128); + // now calculate the real resolution timer period necessary (pwm resolution) + // pwm_res = bus_freq / (pwm_freq * (prescaler + 1)) + int resolution_corrected = (double)_MCPWM_FREQ / 2.0 / (double)pwm_frequency / (double)(prescaler + 1); + // if pwm resolution too low lower the prescaler + if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 ) + resolution_corrected = (double)_MCPWM_FREQ / 2.0 / (double)pwm_frequency / (double)(--prescaler + 1); + resolution_corrected = _constrain(resolution_corrected, _PWM_RES_MIN, _PWM_RES_MAX); + + // set prescaler + mcpwm_num->timer[0].period.prescale = prescaler; + mcpwm_num->timer[1].period.prescale = prescaler; + mcpwm_num->timer[2].period.prescale = prescaler; + _delay(1); + //set period + mcpwm_num->timer[0].period.period = resolution_corrected; + mcpwm_num->timer[1].period.period = resolution_corrected; + mcpwm_num->timer[2].period.period = resolution_corrected; + _delay(1); + mcpwm_num->timer[0].period.upmethod = 0; + mcpwm_num->timer[1].period.upmethod = 0; + mcpwm_num->timer[2].period.upmethod = 0; + _delay(1); + //restart the timers + mcpwm_start(mcpwm_unit, MCPWM_TIMER_0); + mcpwm_start(mcpwm_unit, MCPWM_TIMER_1); + mcpwm_start(mcpwm_unit, MCPWM_TIMER_2); + _delay(1); + + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SELECT_SYNC_INT0, 0); + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_SELECT_SYNC_INT0, 0); + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_SELECT_SYNC_INT0, 0); + _delay(1); + mcpwm_num->timer[0].sync.out_sel = 1; + _delay(1); + mcpwm_num->timer[0].sync.out_sel = 0; +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +// supports Arudino/ATmega328, STM32 and ESP32 +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency + else pwm_frequency = _constrain(pwm_frequency, 0, 40000); // constrain to 40kHz max - centered pwm has twice lower frequency + + stepper_2pwm_motor_slots_t m_slot = {}; + + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 4; slot_num++){ + if(esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = pinA; + m_slot = esp32_stepper_2pwm_motor_slots[slot_num]; + break; + } + } + + // disable all the slots with the same MCPWM + // disable 3pwm bldc motor which would go in the same slot + esp32_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT; + if( slot_num < 2 ){ + // slot 0 of the 4pwm stepper + esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + }else{ + // slot 1 of the stepper + esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + } + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + +} + + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// supports Arudino/ATmega328, STM32 and ESP32 +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency + else pwm_frequency = _constrain(pwm_frequency, 0, 40000); // constrain to 40kHz max - centered pwm has twice lower frequency + + bldc_3pwm_motor_slots_t m_slot = {}; + + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 4; slot_num++){ + if(esp32_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_bldc_3pwm_motor_slots[slot_num].pinA = pinA; + m_slot = esp32_bldc_3pwm_motor_slots[slot_num]; + break; + } + } + // disable all the slots with the same MCPWM + // disable 2pwm steppr motor which would go in the same slot + esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = _TAKEN_SLOT; + if( slot_num < 2 ){ + // slot 0 of the 4pwm stepper + esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + }else{ + // slot 1 of the stepper + esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + } + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_c, pinC); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency + else pwm_frequency = _constrain(pwm_frequency, 0, 40000); // constrain to 50kHz max - centered pwm has twice lower frequency + stepper_4pwm_motor_slots_t m_slot = {}; + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 2; slot_num++){ + if(esp32_stepper_4pwm_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_stepper_4pwm_motor_slots[slot_num].pin1A = pinA; + m_slot = esp32_stepper_4pwm_motor_slots[slot_num]; + break; + } + } + // disable all the slots with the same MCPWM + if( slot_num == 0 ){ + // slots 0 and 1 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; + // slots 0 and 1 of the 2pwm stepper taken + esp32_stepper_2pwm_motor_slots[0].pin1pwm = _TAKEN_SLOT; + esp32_stepper_2pwm_motor_slots[1].pin1pwm = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + }else{ + // slots 2 and 3 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; + // slots 2 and 3 of the 2pwm stepper taken + esp32_stepper_2pwm_motor_slots[2].pin1pwm = _TAKEN_SLOT; + esp32_stepper_2pwm_motor_slots[3].pin1pwm = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + } + + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1a, pinA); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1b, pinB); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2a, pinC); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2b, pinD); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +// ESP32 uses MCPWM +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // determine which motor slot is the motor connected to + for(int i = 0; i < 4; i++){ + if(esp32_stepper_2pwm_motor_slots[i].pin1pwm == pinA){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100] + mcpwm_set_duty(esp32_stepper_2pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_2pwm_motor_slots[i].mcpwm_operator, dc_a*100.0); + mcpwm_set_duty(esp32_stepper_2pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_2pwm_motor_slots[i].mcpwm_operator, dc_b*100.0); + break; + } + } +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +// ESP32 uses MCPWM +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // determine which motor slot is the motor connected to + for(int i = 0; i < 4; i++){ + if(esp32_bldc_3pwm_motor_slots[i].pinA == pinA){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100] + mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_a*100.0); + mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_b*100.0); + mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_c*100.0); + break; + } + } +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +// ESP32 uses MCPWM +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // determine which motor slot is the motor connected to + for(int i = 0; i < 2; i++){ + if(esp32_stepper_4pwm_motor_slots[i].pin1A == pin1A){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100] + mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator1, dc_1a*100.0); + mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator1, dc_1b*100.0); + mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator2, dc_2a*100.0); + mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator2, dc_2b*100.0); + break; + } + } +} + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency + else pwm_frequency = _constrain(pwm_frequency, 0, 40000); // constrain to 40kHz max - centered pwm has twice lower frequency + bldc_6pwm_motor_slots_t m_slot = {}; + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 2; slot_num++){ + if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h; + m_slot = esp32_bldc_6pwm_motor_slots[slot_num]; + break; + } + } + // if no slots available + if(slot_num >= 2) return -1; + + // disable all the slots with the same MCPWM + if( slot_num == 0 ){ + // slots 0 and 1 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; + }else{ + // slots 2 and 3 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; + } + + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_al, pinA_l); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bh, pinB_h); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bl, pinB_l); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ch, pinC_h); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_cl, pinC_l); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit, dead_zone); + // return + return 0; +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + // determine which motor slot is the motor connected to + for(int i = 0; i < 2; i++){ + if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100.0] + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0); + break; + } + } +} +#endif \ No newline at end of file diff --git a/library_source/drivers/hardware_specific/generic_mcu.cpp b/library_source/drivers/hardware_specific/generic_mcu.cpp new file mode 100644 index 00000000..b3641114 --- /dev/null +++ b/library_source/drivers/hardware_specific/generic_mcu.cpp @@ -0,0 +1,87 @@ +#include "../hardware_api.h" + +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if mcu is not atmega328 + +#elif defined(__AVR_ATmega2560__) // if mcu is not atmega2560 + +#elif defined(__arm__) && defined(CORE_TEENSY) // or teensy + +#elif defined(ESP_H) // or esp32 + +#elif defined(_STM32_DEF_) // or stm32 + +#else + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +// in generic case dont do anything +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + return; +} + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// in generic case dont do anything +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + return; +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +// in generic case dont do anything +void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + return; +} + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + return -1; +} + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); + analogWrite(pinC, 255.0*dc_c); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0*dc_1a); + analogWrite(pin1B, 255.0*dc_1b); + analogWrite(pin2A, 255.0*dc_2a); + analogWrite(pin2B, 255.0*dc_2b); +} + + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + return; +} + + +#endif \ No newline at end of file diff --git a/library_source/drivers/hardware_specific/stm32_mcu.cpp b/library_source/drivers/hardware_specific/stm32_mcu.cpp new file mode 100644 index 00000000..0dc83cf7 --- /dev/null +++ b/library_source/drivers/hardware_specific/stm32_mcu.cpp @@ -0,0 +1,326 @@ + +#include "../hardware_api.h" + +#if defined(_STM32_DEF_) + +#define _PWM_RESOLUTION 12 // 12bit +#define _PWM_RANGE 4095.0// 2^12 -1 = 4095 +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 50000 // 50khz + + +#define _HARDWARE_6PWM 1 +#define _SOFTWARE_6PWM 0 +#define _ERROR_6PWM -1 + + +// setting pwm to hardware pin - instead analogWrite() +void _setPwm(int ulPin, uint32_t value, int resolution) +{ + PinName pin = digitalPinToPinName(ulPin); + TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); + uint32_t index = get_timer_index(Instance); + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + + uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + HT->setCaptureCompare(channel, value, (TimerCompareFormat_t)resolution); +} + + +// init pin pwm +HardwareTimer* _initPinPWM(uint32_t PWM_freq, int ulPin) +{ + PinName pin = digitalPinToPinName(ulPin); + TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); + + uint32_t index = get_timer_index(Instance); + if (HardwareTimer_Handle[index] == NULL) { + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); + HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); + } + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, pin); + HT->setOverflow(PWM_freq, HERTZ_FORMAT); + HT->pause(); + HT->refresh(); + return HT; +} + + +// init high side pin +HardwareTimer* _initPinPWMHigh(uint32_t PWM_freq, int ulPin) +{ + return _initPinPWM(PWM_freq, ulPin); +} + +// init low side pin +HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) +{ + PinName pin = digitalPinToPinName(ulPin); + TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); + uint32_t index = get_timer_index(Instance); + + if (HardwareTimer_Handle[index] == NULL) { + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); + HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); + TIM_OC_InitTypeDef sConfigOC = TIM_OC_InitTypeDef(); + sConfigOC.OCMode = TIM_OCMODE_PWM2; + sConfigOC.Pulse = 100; + sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; + sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; + sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; + sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET; + sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; + uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel); + } + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, pin); + HT->setOverflow(PWM_freq, HERTZ_FORMAT); + HT->pause(); + HT->refresh(); + return HT; +} + + +// align the timers to end the init +void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3) +{ + HT1->pause(); + HT1->refresh(); + HT2->pause(); + HT2->refresh(); + HT3->pause(); + HT3->refresh(); + HT1->resume(); + HT2->resume(); + HT3->resume(); +} + +// align the timers to end the init +void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3,HardwareTimer *HT4) +{ + HT1->pause(); + HT1->refresh(); + HT2->pause(); + HT2->refresh(); + HT3->pause(); + HT3->refresh(); + HT4->pause(); + HT4->refresh(); + HT1->resume(); + HT2->resume(); + HT3->resume(); + HT4->resume(); +} + +HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) +{ + PinName uhPinName = digitalPinToPinName(pinA_h); + PinName ulPinName = digitalPinToPinName(pinA_l); + PinName vhPinName = digitalPinToPinName(pinB_h); + PinName vlPinName = digitalPinToPinName(pinB_l); + PinName whPinName = digitalPinToPinName(pinC_h); + PinName wlPinName = digitalPinToPinName(pinC_l); + + TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM); + + uint32_t index = get_timer_index(Instance); + + if (HardwareTimer_Handle[index] == NULL) { + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM)); + HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); + ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT); + } + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + + HT->setMode(STM_PIN_CHANNEL(pinmap_function(uhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, uhPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(ulPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, ulPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(vhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vhPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(vlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vlPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(whPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, whPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, wlPinName); + + // dead time is set in nanoseconds + uint32_t dead_time_ns = (float)(1e9/PWM_freq)*dead_zone; + uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns); + LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! + LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N); + + HT->pause(); + HT->refresh(); + HT->resume(); + return HT; +} + + +// returns 0 if each pair of pwm channels has same channel +// returns 1 all the channels belong to the same timer - hardware inverted channels +// returns -1 if neither - avoid configuring - error!!! +int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + PinName nameAH = digitalPinToPinName(pinA_h); + PinName nameAL = digitalPinToPinName(pinA_l); + PinName nameBH = digitalPinToPinName(pinB_h); + PinName nameBL = digitalPinToPinName(pinB_l); + PinName nameCH = digitalPinToPinName(pinC_h); + PinName nameCL = digitalPinToPinName(pinC_l); + int tim1 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAH, PinMap_PWM)); + int tim2 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAL, PinMap_PWM)); + int tim3 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBH, PinMap_PWM)); + int tim4 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBL, PinMap_PWM)); + int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM)); + int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM)); + if(tim1 == tim2 && tim2==tim3 && tim3==tim4 && tim4==tim5 && tim5==tim6) + return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer + else if(tim1 == tim2 && tim3==tim4 && tim5==tim6) + return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer + else + return _ERROR_6PWM; // might be error avoid configuration +} + + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); + HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); + // allign the timers + _alignPWMTimers(HT1, HT2, HT2); +} + + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); + HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); + HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); + // allign the timers + _alignPWMTimers(HT1, HT2, HT3); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); + HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); + HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); + HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinD); + // allign the timers + _alignPWMTimers(HT1, HT2, HT3, HT4); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +//- hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // transform duty cycle from [0,1] to [0,4095] + _setPwm(pinA, _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(pinB, _PWM_RANGE*dc_b, _PWM_RESOLUTION); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +//- hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,4095] + _setPwm(pinA, _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(pinB, _PWM_RANGE*dc_b, _PWM_RESOLUTION); + _setPwm(pinC, _PWM_RANGE*dc_c, _PWM_RESOLUTION); +} + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +//- hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,4095] + _setPwm(pin1A, _PWM_RANGE*dc_1a, _PWM_RESOLUTION); + _setPwm(pin1B, _PWM_RANGE*dc_1b, _PWM_RESOLUTION); + _setPwm(pin2A, _PWM_RANGE*dc_2a, _PWM_RESOLUTION); + _setPwm(pin2B, _PWM_RANGE*dc_2b, _PWM_RESOLUTION); +} + + + + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to |%0kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + + // find configuration + int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); + // configure accordingly + switch(config){ + case _ERROR_6PWM: + return -1; // fail + break; + case _HARDWARE_6PWM: + _initHardware6PWMInterface(pwm_frequency, dead_zone, pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); + break; + case _SOFTWARE_6PWM: + HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinA_h); + _initPinPWMLow(pwm_frequency, pinA_l); + HardwareTimer* HT2 = _initPinPWMHigh(pwm_frequency,pinB_h); + _initPinPWMLow(pwm_frequency, pinB_l); + HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency,pinC_h); + _initPinPWMLow(pwm_frequency, pinC_l); + _alignPWMTimers(HT1, HT2, HT3); + break; + } + return 0; // success +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + // find configuration + int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); + // set pwm accordingly + switch(config){ + case _HARDWARE_6PWM: + _setPwm(pinA_h, _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(pinB_h, _PWM_RANGE*dc_b, _PWM_RESOLUTION); + _setPwm(pinC_h, _PWM_RANGE*dc_c, _PWM_RESOLUTION); + break; + case _SOFTWARE_6PWM: + _setPwm(pinA_l, _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinA_h, _constrain(dc_a - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinB_l, _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinB_h, _constrain(dc_b - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinC_l, _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinC_h, _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + break; + } +} +#endif \ No newline at end of file diff --git a/library_source/drivers/hardware_specific/teensy_mcu.cpp b/library_source/drivers/hardware_specific/teensy_mcu.cpp new file mode 100644 index 00000000..10bd24c5 --- /dev/null +++ b/library_source/drivers/hardware_specific/teensy_mcu.cpp @@ -0,0 +1,71 @@ +#include "../hardware_api.h" + +#if defined(__arm__) && defined(CORE_TEENSY) + +// configure High PWM frequency +void _setHighFrequency(const long freq, const int pin){ + analogWrite(pin, 0); + analogWriteFrequency(pin, freq); +} + + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); + _setHighFrequency(pwm_frequency, pinC); + if(pinD != NOT_SET) _setHighFrequency(pwm_frequency, pinD); // stepper motor +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); + _setHighFrequency(pwm_frequency, pinC); + _setHighFrequency(pwm_frequency, pinD); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); + analogWrite(pinC, 255.0*dc_c); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0*dc_1a); + analogWrite(pin1B, 255.0*dc_1b); + analogWrite(pin2A, 255.0*dc_2a); + analogWrite(pin2B, 255.0*dc_2b); +} + + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + return -1; +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + return; +} +#endif \ No newline at end of file diff --git a/library_source/sensors/Encoder.cpp b/library_source/sensors/Encoder.cpp new file mode 100644 index 00000000..1965c015 --- /dev/null +++ b/library_source/sensors/Encoder.cpp @@ -0,0 +1,228 @@ +#include "Encoder.h" + + +/* + Encoder(int encA, int encB , int cpr, int index) + - encA, encB - encoder A and B pins + - cpr - counts per rotation number (cpm=ppm*4) + - index pin - (optional input) +*/ + +Encoder::Encoder(int _encA, int _encB , float _ppr, int _index){ + + // Encoder measurement structure init + // hardware pins + pinA = _encA; + pinB = _encB; + // counter setup + pulse_counter = 0; + pulse_timestamp = 0; + + cpr = _ppr; + A_active = 0; + B_active = 0; + I_active = 0; + // index pin + index_pin = _index; // its 0 if not used + index_pulse_counter = 0; + + // velocity calculation variables + prev_Th = 0; + pulse_per_second = 0; + prev_pulse_counter = 0; + prev_timestamp_us = _micros(); + + // extern pullup as default + pullup = Pullup::EXTERN; + // enable quadrature encoder by default + quadrature = Quadrature::ON; +} + +// Encoder interrupt callback functions +// A channel +void Encoder::handleA() { + int A = digitalRead(pinA); + switch (quadrature){ + case Quadrature::ON: + // CPR = 4xPPR + if ( A != A_active ) { + pulse_counter += (A_active == B_active) ? 1 : -1; + pulse_timestamp = _micros(); + A_active = A; + } + break; + case Quadrature::OFF: + // CPR = PPR + if(A && !digitalRead(pinB)){ + pulse_counter++; + pulse_timestamp = _micros(); + } + break; + } +} +// B channel +void Encoder::handleB() { + int B = digitalRead(pinB); + switch (quadrature){ + case Quadrature::ON: + // // CPR = 4xPPR + if ( B != B_active ) { + pulse_counter += (A_active != B_active) ? 1 : -1; + pulse_timestamp = _micros(); + B_active = B; + } + break; + case Quadrature::OFF: + // CPR = PPR + if(B && !digitalRead(pinA)){ + pulse_counter--; + pulse_timestamp = _micros(); + } + break; + } +} + +// Index channel +void Encoder::handleIndex() { + if(hasIndex()){ + int I = digitalRead(index_pin); + if(I && !I_active){ + // align encoder on each index + if(index_pulse_counter){ + long tmp = pulse_counter; + // corrent the counter value + pulse_counter = round((double)pulse_counter/(double)cpr)*cpr; + // preserve relative speed + prev_pulse_counter += pulse_counter - tmp; + } else { + // initial offset + index_pulse_counter = pulse_counter; + } + } + I_active = I; + } +} + +/* + Shaft angle calculation +*/ +float Encoder::getAngle(){ + return natural_direction * _2PI * (pulse_counter) / ((float)cpr); +} +/* + Shaft velocity calculation + function using mixed time and frequency measurement technique +*/ +float Encoder::getVelocity(){ + // timestamp + long timestamp_us = _micros(); + // sampling time calculation + float Ts = (timestamp_us - prev_timestamp_us) * 1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + + // time from last impulse + float Th = (timestamp_us - pulse_timestamp) * 1e-6; + long dN = pulse_counter - prev_pulse_counter; + + // Pulse per second calculation (Eq.3.) + // dN - impulses received + // Ts - sampling time - time in between function calls + // Th - time from last impulse + // Th_1 - time form last impulse of the previous call + // only increment if some impulses received + float dt = Ts + prev_Th - Th; + pulse_per_second = (dN != 0 && dt > Ts/2) ? dN / dt : pulse_per_second; + + // if more than 0.05 passed in between impulses + if ( Th > 0.1) pulse_per_second = 0; + + // velocity calculation + float velocity = pulse_per_second / ((float)cpr) * (_2PI); + + // save variables for next pass + prev_timestamp_us = timestamp_us; + // save velocity calculation variables + prev_Th = Th; + prev_pulse_counter = pulse_counter; + return natural_direction * (velocity); +} + +// getter for index pin +// return -1 if no index +int Encoder::needsAbsoluteZeroSearch(){ + return index_pulse_counter == 0; +} +// getter for index pin +int Encoder::hasAbsoluteZero(){ + return hasIndex(); +} +// initialize counter to zero +float Encoder::initRelativeZero(){ + long angle_offset = -pulse_counter; + pulse_counter = 0; + pulse_timestamp = _micros(); + return _2PI * (angle_offset) / ((float)cpr); +} +// initialize index to zero +float Encoder::initAbsoluteZero(){ + pulse_counter -= index_pulse_counter; + prev_pulse_counter = pulse_counter; + return (index_pulse_counter) / ((float)cpr) * (_2PI); +} +// private function used to determine if encoder has index +int Encoder::hasIndex(){ + return index_pin != 0; +} + + +// encoder initialisation of the hardware pins +// and calculation variables +void Encoder::init(){ + + // Encoder - check if pullup needed for your encoder + if(pullup == Pullup::INTERN){ + pinMode(pinA, INPUT_PULLUP); + pinMode(pinB, INPUT_PULLUP); + if(hasIndex()) pinMode(index_pin,INPUT_PULLUP); + }else{ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if(hasIndex()) pinMode(index_pin,INPUT); + } + + // counter setup + pulse_counter = 0; + pulse_timestamp = _micros(); + // velocity calculation variables + prev_Th = 0; + pulse_per_second = 0; + prev_pulse_counter = 0; + prev_timestamp_us = _micros(); + + // initial cpr = PPR + // change it if the mode is quadrature + if(quadrature == Quadrature::ON) cpr = 4*cpr; + +} + +// function enabling hardware interrupts of the for the callback provided +// if callback is not provided then the interrupt is not enabled +void Encoder::enableInterrupts(void (*doA)(), void(*doB)(), void(*doIndex)()){ + // attach interrupt if functions provided + switch(quadrature){ + case Quadrature::ON: + // A callback and B callback + if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE); + if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE); + break; + case Quadrature::OFF: + // A callback and B callback + if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, RISING); + if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, RISING); + break; + } + + // if index used initialize the index interrupt + if(hasIndex() && doIndex != nullptr) attachInterrupt(digitalPinToInterrupt(index_pin), doIndex, CHANGE); +} diff --git a/library_source/sensors/Encoder.h b/library_source/sensors/Encoder.h new file mode 100644 index 00000000..29122888 --- /dev/null +++ b/library_source/sensors/Encoder.h @@ -0,0 +1,105 @@ +#ifndef ENCODER_LIB_H +#define ENCODER_LIB_H + +#include "Arduino.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/base_classes/Sensor.h" + + +/** + * Quadrature mode configuration structure + */ +enum Quadrature{ + ON, //!< Enable quadrature mode CPR = 4xPPR + OFF //!< Disable quadrature mode / CPR = PPR +}; + +class Encoder: public Sensor{ + public: + /** + Encoder class constructor + @param encA encoder B pin + @param encB encoder B pin + @param ppr impulses per rotation (cpr=ppr*4) + @param index index pin number (optional input) + */ + Encoder(int encA, int encB , float ppr, int index = 0); + + /** encoder initialise pins */ + void init(); + /** + * function enabling hardware interrupts for the encoder channels with provided callback functions + * if callback is not provided then the interrupt is not enabled + * + * @param doA pointer to the A channel interrupt handler function + * @param doB pointer to the B channel interrupt handler function + * @param doIndex pointer to the Index channel interrupt handler function + * + */ + void enableInterrupts(void (*doA)() = nullptr, void(*doB)() = nullptr, void(*doIndex)() = nullptr); + + // Encoder interrupt callback functions + /** A channel callback function */ + void handleA(); + /** B channel callback function */ + void handleB(); + /** Index channel callback function */ + void handleIndex(); + + + // pins A and B + int pinA; //!< encoder hardware pin A + int pinB; //!< encoder hardware pin B + int index_pin; //!< index pin + + // Encoder configuration + Pullup pullup; //!< Configuration parameter internal or external pullups + Quadrature quadrature;//!< Configuration parameter enable or disable quadrature mode + float cpr;//!< encoder cpr number + + // Abstract functions of the Sensor class implementation + /** get current angle (rad) */ + float getAngle() override; + /** get current angular velocity (rad/s) */ + float getVelocity() override; + /** + * set current angle as zero angle + * return the angle [rad] difference + */ + float initRelativeZero() override; + /** + * set index angle as zero angle + * return the angle [rad] difference + */ + float initAbsoluteZero() override; + /** + * returns 0 if it has no index + * 0 - encoder without index + * 1 - encoder with index + */ + int hasAbsoluteZero() override; + /** + * returns 0 if it does need search for absolute zero + * 0 - encoder without index + * 1 - ecoder with index + */ + int needsAbsoluteZeroSearch() override; + + private: + int hasIndex(); //!< function returning 1 if encoder has index pin and 0 if not. + + volatile long pulse_counter;//!< current pulse counter + volatile long pulse_timestamp;//!< last impulse timestamp in us + volatile int A_active; //!< current active states of A channel + volatile int B_active; //!< current active states of B channel + volatile int I_active; //!< current active states of Index channel + volatile long index_pulse_counter; //!< impulse counter number upon first index interrupt + + // velocity calculation variables + float prev_Th, pulse_per_second; + volatile long prev_pulse_counter, prev_timestamp_us; +}; + + +#endif diff --git a/library_source/sensors/HallSensor.cpp b/library_source/sensors/HallSensor.cpp new file mode 100644 index 00000000..2c33a41e --- /dev/null +++ b/library_source/sensors/HallSensor.cpp @@ -0,0 +1,168 @@ +#include "HallSensor.h" + + +/* + HallSensor(int hallA, int hallB , int cpr, int index) + - hallA, hallB, hallC - HallSensor A, B and C pins + - pp - pole pairs +*/ +HallSensor::HallSensor(int _hallA, int _hallB, int _hallC, int _pp){ + + // hardware pins + pinA = _hallA; + pinB = _hallB; + pinC = _hallC; + + // hall has 6 segments per electrical revolution + cpr = _pp * 6; + + // extern pullup as default + pullup = Pullup::EXTERN; +} + +// HallSensor interrupt callback functions +// A channel +void HallSensor::handleA() { + A_active= digitalRead(pinA); + updateState(); +} +// B channel +void HallSensor::handleB() { + B_active = digitalRead(pinB); + updateState(); +} + +// C channel +void HallSensor::handleC() { + C_active = digitalRead(pinC); + updateState(); +} + +/** + * Updates the state and sector following an interrupt + */ +void HallSensor::updateState() { + long new_pulse_timestamp = _micros(); + hall_state = C_active + (B_active << 1) + (A_active << 2); + int8_t new_electric_sector = ELECTRIC_SECTORS[hall_state]; + static Direction old_direction; + if (new_electric_sector - electric_sector > 3) { + //underflow + direction = static_cast(natural_direction * -1); + electric_rotations += direction; + } else if (new_electric_sector - electric_sector < (-3)) { + //overflow + direction = static_cast(natural_direction); + electric_rotations += direction; + } else { + direction = (new_electric_sector > electric_sector)? static_cast(natural_direction) : static_cast(natural_direction * (-1)); + } + electric_sector = new_electric_sector; + if (direction == old_direction) { + // not oscilating or just changed direction + pulse_diff = new_pulse_timestamp - pulse_timestamp; + } else { + pulse_diff = 0; + } + + pulse_timestamp = new_pulse_timestamp; + total_interrupts++; + old_direction = direction; + if (onSectorChange != nullptr) onSectorChange(electric_sector); +} + +/** + * Optionally set a function callback to be fired when sector changes + * void onSectorChange(int sector) { + * ... // for debug or call driver directly? + * } + * sensor.attachSectorCallback(onSectorChange); + */ +void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { + onSectorChange = _onSectorChange; +} + +/* + Shaft angle calculation +*/ +float HallSensor::getAngle() { + return natural_direction * ((electric_rotations * 6 + electric_sector) / cpr) * _2PI; +} + +/* + Shaft velocity calculation + function using mixed time and frequency measurement technique +*/ +float HallSensor::getVelocity(){ + if (pulse_diff == 0 || ((_micros() - pulse_timestamp) > pulse_diff) ) { // last velocity isn't accurate if too old + return 0; + } else { + return direction * (_2PI / cpr) / (pulse_diff / 1000000.0); + } + +} + +// getter for index pin +// return -1 if no index +int HallSensor::needsAbsoluteZeroSearch(){ + return 0; +} + +int HallSensor::hasAbsoluteZero(){ + return 1; +} + +// set current angle as zero angle +// return the angle [rad] difference +float HallSensor::initRelativeZero(){ + + // nothing to do. The interrupts should have changed sector. + electric_rotations = 0; + return 0; + +} + +// set absolute zero angle as zero angle +// return the angle [rad] difference +float HallSensor::initAbsoluteZero(){ + + return -getAngle(); + +} + +// HallSensor initialisation of the hardware pins +// and calculation variables +void HallSensor::init(){ + + // HallSensor - check if pullup needed for your HallSensor + if(pullup == Pullup::INTERN){ + pinMode(pinA, INPUT_PULLUP); + pinMode(pinB, INPUT_PULLUP); + pinMode(pinC, INPUT_PULLUP); + }else{ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + pinMode(pinC, INPUT); + } + + // init hall_state + A_active= digitalRead(pinA); + B_active = digitalRead(pinB); + C_active = digitalRead(pinC); + updateState(); + + pulse_timestamp = _micros(); + +} + +// function enabling hardware interrupts for the callback provided +// if callback is not provided then the interrupt is not enabled +void HallSensor::enableInterrupts(void (*doA)(), void(*doB)(), void(*doC)()){ + // attach interrupt if functions provided + + // A, B and C callback + if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE); + if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE); + if(doC != nullptr) attachInterrupt(digitalPinToInterrupt(pinC), doC, CHANGE); +} + diff --git a/library_source/sensors/HallSensor.h b/library_source/sensors/HallSensor.h new file mode 100644 index 00000000..ad5673e3 --- /dev/null +++ b/library_source/sensors/HallSensor.h @@ -0,0 +1,116 @@ +#ifndef HALL_SENSOR_LIB_H +#define HALL_SENSOR_LIB_H + +#include "Arduino.h" +#include "../common/base_classes/Sensor.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + +// seq 1 > 5 > 4 > 6 > 2 > 3 > 1 000 001 010 011 100 101 110 111 +const int8_t ELECTRIC_SECTORS[8] = { -1, 0, 4, 5, 2, 1, 3 , -1 }; + +class HallSensor: public Sensor{ + public: + /** + HallSensor class constructor + @param encA HallSensor A pin + @param encB HallSensor B pin + @param encC HallSensor C pin + @param pp pole pairs (e.g hoverboard motor has 15pp and small gimbals often have 7pp) + @param index index pin number (optional input) + */ + HallSensor(int encA, int encB, int encC, int pp); + + /** HallSensor initialise pins */ + void init(); + /** + * function enabling hardware interrupts for the HallSensor channels with provided callback functions + * if callback is not provided then the interrupt is not enabled + * + * @param doA pointer to the A channel interrupt handler function + * @param doB pointer to the B channel interrupt handler function + * @param doIndex pointer to the Index channel interrupt handler function + * + */ + void enableInterrupts(void (*doA)() = nullptr, void(*doB)() = nullptr, void(*doC)() = nullptr); + + // HallSensor interrupt callback functions + /** A channel callback function */ + void handleA(); + /** B channel callback function */ + void handleB(); + /** C channel callback function */ + void handleC(); + + + // pins A and B + int pinA; //!< HallSensor hardware pin A + int pinB; //!< HallSensor hardware pin B + int pinC; //!< HallSensor hardware pin C + + // HallSensor configuration + Pullup pullup; //!< Configuration parameter internal or external pullups + float cpr;//!< HallSensor cpr number + + // Abstract functions of the Sensor class implementation + /** get current angle (rad) */ + float getAngle() override; + /** get current angular velocity (rad/s) */ + float getVelocity() override; + /** + * set current angle as zero angle + * return the angle [rad] difference + */ + float initRelativeZero() override; + /** + * set index angle as zero angle + * return the angle [rad] difference + */ + float initAbsoluteZero() override; + /** + * returns 0 if it has no index + * 0 - HallSensor without index + * 1 - HallSensor with index + */ + int hasAbsoluteZero() override; + /** + * returns 0 if it does need search for absolute zero + * 0 - HallSensor without index + * 1 - ecoder with index + */ + int needsAbsoluteZeroSearch() override; + + // whether last step was CW (+1) or CCW (-1) direction + Direction direction; + + void attachSectorCallback(void (*onSectorChange)(int a) = nullptr); + + // the current 3bit state of the hall sensors + volatile int8_t hall_state; + // the current sector of the sensor. Each sector is 60deg electrical + volatile int8_t electric_sector; + // the number of electric rotations + volatile long electric_rotations; + // this is sometimes useful to identify interrupt issues (e.g. weak or no pullup resulting in 1000s of interrupts) + volatile long total_interrupts; + + private: + + Direction decodeDirection(int oldState, int newState); + void updateState(); + + int zero_offset; + volatile long pulse_timestamp;//!< last impulse timestamp in us + volatile int A_active; //!< current active states of A channel + volatile int B_active; //!< current active states of B channel + volatile int C_active; //!< current active states of C channel + + // function pointer for on sector change call back + void (*onSectorChange)(int sector) = nullptr; + + volatile long pulse_diff; + +}; + + +#endif diff --git a/library_source/sensors/MagneticSensorAnalog.cpp b/library_source/sensors/MagneticSensorAnalog.cpp new file mode 100644 index 00000000..ed112a94 --- /dev/null +++ b/library_source/sensors/MagneticSensorAnalog.cpp @@ -0,0 +1,109 @@ +#include "MagneticSensorAnalog.h" + +/** MagneticSensorAnalog(uint8_t _pinAnalog, int _min, int _max) + * @param _pinAnalog the pin that is reading the pwm from magnetic sensor + * @param _min_raw_count the smallest expected reading. Whilst you might expect it to be 0 it is often ~15. Getting this wrong results in a small click once per revolution + * @param _max_raw_count the largest value read. whilst you might expect it to be 2^10 = 1023 it is often ~ 1020. Note: For ESP32 (with 12bit ADC the value will be nearer 4096) + */ +MagneticSensorAnalog::MagneticSensorAnalog(uint8_t _pinAnalog, int _min_raw_count, int _max_raw_count){ + + pinAnalog = _pinAnalog; + + cpr = _max_raw_count - _min_raw_count; + min_raw_count = _min_raw_count; + max_raw_count = _max_raw_count; + + if(pullup == Pullup::INTERN){ + pinMode(pinAnalog, INPUT_PULLUP); + }else{ + pinMode(pinAnalog, INPUT); + } + +} + + +void MagneticSensorAnalog::init(){ + + // velocity calculation init + angle_prev = 0; + velocity_calc_timestamp = _micros(); + + // full rotations tracking number + full_rotation_offset = 0; + raw_count_prev = getRawCount(); + zero_offset = 0; +} + +// Shaft angle calculation +// angle is in radians [rad] +float MagneticSensorAnalog::getAngle(){ + // raw data from the sensor + raw_count = getRawCount(); + + int delta = raw_count - raw_count_prev; + // if overflow happened track it as full rotation + if(abs(delta) > (0.8*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; + + float angle = natural_direction * (full_rotation_offset + ( (float) (raw_count - zero_offset) / (float)cpr) * _2PI); + + // calculate velocity here + long now = _micros(); + float Ts = ( now - velocity_calc_timestamp)*1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + velocity = (angle - angle_prev)/Ts; + + // save variables for future pass + raw_count_prev = raw_count; + angle_prev = angle; + velocity_calc_timestamp = now; + + return angle; +} + +// Shaft velocity calculation +float MagneticSensorAnalog::getVelocity(){ + // TODO: Refactor?: to avoid angle being called twice, velocity is pre-calculted during getAngle + return velocity; +} + +// set current angle as zero angle +// return the angle [rad] difference +float MagneticSensorAnalog::initRelativeZero(){ + + float angle_offset = -getAngle(); + zero_offset = natural_direction * getRawCount(); + + // angle tracking variables + full_rotation_offset = 0; + return angle_offset; +} +// set absolute zero angle as zero angle +// return the angle [rad] difference +float MagneticSensorAnalog::initAbsoluteZero(){ + float rotation = -(int)zero_offset; + // init absolute zero + zero_offset = 0; + + // angle tracking variables + full_rotation_offset = 0; + // return offset in radians + return rotation / (float)cpr * _2PI; +} +// returns 0 if it has no absolute 0 measurement +// 0 - incremental encoder without index +// 1 - encoder with index & magnetic sensors +int MagneticSensorAnalog::hasAbsoluteZero(){ + return 1; +} +// returns 0 if it does need search for absolute zero +// 0 - magnetic sensor +// 1 - ecoder with index +int MagneticSensorAnalog::needsAbsoluteZeroSearch(){ + return 0; +} + +// function reading the raw counter of the magnetic sensor +int MagneticSensorAnalog::getRawCount(){ + return analogRead(pinAnalog); +} diff --git a/library_source/sensors/MagneticSensorAnalog.h b/library_source/sensors/MagneticSensorAnalog.h new file mode 100644 index 00000000..d76b6eb4 --- /dev/null +++ b/library_source/sensors/MagneticSensorAnalog.h @@ -0,0 +1,81 @@ +#ifndef MAGNETICSENSORANALOG_LIB_H +#define MAGNETICSENSORANALOG_LIB_H + +#include "Arduino.h" +#include "../common/base_classes/Sensor.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + +/** + * This sensor has been tested with AS5600 running in 'analog mode'. This is where output pin of AS6000 is connected to an analog pin on your microcontroller. + * This approach is very simple but you may more accurate results with MagneticSensorI2C if that is also supported as its skips the DAC -> ADC conversion (ADC supports MagneticSensorI2C) + */ +class MagneticSensorAnalog: public Sensor{ + public: + /** + * MagneticSensorAnalog class constructor + * @param _pinAnalog the pin to read the PWM signal + */ + MagneticSensorAnalog(uint8_t _pinAnalog, int _min = 0, int _max = 0); + + + /** sensor initialise pins */ + void init(); + + int pinAnalog; //!< encoder hardware pin A + + // Encoder configuration + Pullup pullup; + + // implementation of abstract functions of the Sensor class + /** get current angle (rad) */ + float getAngle() override; + /** get current angular velocity (rad/s) **/ + float getVelocity() override; + /** + * set current angle as zero angle + * return the angle [rad] difference + */ + float initRelativeZero() override; + /** + * set absolute zero angle as zero angle + * return the angle [rad] difference + */ + float initAbsoluteZero() override; + /** returns 1 because it is the absolute sensor */ + int hasAbsoluteZero() override; + /** returns 0 maning it doesn't need search for absolute zero */ + int needsAbsoluteZeroSearch() override; + /** raw count (typically in range of 0-1023), useful for debugging resolution issues */ + int raw_count; + int min_raw_count; + int max_raw_count; + int cpr; + + private: + // float cpr; //!< Maximum range of the magnetic sensor + + + int read(); + + int zero_offset; //!< user defined zero offset + /** + * Function getting current angle register value + * it uses angle_register variable + */ + int getRawCount(); + + // total angle tracking variables + float full_rotation_offset; //!begin(); + + // velocity calculation init + angle_prev = 0; + velocity_calc_timestamp = _micros(); + + // full rotations tracking number + full_rotation_offset = 0; + angle_data_prev = getRawCount(); + zero_offset = 0; +} + +// Shaft angle calculation +// angle is in radians [rad] +float MagneticSensorI2C::getAngle(){ + // raw data from the sensor + float angle_data = getRawCount(); + + // tracking the number of rotations + // in order to expand angle range form [0,2PI] + // to basically infinity + float d_angle = angle_data - angle_data_prev; + // if overflow happened track it as full rotation + if(abs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; + // save the current angle value for the next steps + // in order to know if overflow happened + angle_data_prev = angle_data; + + // zero offset adding + angle_data -= (int)zero_offset; + // return the full angle + // (number of full rotations)*2PI + current sensor angle + return natural_direction * (full_rotation_offset + ( angle_data / (float)cpr) * _2PI); +} + +// Shaft velocity calculation +float MagneticSensorI2C::getVelocity(){ + // calculate sample time + unsigned long now_us = _micros(); + float Ts = (now_us - velocity_calc_timestamp)*1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + + // current angle + float angle_c = getAngle(); + // velocity calculation + float vel = (angle_c - angle_prev)/Ts; + + // save variables for future pass + angle_prev = angle_c; + velocity_calc_timestamp = now_us; + return vel; +} + +// set current angle as zero angle +// return the angle [rad] difference +float MagneticSensorI2C::initRelativeZero(){ + float angle_offset = -getAngle(); + zero_offset = natural_direction * getRawCount(); + + // angle tracking variables + full_rotation_offset = 0; + return angle_offset; +} +// set absolute zero angle as zero angle +// return the angle [rad] difference +float MagneticSensorI2C::initAbsoluteZero(){ + float rotation = -(int)zero_offset; + // init absolute zero + zero_offset = 0; + + // angle tracking variables + full_rotation_offset = 0; + // return offset in radians + return rotation / (float)cpr * _2PI; +} +// returns 0 if it has no absolute 0 measurement +// 0 - incremental encoder without index +// 1 - encoder with index & magnetic sensors +int MagneticSensorI2C::hasAbsoluteZero(){ + return 1; +} +// returns 0 if it does need search for absolute zero +// 0 - magnetic sensor +// 1 - ecoder with index +int MagneticSensorI2C::needsAbsoluteZeroSearch(){ + return 0; +} + + +// function reading the raw counter of the magnetic sensor +int MagneticSensorI2C::getRawCount(){ + return (int)MagneticSensorI2C::read(angle_register_msb); +} + +// I2C functions +/* +* Read a register from the sensor +* Takes the address of the register as a uint8_t +* Returns the value of the register +*/ +int MagneticSensorI2C::read(uint8_t angle_reg_msb) { + // read the angle register first MSB then LSB + byte readArray[2]; + uint16_t readValue = 0; + // notify the device that is aboout to be read + wire->beginTransmission(chip_address); + wire->write(angle_reg_msb); + wire->endTransmission(false); + + // read the data msb and lsb + wire->requestFrom(chip_address, (uint8_t)2); + for (byte i=0; i < 2; i++) { + readArray[i] = wire->read(); + } + + // depending on the sensor architecture there are different combinations of + // LSB and MSB register used bits + // AS5600 uses 0..7 LSB and 8..11 MSB + // AS5048 uses 0..5 LSB and 6..13 MSB + readValue = ( readArray[1] & lsb_mask ); + readValue += ( ( readArray[0] & msb_mask ) << lsb_used ); + return readValue; +} + +/* +* Checks whether other devices have locked the bus. Can clear SDA locks. +* This should be called before sensor.init() on devices that suffer i2c slaves locking sda +* e.g some stm32 boards with AS5600 chips +* Takes the sda_pin and scl_pin +* Returns 0 for OK, 1 for other master and 2 for unfixable sda locked LOW +*/ +int MagneticSensorI2C::checkBus(byte sda_pin, byte scl_pin) { + + pinMode(scl_pin, INPUT_PULLUP); + pinMode(sda_pin, INPUT_PULLUP); + delay(250); + + if (digitalRead(scl_pin) == LOW) { + // Someone else has claimed master!"); + return 1; + } + + if(digitalRead(sda_pin) == LOW) { + // slave is communicating and awaiting clocks, we are blocked + pinMode(scl_pin, OUTPUT); + for (byte i = 0; i < 16; i++) { + // toggle clock for 2 bytes of data + digitalWrite(scl_pin, LOW); + delayMicroseconds(20); + digitalWrite(scl_pin, HIGH); + delayMicroseconds(20); + } + pinMode(sda_pin, INPUT); + delayMicroseconds(20); + if (digitalRead(sda_pin) == LOW) { + // SDA still blocked + return 2; + } + _delay(1000); + } + // SDA is clear (HIGH) + pinMode(sda_pin, INPUT); + pinMode(scl_pin, INPUT); + + return 0; +} diff --git a/library_source/sensors/MagneticSensorI2C.h b/library_source/sensors/MagneticSensorI2C.h new file mode 100644 index 00000000..7fe4efeb --- /dev/null +++ b/library_source/sensors/MagneticSensorI2C.h @@ -0,0 +1,101 @@ +#ifndef MAGNETICSENSORI2C_LIB_H +#define MAGNETICSENSORI2C_LIB_H + +#include "Arduino.h" +#include +#include "../common/base_classes/Sensor.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + +struct MagneticSensorI2CConfig_s { + int chip_address; + int bit_resolution; + int angle_register; + int data_start_bit; +}; +// some predefined structures +extern MagneticSensorI2CConfig_s AS5600_I2C,AS5048_I2C; + +class MagneticSensorI2C: public Sensor{ + public: + /** + * MagneticSensorI2C class constructor + * @param chip_address I2C chip address + * @param bits number of bits of the sensor resolution + * @param angle_register_msb angle read register msb + * @param _bits_used_msb number of used bits in msb + */ + MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _msb_bits_used); + + /** + * MagneticSensorI2C class constructor + * @param config I2C config + */ + MagneticSensorI2C(MagneticSensorI2CConfig_s config); + + static MagneticSensorI2C AS5600(); + + /** sensor initialise pins */ + void init(TwoWire* _wire = &Wire); + + // implementation of abstract functions of the Sensor class + /** get current angle (rad) */ + float getAngle() override; + /** get current angular velocity (rad/s) **/ + float getVelocity() override; + /** + * set current angle as zero angle + * return the angle [rad] difference + */ + float initRelativeZero() override; + /** + * set absolute zero angle as zero angle + * return the angle [rad] difference + */ + float initAbsoluteZero() override; + /** returns 1 because it is the absolute sensor */ + int hasAbsoluteZero() override; + /** returns 0 maning it doesn't need search for absolute zero */ + + int needsAbsoluteZeroSearch() override; + + /** experimental function to check and fix SDA locked LOW issues */ + int checkBus(byte sda_pin = SDA, byte scl_pin = SCL); + + private: + float cpr; //!< Maximum range of the magnetic sensor + uint16_t lsb_used; //!< Number of bits used in LSB register + uint8_t lsb_mask; + uint8_t msb_mask; + + // I2C variables + uint8_t angle_register_msb; //!< I2C angle register to read + uint8_t chip_address; //!< I2C chip select pins + + // I2C functions + /** Read one I2C register value */ + int read(uint8_t angle_register_msb); + + word zero_offset; //!< user defined zero offset + /** + * Function getting current angle register value + * it uses angle_register variable + */ + int getRawCount(); + + // total angle tracking variables + float full_rotation_offset; //!begin(); +#ifndef ESP_H // if not ESP32 board + spi->setBitOrder(MSBFIRST); // Set the SPI_1 bit order + spi->setDataMode(spi_mode) ; + spi->setClockDivider(SPI_CLOCK_DIV8); +#endif + + digitalWrite(chip_select_pin, HIGH); + // velocity calculation init + angle_prev = 0; + velocity_calc_timestamp = _micros(); + + // full rotations tracking number + full_rotation_offset = 0; + angle_data_prev = getRawCount(); + zero_offset = 0; +} + +// Shaft angle calculation +// angle is in radians [rad] +float MagneticSensorSPI::getAngle(){ + // raw data from the sensor + float angle_data = getRawCount(); + + // tracking the number of rotations + // in order to expand angle range form [0,2PI] + // to basically infinity + float d_angle = angle_data - angle_data_prev; + // if overflow happened track it as full rotation + if(abs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; + // save the current angle value for the next steps + // in order to know if overflow happened + angle_data_prev = angle_data; + + // zero offset adding + angle_data -= (int)zero_offset; + // return the full angle + // (number of full rotations)*2PI + current sensor angle + return natural_direction * (full_rotation_offset + ( angle_data / (float)cpr) * _2PI); +} + +// Shaft velocity calculation +float MagneticSensorSPI::getVelocity(){ + // calculate sample time + unsigned long now_us = _micros(); + float Ts = (now_us - velocity_calc_timestamp)*1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + + // current angle + float angle_c = getAngle(); + // velocity calculation + float vel = (angle_c - angle_prev)/Ts; + + // save variables for future pass + angle_prev = angle_c; + velocity_calc_timestamp = now_us; + return vel; +} + +// set current angle as zero angle +// return the angle [rad] difference +float MagneticSensorSPI::initRelativeZero(){ + float angle_offset = -getAngle(); + zero_offset = natural_direction * getRawCount(); + + // angle tracking variables + full_rotation_offset = 0; + return angle_offset; +} +// set absolute zero angle as zero angle +// return the angle [rad] difference +float MagneticSensorSPI::initAbsoluteZero(){ + float rotation = -(int)zero_offset; + // init absolute zero + zero_offset = 0; + + // angle tracking variables + full_rotation_offset = 0; + // return offset in radians + return rotation / (float)cpr * _2PI; +} +// returns 0 if it has no absolute 0 measurement +// 0 - incremental encoder without index +// 1 - encoder with index & magnetic sensors +int MagneticSensorSPI::hasAbsoluteZero(){ + return 1; +} +// returns 0 if it does need search for absolute zero +// 0 - magnetic sensor +// 1 - ecoder with index +int MagneticSensorSPI::needsAbsoluteZeroSearch(){ + return 0; +} + + +// function reading the raw counter of the magnetic sensor +int MagneticSensorSPI::getRawCount(){ + return (int)MagneticSensorSPI::read(angle_register); +} + +// SPI functions +/** + * Utility function used to calculate even parity of word + */ +byte MagneticSensorSPI::spiCalcEvenParity(word value){ + byte cnt = 0; + byte i; + + for (i = 0; i < 16; i++) + { + if (value & 0x1) cnt++; + value >>= 1; + } + return cnt & 0x1; +} + + /* + * Read a register from the sensor + * Takes the address of the register as a 16 bit word + * Returns the value of the register + */ +word MagneticSensorSPI::read(word angle_register){ + + word command = angle_register; + + if (command_rw_bit > 0) { + command = angle_register | (1 << command_rw_bit); + } + if (command_parity_bit > 0) { + //Add a parity bit on the the MSB + command |= ((word)spiCalcEvenParity(command) << command_parity_bit); + } + +#if !defined(_STM32_DEF_) // if not stm chips + //SPI - begin transaction + spi->beginTransaction(settings); +#endif + + //Send the command + digitalWrite(chip_select_pin, LOW); + digitalWrite(chip_select_pin, LOW); + spi->transfer16(command); + digitalWrite(chip_select_pin,HIGH); + digitalWrite(chip_select_pin,HIGH); + +#if defined( ESP_H ) // if ESP32 board + delayMicroseconds(50); +#else + delayMicroseconds(10); +#endif + + //Now read the response + digitalWrite(chip_select_pin, LOW); + digitalWrite(chip_select_pin, LOW); + word register_value = spi->transfer16(0x00); + digitalWrite(chip_select_pin, HIGH); + digitalWrite(chip_select_pin,HIGH); + +#if !defined(_STM32_DEF_) // if not stm chips + //SPI - end transaction + spi->endTransaction(); +#endif + + register_value = register_value >> (1 + data_start_bit - bit_resolution); //this should shift data to the rightmost bits of the word + + const static word data_mask = 0xFFFF >> (16 - bit_resolution); + + return register_value & data_mask; // Return the data, stripping the non data (e.g parity) bits +} + +/** + * Closes the SPI connection + * SPI has an internal SPI-device counter, for each init()-call the close() function must be called exactly 1 time + */ +void MagneticSensorSPI::close(){ + spi->end(); +} diff --git a/library_source/sensors/MagneticSensorSPI.h b/library_source/sensors/MagneticSensorSPI.h new file mode 100644 index 00000000..05fb9091 --- /dev/null +++ b/library_source/sensors/MagneticSensorSPI.h @@ -0,0 +1,110 @@ +#ifndef MAGNETICSENSORSPI_LIB_H +#define MAGNETICSENSORSPI_LIB_H + +#include "Arduino.h" +#include +#include "../common/base_classes/Sensor.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + +#define DEF_ANGLE_REGISTER 0x3FFF + +struct MagneticSensorSPIConfig_s { + int spi_mode; + long clock_speed; + int bit_resolution; + int angle_register; + int data_start_bit; + int command_rw_bit; + int command_parity_bit; +}; +// typical configuration structures +extern MagneticSensorSPIConfig_s AS5147_SPI, MA730_SPI; + +class MagneticSensorSPI: public Sensor{ + public: + /** + * MagneticSensorSPI class constructor + * @param cs SPI chip select pin + * @param bit_resolution sensor resolution bit number + * @param angle_register (optional) angle read register - default 0x3FFF + */ + MagneticSensorSPI(int cs, float bit_resolution, int angle_register = 0); + /** + * MagneticSensorSPI class constructor + * @param config SPI config + * @param cs SPI chip select pin + */ + MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs); + + /** sensor initialise pins */ + void init(SPIClass* _spi = &SPI); + + // implementation of abstract functions of the Sensor class + /** get current angle (rad) */ + float getAngle() override; + /** get current angular velocity (rad/s) **/ + float getVelocity() override; + /** + * set current angle as zero angle + * return the angle [rad] difference + */ + float initRelativeZero() override; + /** + * set absolute zero angle as zero angle + * return the angle [rad] difference + */ + float initAbsoluteZero() override; + /** returns 1 because it is the absolute sensor */ + int hasAbsoluteZero() override; + /** returns 0 maning it doesn't need search for absolute zero */ + + int needsAbsoluteZeroSearch() override; + + // returns the spi mode (phase/polarity of read/writes) i.e one of SPI_MODE0 | SPI_MODE1 | SPI_MODE2 | SPI_MODE3 + int spi_mode; + + /* returns the speed of the SPI clock signal */ + long clock_speed; + + + private: + float cpr; //!< Maximum range of the magnetic sensor + // spi variables + int angle_register; //!< SPI angle register to read + int chip_select_pin; //!< SPI chip select pin + SPISettings settings; //!< SPI settings variable + // spi functions + /** Stop SPI communication */ + void close(); + /** Read one SPI register value */ + word read(word angle_register); + /** Calculate parity value */ + byte spiCalcEvenParity(word value); + + + word zero_offset; //!< user defined zero offset + /** + * Function getting current angle register value + * it uses angle_register variable + */ + int getRawCount(); + + // total angle tracking variables + float full_rotation_offset; //! torque/voltage : target 2V."); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor.loopFOC(); + + // iterative function setting the outter loop target + // velocity, position or voltage + // if tatget not set in parameter uses motor.target variable + motor.move(); + + // user communication + motor.command(serialReceiveUserCommand()); +} + +// utility function enabling serial communication the user +String serialReceiveUserCommand() { + + // a string to hold incoming data + static String received_chars; + + String command = ""; + + while (Serial.available()) { + // get the new byte: + char inChar = (char)Serial.read(); + // add it to the string buffer: + received_chars += inChar; + + // end of user input + if (inChar == '\n') { + + // execute the user command + command = received_chars; + + // reset the command buffer + received_chars = ""; + } + } + return command; +} diff --git a/minimal_project_examples/atmega2560_stepper_encoder/src/Encoder.cpp b/minimal_project_examples/atmega2560_stepper_encoder/src/Encoder.cpp new file mode 100644 index 00000000..1965c015 --- /dev/null +++ b/minimal_project_examples/atmega2560_stepper_encoder/src/Encoder.cpp @@ -0,0 +1,228 @@ +#include "Encoder.h" + + +/* + Encoder(int encA, int encB , int cpr, int index) + - encA, encB - encoder A and B pins + - cpr - counts per rotation number (cpm=ppm*4) + - index pin - (optional input) +*/ + +Encoder::Encoder(int _encA, int _encB , float _ppr, int _index){ + + // Encoder measurement structure init + // hardware pins + pinA = _encA; + pinB = _encB; + // counter setup + pulse_counter = 0; + pulse_timestamp = 0; + + cpr = _ppr; + A_active = 0; + B_active = 0; + I_active = 0; + // index pin + index_pin = _index; // its 0 if not used + index_pulse_counter = 0; + + // velocity calculation variables + prev_Th = 0; + pulse_per_second = 0; + prev_pulse_counter = 0; + prev_timestamp_us = _micros(); + + // extern pullup as default + pullup = Pullup::EXTERN; + // enable quadrature encoder by default + quadrature = Quadrature::ON; +} + +// Encoder interrupt callback functions +// A channel +void Encoder::handleA() { + int A = digitalRead(pinA); + switch (quadrature){ + case Quadrature::ON: + // CPR = 4xPPR + if ( A != A_active ) { + pulse_counter += (A_active == B_active) ? 1 : -1; + pulse_timestamp = _micros(); + A_active = A; + } + break; + case Quadrature::OFF: + // CPR = PPR + if(A && !digitalRead(pinB)){ + pulse_counter++; + pulse_timestamp = _micros(); + } + break; + } +} +// B channel +void Encoder::handleB() { + int B = digitalRead(pinB); + switch (quadrature){ + case Quadrature::ON: + // // CPR = 4xPPR + if ( B != B_active ) { + pulse_counter += (A_active != B_active) ? 1 : -1; + pulse_timestamp = _micros(); + B_active = B; + } + break; + case Quadrature::OFF: + // CPR = PPR + if(B && !digitalRead(pinA)){ + pulse_counter--; + pulse_timestamp = _micros(); + } + break; + } +} + +// Index channel +void Encoder::handleIndex() { + if(hasIndex()){ + int I = digitalRead(index_pin); + if(I && !I_active){ + // align encoder on each index + if(index_pulse_counter){ + long tmp = pulse_counter; + // corrent the counter value + pulse_counter = round((double)pulse_counter/(double)cpr)*cpr; + // preserve relative speed + prev_pulse_counter += pulse_counter - tmp; + } else { + // initial offset + index_pulse_counter = pulse_counter; + } + } + I_active = I; + } +} + +/* + Shaft angle calculation +*/ +float Encoder::getAngle(){ + return natural_direction * _2PI * (pulse_counter) / ((float)cpr); +} +/* + Shaft velocity calculation + function using mixed time and frequency measurement technique +*/ +float Encoder::getVelocity(){ + // timestamp + long timestamp_us = _micros(); + // sampling time calculation + float Ts = (timestamp_us - prev_timestamp_us) * 1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + + // time from last impulse + float Th = (timestamp_us - pulse_timestamp) * 1e-6; + long dN = pulse_counter - prev_pulse_counter; + + // Pulse per second calculation (Eq.3.) + // dN - impulses received + // Ts - sampling time - time in between function calls + // Th - time from last impulse + // Th_1 - time form last impulse of the previous call + // only increment if some impulses received + float dt = Ts + prev_Th - Th; + pulse_per_second = (dN != 0 && dt > Ts/2) ? dN / dt : pulse_per_second; + + // if more than 0.05 passed in between impulses + if ( Th > 0.1) pulse_per_second = 0; + + // velocity calculation + float velocity = pulse_per_second / ((float)cpr) * (_2PI); + + // save variables for next pass + prev_timestamp_us = timestamp_us; + // save velocity calculation variables + prev_Th = Th; + prev_pulse_counter = pulse_counter; + return natural_direction * (velocity); +} + +// getter for index pin +// return -1 if no index +int Encoder::needsAbsoluteZeroSearch(){ + return index_pulse_counter == 0; +} +// getter for index pin +int Encoder::hasAbsoluteZero(){ + return hasIndex(); +} +// initialize counter to zero +float Encoder::initRelativeZero(){ + long angle_offset = -pulse_counter; + pulse_counter = 0; + pulse_timestamp = _micros(); + return _2PI * (angle_offset) / ((float)cpr); +} +// initialize index to zero +float Encoder::initAbsoluteZero(){ + pulse_counter -= index_pulse_counter; + prev_pulse_counter = pulse_counter; + return (index_pulse_counter) / ((float)cpr) * (_2PI); +} +// private function used to determine if encoder has index +int Encoder::hasIndex(){ + return index_pin != 0; +} + + +// encoder initialisation of the hardware pins +// and calculation variables +void Encoder::init(){ + + // Encoder - check if pullup needed for your encoder + if(pullup == Pullup::INTERN){ + pinMode(pinA, INPUT_PULLUP); + pinMode(pinB, INPUT_PULLUP); + if(hasIndex()) pinMode(index_pin,INPUT_PULLUP); + }else{ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if(hasIndex()) pinMode(index_pin,INPUT); + } + + // counter setup + pulse_counter = 0; + pulse_timestamp = _micros(); + // velocity calculation variables + prev_Th = 0; + pulse_per_second = 0; + prev_pulse_counter = 0; + prev_timestamp_us = _micros(); + + // initial cpr = PPR + // change it if the mode is quadrature + if(quadrature == Quadrature::ON) cpr = 4*cpr; + +} + +// function enabling hardware interrupts of the for the callback provided +// if callback is not provided then the interrupt is not enabled +void Encoder::enableInterrupts(void (*doA)(), void(*doB)(), void(*doIndex)()){ + // attach interrupt if functions provided + switch(quadrature){ + case Quadrature::ON: + // A callback and B callback + if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE); + if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE); + break; + case Quadrature::OFF: + // A callback and B callback + if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, RISING); + if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, RISING); + break; + } + + // if index used initialize the index interrupt + if(hasIndex() && doIndex != nullptr) attachInterrupt(digitalPinToInterrupt(index_pin), doIndex, CHANGE); +} diff --git a/minimal_project_examples/atmega2560_stepper_encoder/src/Encoder.h b/minimal_project_examples/atmega2560_stepper_encoder/src/Encoder.h new file mode 100644 index 00000000..d1a55f0e --- /dev/null +++ b/minimal_project_examples/atmega2560_stepper_encoder/src/Encoder.h @@ -0,0 +1,105 @@ +#ifndef ENCODER_LIB_H +#define ENCODER_LIB_H + +#include "Arduino.h" +#include "common/foc_utils.h" +#include "common/hardware_utils.h" +#include "common/Sensor.h" + + +/** + * Quadrature mode configuration structure + */ +enum Quadrature{ + ON, //!< Enable quadrature mode CPR = 4xPPR + OFF //!< Disable quadrature mode / CPR = PPR +}; + +class Encoder: public Sensor{ + public: + /** + Encoder class constructor + @param encA encoder B pin + @param encB encoder B pin + @param ppr impulses per rotation (cpr=ppr*4) + @param index index pin number (optional input) + */ + Encoder(int encA, int encB , float ppr, int index = 0); + + /** encoder initialise pins */ + void init(); + /** + * function enabling hardware interrupts for the encoder channels with provided callback functions + * if callback is not provided then the interrupt is not enabled + * + * @param doA pointer to the A channel interrupt handler function + * @param doB pointer to the B channel interrupt handler function + * @param doIndex pointer to the Index channel interrupt handler function + * + */ + void enableInterrupts(void (*doA)() = nullptr, void(*doB)() = nullptr, void(*doIndex)() = nullptr); + + // Encoder interrupt callback functions + /** A channel callback function */ + void handleA(); + /** B channel callback function */ + void handleB(); + /** Index channel callback function */ + void handleIndex(); + + + // pins A and B + int pinA; //!< encoder hardware pin A + int pinB; //!< encoder hardware pin B + int index_pin; //!< index pin + + // Encoder configuration + Pullup pullup; //!< Configuration parameter internal or external pullups + Quadrature quadrature;//!< Configuration parameter enable or disable quadrature mode + float cpr;//!< encoder cpr number + + // Abstract functions of the Sensor class implementation + /** get current angle (rad) */ + float getAngle() override; + /** get current angular velocity (rad/s) */ + float getVelocity() override; + /** + * set current angle as zero angle + * return the angle [rad] difference + */ + float initRelativeZero() override; + /** + * set index angle as zero angle + * return the angle [rad] difference + */ + float initAbsoluteZero() override; + /** + * returns 0 if it has no index + * 0 - encoder without index + * 1 - encoder with index + */ + int hasAbsoluteZero() override; + /** + * returns 0 if it does need search for absolute zero + * 0 - encoder without index + * 1 - ecoder with index + */ + int needsAbsoluteZeroSearch() override; + + private: + int hasIndex(); //!< function returning 1 if encoder has index pin and 0 if not. + + volatile long pulse_counter;//!< current pulse counter + volatile long pulse_timestamp;//!< last impulse timestamp in us + volatile int A_active; //!< current active states of A channel + volatile int B_active; //!< current active states of B channel + volatile int I_active; //!< current active states of Index channel + volatile long index_pulse_counter; //!< impulse counter number upon first index interrupt + + // velocity calculation variables + float prev_Th, pulse_per_second; + volatile long prev_pulse_counter, prev_timestamp_us; +}; + + +#endif diff --git a/minimal_project_examples/atmega2560_stepper_encoder/src/StepperMotor.cpp b/minimal_project_examples/atmega2560_stepper_encoder/src/StepperMotor.cpp new file mode 100644 index 00000000..22ceb256 --- /dev/null +++ b/minimal_project_examples/atmega2560_stepper_encoder/src/StepperMotor.cpp @@ -0,0 +1,309 @@ +#include "StepperMotor.h" + +// StepperMotor( int phA, int phB, int phC, int pp, int cpr, int en) +// - ph1A, ph1B - motor phase 1 pwm pins +// - ph2A, ph2B - motor phase 2 pwm pins +// - pp - pole pair number +// - enable pin - (optional input) +StepperMotor::StepperMotor(int ph1A, int ph1B, int ph2A, int ph2B, int pp, int en1, int en2) +: FOCMotor() +{ + // Pin initialization + pwm1A = ph1A; + pwm1B = ph1B; + pwm2A = ph2A; + pwm2B = ph2B; + pole_pairs = pp; + + // enable_pin pin + enable_pin1 = en1; + enable_pin2 = en2; +} + +// init hardware pins +void StepperMotor::init(long pwm_frequency) { + if(monitor_port) monitor_port->println("MOT: Init pins."); + // PWM pins + pinMode(pwm1A, OUTPUT); + pinMode(pwm1B, OUTPUT); + pinMode(pwm2A, OUTPUT); + pinMode(pwm2B, OUTPUT); + if ( enable_pin1 != NOT_SET ) pinMode(enable_pin1, OUTPUT); + if ( enable_pin2 != NOT_SET ) pinMode(enable_pin2, OUTPUT); + + if(monitor_port) monitor_port->println("MOT: PWM config."); + // Increase PWM frequency + // make silent + _setPwmFrequency(pwm_frequency, pwm1A, pwm1B, pwm2A, pwm2B); + + // sanity check for the voltage limit configuration + if(voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + // constrain voltage for sensor alignment + if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; + + // update the controller limits + PID_velocity.limit = voltage_limit; + P_angle.limit = velocity_limit; + + _delay(500); + // enable motor + if(monitor_port) monitor_port->println("MOT: Enable."); + enable(); + _delay(500); + +} + + +// disable motor driver +void StepperMotor::disable() +{ + // disable the driver - if enable_pin pin available + if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, LOW); + if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, LOW); + // set zero to PWM + setPwm(0, 0); +} +// enable motor driver +void StepperMotor::enable() +{ + // set zero to PWM + setPwm(0, 0); + // enable_pin the driver - if enable_pin pin available + if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, HIGH); + if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, HIGH); + +} + + +/** + FOC functions +*/ +// FOC initialization function +int StepperMotor::initFOC( float zero_electric_offset, Direction sensor_direction ) { + int exit_flag = 1; + // align motor if necessary + // alignment necessary for encoders! + if(zero_electric_offset != NOT_SET){ + // abosolute zero offset provided - no need to align + zero_electric_angle = zero_electric_offset; + // set the sensor direction - default CW + sensor->natural_direction = sensor_direction; + }else{ + // sensor and motor alignment + _delay(500); + exit_flag = alignSensor(); + _delay(500); + } + if(monitor_port) monitor_port->println("MOT: Motor ready."); + + return exit_flag; +} +// Encoder alignment to electrical 0 angle +int StepperMotor::alignSensor() { + if(monitor_port) monitor_port->println("MOT: Align sensor."); + // align the electrical phases of the motor and sensor + // set angle -90 degrees + + float start_angle = shaftAngle(); + for (int i = 0; i <=5; i++ ) { + float angle = _3PI_2 + _2PI * i / 6.0; + setPhaseVoltage(voltage_sensor_align, angle); + _delay(200); + } + float mid_angle = shaftAngle(); + for (int i = 5; i >=0; i-- ) { + float angle = _3PI_2 + _2PI * i / 6.0; + setPhaseVoltage(voltage_sensor_align, angle); + _delay(200); + } + if (mid_angle < start_angle) { + if(monitor_port) monitor_port->println("MOT: natural_direction==CCW"); + sensor->natural_direction = Direction::CCW; + } else if (mid_angle == start_angle) { + if(monitor_port) monitor_port->println("MOT: Sensor failed to notice movement"); + } + + // let the motor stabilize for 2 sec + _delay(2000); + // set sensor to zero + sensor->initRelativeZero(); + _delay(500); + setPhaseVoltage(0,0); + _delay(200); + + // find the index if available + int exit_flag = absoluteZeroAlign(); + _delay(500); + if(monitor_port){ + if(exit_flag< 0 ) monitor_port->println("MOT: Error: Not found!"); + if(exit_flag> 0 ) monitor_port->println("MOT: Success!"); + else monitor_port->println("MOT: Not available!"); + } + return exit_flag; +} + + +// Encoder alignment the absolute zero angle +// - to the index +int StepperMotor::absoluteZeroAlign() { + + if(monitor_port) monitor_port->println("MOT: Absolute zero align."); + // if no absolute zero return + if(!sensor->hasAbsoluteZero()) return 0; + + + if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println("MOT: Searching..."); + // search the absolute zero with small velocity + while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){ + loopFOC(); + voltage_q = PID_velocity(velocity_index_search - shaftVelocity()); + } + voltage_q = 0; + // disable motor + setPhaseVoltage(0,0); + + // align absolute zero if it has been found + if(!sensor->needsAbsoluteZeroSearch()){ + // align the sensor with the absolute zero + float zero_offset = sensor->initAbsoluteZero(); + // remember zero electric angle + zero_electric_angle = _normalizeAngle(_electricalAngle(zero_offset, pole_pairs)); + } + // return bool if zero found + return !sensor->needsAbsoluteZeroSearch() ? 1 : -1; +} + +// Iterative function looping FOC algorithm, setting Uq on the Motor +// The faster it can be run the better +void StepperMotor::loopFOC() { + // shaft angle + shaft_angle = shaftAngle(); + // set the phase voltage - FOC heart function :) + setPhaseVoltage(voltage_q, _electricalAngle(shaft_angle,pole_pairs)); +} + +// Iterative function running outer loop of the FOC algorithm +// Behavior of this function is determined by the motor.controller variable +// It runs either angle, velocity or voltage loop +// - needs to be called iteratively it is asynchronous function +// - if target is not set it uses motor.target value +void StepperMotor::move(float new_target) { + // set internal target variable + if( new_target != NOT_SET ) target = new_target; + // get angular velocity + shaft_velocity = shaftVelocity(); + // choose control loop + switch (controller) { + case ControlType::voltage: + voltage_q = target; + break; + case ControlType::angle: + // angle set point + // include angle loop + shaft_angle_sp = target; + shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); + voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + break; + case ControlType::velocity: + // velocity set point + // include velocity loop + shaft_velocity_sp = target; + voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + break; + case ControlType::velocity_openloop: + // velocity control in open loop + // loopFOC should not be called + shaft_velocity_sp = target; + velocityOpenloop(shaft_velocity_sp); + break; + case ControlType::angle_openloop: + // angle control in open loop + // loopFOC should not be called + shaft_angle_sp = target; + angleOpenloop(shaft_angle_sp); + break; + } +} + + +// Method using FOC to set Uq to the motor at the optimal angle +// Function implementingSine PWM algorithms +// - space vector not implemented yet +// +// Function using sine approximation +// regular sin + cos ~300us (no memory usaage) +// approx _sin + _cos ~110us (400Byte ~ 20% of memory) +void StepperMotor::setPhaseVoltage(float Uq, float angle_el) { + // Sinusoidal PWM modulation + // Inverse Park transformation + + // angle normalization in between 0 and 2pi + // only necessary if using _sin and _cos - approximation functions + angle_el = _normalizeAngle(angle_el + zero_electric_angle); + // Inverse park transform + Ualpha = -_sin(angle_el) * Uq; // -sin(angle) * Uq; + Ubeta = _cos(angle_el) * Uq; // cos(angle) * Uq; + // set the voltages in hardware + setPwm(Ualpha,Ubeta); +} + + + +// Set voltage to the pwm pin +void StepperMotor::setPwm(float Ualpha, float Ubeta) { + float duty_cycle1A(0.0),duty_cycle1B(0.0),duty_cycle2A(0.0),duty_cycle2B(0.0); + // hardware specific writing + if( Ualpha > 0 ) + duty_cycle1B = constrain(abs(Ualpha)/voltage_power_supply,0,1); + else + duty_cycle1A = constrain(abs(Ualpha)/voltage_power_supply,0,1); + + if( Ubeta > 0 ) + duty_cycle2B = constrain(abs(Ubeta)/voltage_power_supply,0,1); + else + duty_cycle2A = constrain(abs(Ubeta)/voltage_power_supply,0,1); + // write to hardware + _writeDutyCycle(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, pwm1A, pwm1B, pwm2A, pwm2B); +} + +// Function (iterative) generating open loop movement for target velocity +// - target_velocity - rad/s +// it uses voltage_limit variable +void StepperMotor::velocityOpenloop(float target_velocity){ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6; + + // calculate the necessary angle to achieve target velocity + shaft_angle += target_velocity*Ts; + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(voltage_limit, _electricalAngle(shaft_angle,pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; +} + +// Function (iterative) generating open loop movement towards the target angle +// - target_angle - rad +// it uses voltage_limit and velocity_limit variables +void StepperMotor::angleOpenloop(float target_angle){ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6; + + // calculate the necessary angle to move from current position towards target angle + // with maximal velocity (velocity_limit) + if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)) + shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; + else + shaft_angle = target_angle; + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(voltage_limit, _electricalAngle(shaft_angle,pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega2560_stepper_encoder/src/StepperMotor.h b/minimal_project_examples/atmega2560_stepper_encoder/src/StepperMotor.h new file mode 100644 index 00000000..bf14e096 --- /dev/null +++ b/minimal_project_examples/atmega2560_stepper_encoder/src/StepperMotor.h @@ -0,0 +1,122 @@ +/** + * @file StepperMotor.h + * + */ + +#ifndef StepperMotor_h +#define StepperMotor_h + +#include "Arduino.h" +#include "common/FOCMotor.h" +#include "common/foc_utils.h" +#include "common/hardware_utils.h" +#include "common/Sensor.h" +#include "common/defaults.h" + +/** + Stepper Motor class +*/ +class StepperMotor: public FOCMotor +{ + public: + /** + StepperMotor class constructor + @param ph1A 1A phase pwm pin + @param ph1B 1B phase pwm pin + @param ph2A 2A phase pwm pin + @param ph2B 2B phase pwm pin + @param pp pole pair number - cpr counts per rotation number (cpm=ppm*4) + @param en1 enable pin phase 1 (optional input) + @param en2 enable pin phase 2 (optional input) + */ + StepperMotor(int ph1A,int ph1B,int ph2A,int ph2B,int pp, int en1 = NOT_SET, int en2 = NOT_SET); + + /** Motor hardware init function */ + void init(long pwm_frequency = NOT_SET) override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + * + * - If zero_electric_offset parameter is set the alignment procedure is skipped + * + * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. + * @param sensor_direction sensor natural direction - default is CW + * + */ + int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override; + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + void loopFOC() override; + /** + * Function executing the control loops set by the controller parameter of the BLDCMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + void move(float target = NOT_SET) override; + + // hardware variables + int pwm1A; //!< phase 1A pwm pin number + int pwm1B; //!< phase 1B pwm pin number + int pwm2A; //!< phase 2A pwm pin number + int pwm2B; //!< phase 2B pwm pin number + int enable_pin1; //!< enable pin number phase 1 + int enable_pin2; //!< enable pin number phase 2 + + private: + + // FOC methods + /** + * Method using FOC to set Uq to the motor at the optimal angle + * Heart of the FOC algorithm + * + * @param Uq Current voltage in q axis to set to the motor + * @param angle_el current electrical angle of the motor + */ + void setPhaseVoltage(float Uq, float angle_el); + + /** Sensor alignment to electrical 0 angle of the motor */ + int alignSensor(); + /** Motor and sensor alignment to the sensors absolute 0 angle */ + int absoluteZeroAlign(); + /** + * Set phase voltages to the harware + * + * @param Ua phase A voltage + * @param Ub phase B voltage + * @param Uc phase C voltage + */ + void setPwm(float Ualpha, float Ubeta); + + // Open loop motion control + /** + * Function (iterative) generating open loop movement for target velocity + * it uses voltage_limit variable + * + * @param target_velocity - rad/s + */ + void velocityOpenloop(float target_velocity); + /** + * Function (iterative) generating open loop movement towards the target angle + * it uses voltage_limit and velocity_limit variables + * + * @param target_angle - rad + */ + void angleOpenloop(float target_angle); + // open loop variables + long open_loop_timestamp; +}; + + +#endif diff --git a/minimal_project_examples/atmega2560_stepper_encoder/src/common/FOCMotor.cpp b/minimal_project_examples/atmega2560_stepper_encoder/src/common/FOCMotor.cpp new file mode 100644 index 00000000..adecbeec --- /dev/null +++ b/minimal_project_examples/atmega2560_stepper_encoder/src/common/FOCMotor.cpp @@ -0,0 +1,245 @@ +#include "FOCMotor.h" + +/** + * Default constructor - setting all variabels to default values + */ +FOCMotor::FOCMotor() +{ + // Power supply voltage + voltage_power_supply = DEF_POWER_SUPPLY; + + // maximum angular velocity to be used for positioning + velocity_limit = DEF_VEL_LIM; + // maximum voltage to be set to the motor + voltage_limit = voltage_power_supply; + + // index search velocity + velocity_index_search = DEF_INDEX_SEARCH_TARGET_VELOCITY; + // sensor and motor align voltage + voltage_sensor_align = DEF_VOLTAGE_SENSOR_ALIGN; + + // electric angle of comthe zero angle + zero_electric_angle = 0; + + // default modulation is SinePWM + foc_modulation = FOCModulationType::SinePWM; + + // default target value + target = 0; + + //monitor_port + monitor_port = nullptr; + //sensor + sensor = nullptr; +} + + +/** + Sensor communication methods +*/ +void FOCMotor::linkSensor(Sensor* _sensor) { + sensor = _sensor; +} +// shaft angle calculation +float FOCMotor::shaftAngle() { + // if no sensor linked return 0 + if(!sensor) return shaft_angle; + return sensor->getAngle(); +} +// shaft velocity calculation +float FOCMotor::shaftVelocity() { + // if no sensor linked return 0 + if(!sensor) return 0; + return LPF_velocity(sensor->getVelocity()); +} + + + + +/** + * Monitoring functions + */ +// function implementing the monitor_port setter +void FOCMotor::useMonitoring(Print &print){ + monitor_port = &print; //operate on the address of print + if(monitor_port ) monitor_port->println("MOT: Monitor enabled!"); +} +// utility function intended to be used with serial plotter to monitor motor variables +// significantly slowing the execution down!!!! +void FOCMotor::monitor() { + if(!monitor_port) return; + switch (controller) { + case ControlType::velocity_openloop: + case ControlType::velocity: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_velocity_sp); + monitor_port->print("\t"); + monitor_port->println(shaft_velocity); + break; + case ControlType::angle_openloop: + case ControlType::angle: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_angle_sp); + monitor_port->print("\t"); + monitor_port->println(shaft_angle); + break; + case ControlType::voltage: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_angle); + monitor_port->print("\t"); + monitor_port->println(shaft_velocity); + break; + } +} + +int FOCMotor::command(String user_command) { + // error flag + int errorFlag = 1; + // if empty string + if(user_command.length() < 1) return errorFlag; + + // parse command letter + char cmd = user_command.charAt(0); + // check if get command + char GET = user_command.charAt(1) == '\n'; + // parse command values + float value = user_command.substring(1).toFloat(); + + // a bit of optimisation of variable memory for Arduino UNO (atmega328) + switch(cmd){ + case 'P': // velocity P gain change + case 'I': // velocity I gain change + case 'D': // velocity D gain change + case 'R': // velocity voltage ramp change + if(monitor_port) monitor_port->print(" PID velocity| "); + break; + case 'F': // velocity Tf low pass filter change + if(monitor_port) monitor_port->print(" LPF velocity| "); + break; + case 'K': // angle loop gain P change + if(monitor_port) monitor_port->print(" P angle| "); + break; + case 'L': // velocity voltage limit change + case 'N': // angle loop gain velocity_limit change + if(monitor_port) monitor_port->print(" Limits| "); + break; + } + + // apply the the command + switch(cmd){ + case 'P': // velocity P gain change + if(monitor_port) monitor_port->print("P: "); + if(!GET) PID_velocity.P = value; + if(monitor_port) monitor_port->println(PID_velocity.P); + break; + case 'I': // velocity I gain change + if(monitor_port) monitor_port->print("I: "); + if(!GET) PID_velocity.I = value; + if(monitor_port) monitor_port->println(PID_velocity.I); + break; + case 'D': // velocity D gain change + if(monitor_port) monitor_port->print("D: "); + if(!GET) PID_velocity.D = value; + if(monitor_port) monitor_port->println(PID_velocity.D); + break; + case 'R': // velocity voltage ramp change + if(monitor_port) monitor_port->print("volt_ramp: "); + if(!GET) PID_velocity.output_ramp = value; + if(monitor_port) monitor_port->println(PID_velocity.output_ramp); + break; + case 'L': // velocity voltage limit change + if(monitor_port) monitor_port->print("volt_limit: "); + if(!GET) { + voltage_limit = value; + PID_velocity.limit = value; + } + if(monitor_port) monitor_port->println(voltage_limit); + break; + case 'F': // velocity Tf low pass filter change + if(monitor_port) monitor_port->print("Tf: "); + if(!GET) LPF_velocity.Tf = value; + if(monitor_port) monitor_port->println(LPF_velocity.Tf); + break; + case 'K': // angle loop gain P change + if(monitor_port) monitor_port->print(" P: "); + if(!GET) P_angle.P = value; + if(monitor_port) monitor_port->println(P_angle.P); + break; + case 'N': // angle loop gain velocity_limit change + if(monitor_port) monitor_port->print("vel_limit: "); + if(!GET){ + velocity_limit = value; + P_angle.limit = value; + } + if(monitor_port) monitor_port->println(velocity_limit); + break; + case 'C': + // change control type + if(monitor_port) monitor_port->print("Control: "); + + if(GET){ // if get command + switch(controller){ + case ControlType::voltage: + if(monitor_port) monitor_port->println("voltage"); + break; + case ControlType::velocity: + if(monitor_port) monitor_port->println("velocity"); + break; + case ControlType::angle: + if(monitor_port) monitor_port->println("angle"); + break; + default: + if(monitor_port) monitor_port->println("open loop"); + } + }else{ // if set command + switch((int)value){ + case 0: + if(monitor_port) monitor_port->println("voltage"); + controller = ControlType::voltage; + break; + case 1: + if(monitor_port) monitor_port->println("velocity"); + controller = ControlType::velocity; + break; + case 2: + if(monitor_port) monitor_port->println("angle"); + controller = ControlType::angle; + break; + default: // not valid command + errorFlag = 0; + } + } + break; + case 'V': // get current values of the state variables + switch((int)value){ + case 0: // get voltage + if(monitor_port) monitor_port->print("Uq: "); + if(monitor_port) monitor_port->println(voltage_q); + break; + case 1: // get velocity + if(monitor_port) monitor_port->print("Velocity: "); + if(monitor_port) monitor_port->println(shaft_velocity); + break; + case 2: // get angle + if(monitor_port) monitor_port->print("Angle: "); + if(monitor_port) monitor_port->println(shaft_angle); + break; + case 3: // get angle + if(monitor_port) monitor_port->print("Target: "); + if(monitor_port) monitor_port->println(target); + break; + default: // not valid command + errorFlag = 0; + } + break; + default: // target change + if(monitor_port) monitor_port->print("Target : "); + target = user_command.toFloat(); + if(monitor_port) monitor_port->println(target); + } + // return 0 if error and 1 if ok + return errorFlag; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega2560_stepper_encoder/src/common/FOCMotor.h b/minimal_project_examples/atmega2560_stepper_encoder/src/common/FOCMotor.h new file mode 100644 index 00000000..40f7f6d4 --- /dev/null +++ b/minimal_project_examples/atmega2560_stepper_encoder/src/common/FOCMotor.h @@ -0,0 +1,195 @@ +#ifndef FOCMOTOR_H +#define FOCMOTOR_H + +#include "Arduino.h" +#include "hardware_utils.h" +#include "foc_utils.h" +#include "defaults.h" + +#include "Sensor.h" +#include "pid.h" +#include "lowpass_filter.h" + + +/** + * Motiron control type + */ +enum ControlType{ + voltage,//!< Torque control using voltage + velocity,//!< Velocity motion control + angle,//!< Position/angle motion control + velocity_openloop, + angle_openloop +}; + +/** + * FOC modulation type + */ +enum FOCModulationType{ + SinePWM, //!< Sinusoidal PWM modulation + SpaceVectorPWM //!< Space vector modulation method +}; + +/** + Generic motor class +*/ +class FOCMotor +{ + public: + /** + * Default constructor - setting all variabels to default values + */ + FOCMotor(); + + /** Motor hardware init function */ + virtual void init(long pwm_frequency)=0; + /** Motor disable function */ + virtual void disable()=0; + /** Motor enable function */ + virtual void enable()=0; + + /** + * Function linking a motor and a sensor + * + * @param sensor Sensor class wrapper for the FOC algorihtm to read the motor angle and velocity + */ + void linkSensor(Sensor* sensor); + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + * + * - If zero_electric_offset parameter is set the alignment procedure is skipped + * + * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. + * @param sensor_direction sensor natural direction - default is CW + * + */ + virtual int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW)=0; + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + virtual void loopFOC()=0; + /** + * Function executing the control loops set by the controller parameter of the BLDCMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + virtual void move(float target = NOT_SET)=0; + + // State calculation methods + /** Shaft angle calculation in radians [rad] */ + float shaftAngle(); + /** + * Shaft angle calculation function in radian per second [rad/s] + * It implements low pass filtering + */ + float shaftVelocity(); + + // state variables + float target; //!< current target value - depends of the controller + float shaft_angle;//!< current motor angle + float shaft_velocity;//!< current motor velocity + float shaft_velocity_sp;//!< current target velocity + float shaft_angle_sp;//!< current target angle + float voltage_q;//!< current voltage u_q set + float Ua,Ub,Uc;//!< Current phase voltages Ua,Ub and Uc set to motor + float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform + + // motor configuration parameters + float voltage_power_supply;//!< Power supply voltage + float voltage_sensor_align;//!< sensor and motor align voltage parameter + float velocity_index_search;//!< target velocity for index search + int pole_pairs;//!< Motor pole pairs number + + // limiting variables + float voltage_limit; //!< Voltage limitting variable - global limit + float velocity_limit; //!< Velocity limitting variable - global limit + + float zero_electric_angle;//! _2PI ? a_sin - _2PI : a_sin; + return _sin(a_sin); +} + + +// normalizing radian angle to [0,2PI] +float _normalizeAngle(float angle){ + float a = fmod(angle, _2PI); + return a >= 0 ? a : (a + _2PI); +} + +// Electrical angle calculation +float _electricalAngle(float shaft_angle, int pole_pairs) { + return (shaft_angle * pole_pairs); +} \ No newline at end of file diff --git a/minimal_project_examples/atmega2560_stepper_encoder/src/common/foc_utils.h b/minimal_project_examples/atmega2560_stepper_encoder/src/common/foc_utils.h new file mode 100644 index 00000000..5ab34f42 --- /dev/null +++ b/minimal_project_examples/atmega2560_stepper_encoder/src/common/foc_utils.h @@ -0,0 +1,55 @@ +#ifndef FOCUTILS_LIB_H +#define FOCUTILS_LIB_H + +#include "Arduino.h" + +// sign function +#define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) ) +#define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) + +// utility defines +#define _2_SQRT3 1.15470053838 +#define _SQRT3 1.73205080757 +#define _1_SQRT3 0.57735026919 +#define _SQRT3_2 0.86602540378 +#define _SQRT2 1.41421356237 +#define _120_D2R 2.09439510239 +#define _PI 3.14159265359 +#define _PI_2 1.57079632679 +#define _PI_3 1.0471975512 +#define _2PI 6.28318530718 +#define _3PI_2 4.71238898038 + + +#define NOT_SET -12345.0 + +/** + * Function approximating the sine calculation by using fixed size array + * - execution time ~40us (Arduino UNO) + * + * @param a angle in between 0 and 2PI + */ +float _sin(float a); +/** + * Function approximating cosine calculation by using fixed size array + * - execution time ~50us (Arduino UNO) + * + * @param a angle in between 0 and 2PI + */ +float _cos(float a); + +/** + * normalizing radian angle to [0,2PI] + * @param angle - angle to be normalized + */ +float _normalizeAngle(float angle); + + +/** + * Electrical angle calculation + * + * @param shaft_angle - shaft angle of the motor + * @param pole_pairs - number of pole pairs + */ +float _electricalAngle(float shaft_angle, int pole_pairs); +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega2560_stepper_encoder/src/common/hardware_utils.cpp b/minimal_project_examples/atmega2560_stepper_encoder/src/common/hardware_utils.cpp new file mode 100644 index 00000000..0d080bab --- /dev/null +++ b/minimal_project_examples/atmega2560_stepper_encoder/src/common/hardware_utils.cpp @@ -0,0 +1,284 @@ +#include "hardware_utils.h" + +#if defined(ESP_H) // if ESP32 board +// empty motor slot +#define _EMPTY_SLOT -20 + +// 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_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_motor_slots_t; + +// define bldc motor slots array +bldc_motor_slots_t esp32_bldc_motor_slots[4] = { + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}, // 2nd motor will be MCPWM0 channel B + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 3rd motor will be MCPWM1 channel A + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B} // 4th motor will be MCPWM1 channel B + }; + +// define stepper motor slots array +stepper_motor_slots_t esp32_stepper_motor_slots[2] = { + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1 + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0 + }; + +// configuring high frequency pwm timer +// https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller +void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit){ + + mcpwm_config_t pwm_config; + pwm_config.frequency = pwm_frequency; //frequency + pwm_config.cmpr_a = 0; //duty cycle of PWMxA = 50.0% + pwm_config.cmpr_b = 0; //duty cycle of PWMxB = 50.0% + pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave) + pwm_config.duty_mode = MCPWM_DUTY_MODE_0; // Active HIGH + mcpwm_init(mcpwm_unit, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings + mcpwm_init(mcpwm_unit, MCPWM_TIMER_1, &pwm_config); //Configure PWM0A & PWM0B with above settings + mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &pwm_config); //Configure PWM0A & PWM0B with above settings + + _delay(100); + + mcpwm_stop(mcpwm_unit, MCPWM_TIMER_0); + mcpwm_stop(mcpwm_unit, MCPWM_TIMER_1); + mcpwm_stop(mcpwm_unit, MCPWM_TIMER_2); + mcpwm_num->clk_cfg.prescale = 0; + + mcpwm_num->timer[0].period.prescale = 4; + mcpwm_num->timer[1].period.prescale = 4; + mcpwm_num->timer[2].period.prescale = 4; + _delay(1); + mcpwm_num->timer[0].period.period = 2048; + mcpwm_num->timer[1].period.period = 2048; + mcpwm_num->timer[2].period.period = 2048; + _delay(1); + mcpwm_num->timer[0].period.upmethod = 0; + mcpwm_num->timer[1].period.upmethod = 0; + mcpwm_num->timer[2].period.upmethod = 0; + _delay(1); + mcpwm_start(mcpwm_unit, MCPWM_TIMER_0); + mcpwm_start(mcpwm_unit, MCPWM_TIMER_1); + mcpwm_start(mcpwm_unit, MCPWM_TIMER_2); + + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SELECT_SYNC_INT0, 0); + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_SELECT_SYNC_INT0, 0); + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_SELECT_SYNC_INT0, 0); + _delay(1); + mcpwm_num->timer[0].sync.out_sel = 1; + _delay(1); + mcpwm_num->timer[0].sync.out_sel = 0; +} + +#elif defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if arduino uno and other ATmega328p chips +// set pwm frequency to 32KHz +void _pinHighFrequency(const int pin){ + // High PWM frequency + // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328 + if (pin == 5 || pin == 6 ) { + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 + } + if (pin == 9 || pin == 10 ) + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 + if (pin == 3 || pin == 11) + TCCR2B = ((TCCR2B & 0b11111000) | 0x01);// set prescaler to 1 + +} +#elif defined(_STM32_DEF_) // if stm chips +// configure High PWM frequency +void _setHighFrequency(const long freq, const int pin){ + analogWrite(pin, 0); + analogWriteFrequency(freq); + analogWriteResolution(12); // resolution 12 bit 0 - 4096 +} +#elif defined(__arm__) && defined(CORE_TEENSY) //if teensy 3x / 4x / LC boards +// configure High PWM frequency +void _setHighFrequency(const long freq, const int pin){ + analogWrite(pin, 0); + analogWriteFrequency(freq); +} +#endif + + +// function setting the high pwm frequency to the supplied pins +// - hardware speciffic +// supports Arudino/ATmega328, STM32 and ESP32 +void _setPwmFrequency(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if arduino uno and other ATmega328p chips + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); + _pinHighFrequency(pinC); + if(pinD != NOT_SET) _pinHighFrequency(pinD); // stepper motor + +#elif defined(_STM32_DEF_) || (defined(__arm__) && defined(CORE_TEENSY)) //if stm32 or teensy 3x / 4x / LC boards + if(pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz + else pwm_frequency = constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); + _setHighFrequency(pwm_frequency, pinC); + if(pinD != NOT_SET) _setHighFrequency(pwm_frequency, pinD); // stepper motor + +#elif defined(ESP_H) // if esp32 boards + if(pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency + else pwm_frequency = constrain(2*pwm_frequency, 0, 60000); // constrain to 30kHz max - centered pwm has twice lower frequency + + if(pinD == NOT_SET){ + bldc_motor_slots_t m_slot = {}; + + // determine which motor are we connecting + // and set the appropriate configuration parameters + for(int i = 0; i < 4; i++){ + if(esp32_bldc_motor_slots[i].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_bldc_motor_slots[i].pinA = pinA; + m_slot = esp32_bldc_motor_slots[i]; + break; + } + } + + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_c, pinC); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + + }else{ + stepper_motor_slots_t m_slot = {}; + // determine which motor are we connecting + // and set the appropriate configuration parameters + for(int i = 0; i < 2; i++){ + if(esp32_stepper_motor_slots[i].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_stepper_motor_slots[i].pin1A = pinA; + m_slot = esp32_stepper_motor_slots[i]; + break; + } + } + + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1a, pinA); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1b, pinB); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2a, pinC); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2b, pinD); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + } +#endif +} + +// function setting the pwm duty cycle to the hardware +//- hardware speciffic +// +// Arduino and STM32 devices use analogWrite() +// ESP32 uses MCPWM +void _writeDutyCycle(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ +#if defined(ESP_H) // if ESP32 boards + // determine which motor slot is the motor connected to + for(int i = 0; i < 4; i++){ + if(esp32_bldc_motor_slots[i].pinA == pinA){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,2047] + esp32_bldc_motor_slots[i].mcpwm_num->channel[0].cmpr_value[esp32_bldc_motor_slots[i].mcpwm_operator].cmpr_val = dc_a*2047; + esp32_bldc_motor_slots[i].mcpwm_num->channel[1].cmpr_value[esp32_bldc_motor_slots[i].mcpwm_operator].cmpr_val = dc_b*2047; + esp32_bldc_motor_slots[i].mcpwm_num->channel[2].cmpr_value[esp32_bldc_motor_slots[i].mcpwm_operator].cmpr_val = dc_c*2047; + break; + } + } +#elif defined(_STM32_DEF_) // STM32 devices + // transform duty cycle from [0,1] to [0,4095] + analogWrite(pinA, 4095.0*dc_a); + analogWrite(pinB, 4095.0*dc_b); + analogWrite(pinC, 4095.0*dc_c); +#else // Arduino & Teensy + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255*dc_a); + analogWrite(pinB, 255*dc_b); + analogWrite(pinC, 255*dc_c); +#endif +} + +// function setting the pwm duty cycle to the hardware +//- hardware speciffic +// +// Arduino and STM32 devices use analogWrite() +// ESP32 uses MCPWM +void _writeDutyCycle(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ +#if defined(ESP_H) // if ESP32 boards + // determine which motor slot is the motor connected to + for(int i = 0; i < 2; i++){ + if(esp32_stepper_motor_slots[i].pin1A == pin1A){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,2047] + esp32_stepper_motor_slots[i].mcpwm_num->channel[0].cmpr_value[esp32_stepper_motor_slots[i].mcpwm_operator1].cmpr_val = dc_1a*2047; + esp32_stepper_motor_slots[i].mcpwm_num->channel[1].cmpr_value[esp32_stepper_motor_slots[i].mcpwm_operator1].cmpr_val = dc_1b*2047; + esp32_stepper_motor_slots[i].mcpwm_num->channel[0].cmpr_value[esp32_stepper_motor_slots[i].mcpwm_operator2].cmpr_val = dc_2a*2047; + esp32_stepper_motor_slots[i].mcpwm_num->channel[1].cmpr_value[esp32_stepper_motor_slots[i].mcpwm_operator2].cmpr_val = dc_2b*2047; + break; + } + } +#elif defined(_STM32_DEF_) // STM32 devices + // transform duty cycle from [0,1] to [0,4095] + analogWrite(pin1A, 4095.0*dc_1a); + analogWrite(pin1B, 4095.0*dc_1b); + analogWrite(pin2A, 4095.0*dc_2a); + analogWrite(pin2B, 4095.0*dc_2b); +#else // Arduino & Teensy + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255*dc_1a); + analogWrite(pin1B, 255*dc_1b); + analogWrite(pin2A, 255*dc_2a); + analogWrite(pin2B, 255*dc_2b); +#endif +} + + +// function buffering delay() +// arduino uno function doesn't work well with interrupts +void _delay(unsigned long ms){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) + // if arduino uno and other atmega328p chips + // use while instad of delay, + // due to wrong measurement based on changed timer0 + unsigned long t = _micros() + ms*1000; + while( _micros() < t ){}; +#else + // regular micros + delay(ms); +#endif +} + + +// function buffering _micros() +// arduino function doesn't work well with interrupts +unsigned long _micros(){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) +// if arduino uno and other atmega328p chips + //return the value based on the prescaler + if((TCCR0B & 0b00000111) == 0x01) return (micros()/32); + else return (micros()); +#else + // regular micros + return micros(); +#endif +} diff --git a/minimal_project_examples/atmega2560_stepper_encoder/src/common/hardware_utils.h b/minimal_project_examples/atmega2560_stepper_encoder/src/common/hardware_utils.h new file mode 100644 index 00000000..f99c260a --- /dev/null +++ b/minimal_project_examples/atmega2560_stepper_encoder/src/common/hardware_utils.h @@ -0,0 +1,72 @@ +#ifndef HARDWARE_UTILS_H +#define HARDWARE_UTILS_H + +#include "Arduino.h" +#include "foc_utils.h" + + +#if defined(ESP_H) // if esp32 boards + +#include "driver/mcpwm.h" +#include "soc/mcpwm_reg.h" +#include "soc/mcpwm_struct.h" + +#endif + +/** + * High PWM frequency setting function + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA bldc motor or pin1A stepper motor + * @param pinB pinB bldc motor or pin1B stepper motor + * @param pinC pinC bldc motor or pin2A stepper motor + * @param pinD pin2B stepper motor + */ +void _setPwmFrequency(long pwm_frequency, const int pinA, const int pinB, const int pinC, const int pinD = NOT_SET); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param pinA phase A hardware pin number + * @param pinB phase B hardware pin number + * @param pinC phase C hardware pin number + */ +void _writeDutyCycle(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * hardware specific + * + * @param dc_1a duty cycle phase 1A [0, 1] + * @param dc_1b duty cycle phase 1B [0, 1] + * @param dc_2a duty cycle phase 2A [0, 1] + * @param dc_2b duty cycle phase 2B [0, 1] + * @param pin1A phase 1A hardware pin number + * @param pin1B phase 1B hardware pin number + * @param pin2A phase 2A hardware pin number + * @param pin2B phase 2B hardware pin number + */ +void _writeDutyCycle(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B); + +/** + * Function implementing delay() function in milliseconds + * - blocking function + * - hardware specific + + * @param ms number of milliseconds to wait + */ +void _delay(unsigned long ms); + +/** + * Function implementing timestamp getting function in microseconds + * hardware specific + */ +unsigned long _micros(); + + +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega2560_stepper_encoder/src/common/lowpass_filter.cpp b/minimal_project_examples/atmega2560_stepper_encoder/src/common/lowpass_filter.cpp new file mode 100644 index 00000000..9bf4ab7a --- /dev/null +++ b/minimal_project_examples/atmega2560_stepper_encoder/src/common/lowpass_filter.cpp @@ -0,0 +1,25 @@ +#include "lowpass_filter.h" + +LowPassFilter::LowPassFilter(float time_constant) + : Tf(time_constant) + , y_prev(0.0f) +{ + timestamp_prev = _micros(); +} + + +float LowPassFilter::operator() (float x) +{ + unsigned long timestamp = _micros(); + float dt = (timestamp - timestamp_prev)*1e-6f; + + if (dt < 0.0f || dt > 0.5f) + dt = 1e-3f; + + float alpha = Tf/(Tf + dt); + float y = alpha*y_prev + (1.0f - alpha)*x; + + y_prev = y; + timestamp_prev = timestamp; + return y; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega2560_stepper_encoder/src/common/lowpass_filter.h b/minimal_project_examples/atmega2560_stepper_encoder/src/common/lowpass_filter.h new file mode 100644 index 00000000..5a7619cf --- /dev/null +++ b/minimal_project_examples/atmega2560_stepper_encoder/src/common/lowpass_filter.h @@ -0,0 +1,29 @@ +#ifndef LOWPASS_FILTER_H +#define LOWPASS_FILTER_H + + +#include "hardware_utils.h" +#include "foc_utils.h" + + +/** + * Low pass filter class + */ +class LowPassFilter +{ +public: + /** + * @param Tf - Low pass filter time constant + */ + LowPassFilter(float Tf); + ~LowPassFilter() = default; + + float operator() (float x); + float Tf; //!< Low pass filter time constant + +protected: + unsigned long timestamp_prev; //!< Last execution timestamp + float y_prev; //!< filtered value in previous execution step +}; + +#endif // LOWPASS_FILTER_H \ No newline at end of file diff --git a/minimal_project_examples/atmega2560_stepper_encoder/src/common/pid.cpp b/minimal_project_examples/atmega2560_stepper_encoder/src/common/pid.cpp new file mode 100644 index 00000000..277e17ce --- /dev/null +++ b/minimal_project_examples/atmega2560_stepper_encoder/src/common/pid.cpp @@ -0,0 +1,56 @@ +#include "pid.h" + +PIDController::PIDController(float P, float I, float D, float ramp, float limit) + : P(P) + , I(I) + , D(D) + , output_ramp(ramp) // output derivative limit [volts/second] + , limit(limit) // output supply limit [volts] + , integral_prev(0.0) + , error_prev(0.0) + , output_prev(0.0) +{ + timestamp_prev = _micros()*1e-6; +} + +// PID controller function +float PIDController::operator() (float error){ + // calculate the time from the last call + unsigned long timestamp_now = _micros(); + float Ts = (timestamp_now - timestamp_prev) * 1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + + // u(s) = (P + I/s + Ds)e(s) + // Discrete implementations + // proportional part + // u_p = P *e(k) + float proportional = P * error_prev; + // Tustin transform of the integral part + // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) + float integral = integral_prev + I*Ts*0.5*(error + error_prev); + // antiwindup - limit the output voltage_q + integral = constrain(integral, -limit, limit); + // Discrete derivation + // u_dk = D(ek - ek_1)/Ts + float derivative = D*(error - error_prev)/Ts; + + // sum all the components + float output = proportional + integral + derivative; + // antiwindup - limit the output variable + output = constrain(output, -limit, limit); + + // limit the acceleration by ramping the output + float output_rate = (output - output_prev)/Ts; + if (output_rate > output_ramp) + output = output_prev + output_ramp*Ts; + else if (output_rate < -output_ramp) + output = output_prev - output_ramp*Ts; + + // saving for the next pass + integral_prev = integral; + output_prev = output; + error_prev = error; + timestamp_prev = timestamp_now; + return output; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega2560_stepper_encoder/src/common/pid.h b/minimal_project_examples/atmega2560_stepper_encoder/src/common/pid.h new file mode 100644 index 00000000..7ee337c4 --- /dev/null +++ b/minimal_project_examples/atmega2560_stepper_encoder/src/common/pid.h @@ -0,0 +1,40 @@ +#ifndef PID_H +#define PID_H + + +#include "hardware_utils.h" +#include "foc_utils.h" + +/** + * PID controller class + */ +class PIDController +{ +public: + /** + * + * @param P - Proportional gain + * @param I - Integral gain + * @param D - Derivative gain + * @param ramp - Maximum speed of change of the output value + * @param limit - Maximum output value + */ + PIDController(float P, float I, float D, float ramp, float limit); + ~PIDController() = default; + + float operator() (float error); + + float P; //!< Proportional gain + float I; //!< Integral gain + float D; //!< Derivative gain + float output_ramp; //!< Maximum speed of change of the output value + float limit; //!< Maximum output value + +protected: + float integral_prev; //!< last integral component value + float error_prev; //!< last tracking error value + float timestamp_prev; //!< Last execution timestamp + float output_prev; //!< last pid output value +}; + +#endif // PID_H \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_encoder/atmega328_bldc_encoder.ino b/minimal_project_examples/atmega328_bldc_encoder/atmega328_bldc_encoder.ino new file mode 100644 index 00000000..5a9e2e98 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/atmega328_bldc_encoder.ino @@ -0,0 +1,151 @@ +/** + * Comprehensive BLDC motor control example using encoder + * + * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: + * - configure PID controller constants + * - change motion control loops + * - monitor motor variabels + * - set target values + * - check all the configuration values + * + * To check the config value just enter the command letter. + * For example: - to read velocity PI controller P gain run: P + * - to set velocity PI controller P gain to 1.2 run: P1.2 + * + * To change the target value just enter a number in the terminal: + * For example: - to change the target value to -0.1453 enter: -0.1453 + * - to get the current target value enter: V3 + * + * List of commands: + * - P: velocity PID controller P gain + * - I: velocity PID controller I gain + * - D: velocity PID controller D gain + * - R: velocity PID controller voltage ramp + * - F: velocity Low pass filter time constant + * - K: angle P controller P gain + * - N: angle P controller velocity limit + * - L: system voltage limit + * - C: control loop + * - 0: voltage + * - 1: velocity + * - 2: angle + * - V: get motor variables + * - 0: currently set voltage + * - 1: current velocity + * - 2: current anglex + * - 3: current target value + * + */ +#include "src/BLDCMotor.h" +#include "src/sensors/Encoder.h" +#include "src/drivers/BLDCDriver3PWM.h" + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); + +// encoder instance +Encoder encoder = Encoder(2, 3, 500); + +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + +void setup() { + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link driver + motor.linkDriver(&driver); + + + // choose FOC modulation + motor.foc_modulation = FOCModulationType::SpaceVectorPWM; + // set control loop type to be used + motor.controller = ControlType::voltage; + + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.2; + motor.PID_velocity.I = 20; + motor.PID_velocity.D = 0; + // default voltage_power_supply + motor.voltage_limit = 12; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01; + + // angle loop controller + motor.P_angle.P = 20; + // angle loop velocity limit + motor.velocity_limit = 50; + + // use monitoring with serial for motor init + // monitoring port + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialise motor + motor.init(); + // align encoder and start FOC + motor.initFOC(); + + // set the inital target value + motor.target = 2; + + + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + Serial.println("Motor commands sketch | Initial motion control > torque/voltage : target 2V."); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor.loopFOC(); + + // iterative function setting the outter loop target + // velocity, position or voltage + // if tatget not set in parameter uses motor.target variable + motor.move(); + + // user communication + motor.command(serialReceiveUserCommand()); +} + +// utility function enabling serial communication the user +String serialReceiveUserCommand() { + + // a string to hold incoming data + static String received_chars; + + String command = ""; + + while (Serial.available()) { + // get the new byte: + char inChar = (char)Serial.read(); + // add it to the string buffer: + received_chars += inChar; + + // end of user input + if (inChar == '\n') { + + // execute the user command + command = received_chars; + + // reset the command buffer + received_chars = ""; + } + } + return command; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/BLDCMotor.cpp b/minimal_project_examples/atmega328_bldc_encoder/src/BLDCMotor.cpp new file mode 100644 index 00000000..bed4cd6a --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/BLDCMotor.cpp @@ -0,0 +1,419 @@ +#include "BLDCMotor.h" + +// // BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en) +// // - phA, phB, phC - motor A,B,C phase pwm pins +// // - pp - pole pair number +// // - cpr - counts per rotation number (cpm=ppm*4) +// // - enable pin - (optional input) +// BLDCMotor::BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en){ +// noop; +// } + +// BLDCMotor( int pp) +// - phA, phB, phC - motor A,B,C phase pwm pins +// - pp - pole pair number +// - cpr - counts per rotation number (cpm=ppm*4) +// - enable pin - (optional input) +BLDCMotor::BLDCMotor(int pp) +: FOCMotor() +{ + // save pole pairs number + pole_pairs = pp; +} + + +/** + Link the driver which controls the motor +*/ +void BLDCMotor::linkDriver(BLDCDriver* _driver) { + driver = _driver; +} + +// init hardware pins +void BLDCMotor::init() { + if(monitor_port) monitor_port->println("MOT: Initialise variables."); + // sanity check for the voltage limit configuration + if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; + // constrain voltage for sensor alignment + if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; + // update the controller limits + PID_velocity.limit = voltage_limit; + P_angle.limit = velocity_limit; + + _delay(500); + // enable motor + if(monitor_port) monitor_port->println("MOT: Enable driver."); + enable(); + _delay(500); +} + + +// disable motor driver +void BLDCMotor::disable() +{ + // set zero to PWM + driver->setPwm(0, 0, 0); + // disable the driver + driver->disable(); +} +// enable motor driver +void BLDCMotor::enable() +{ + // enable the driver + driver->enable(); + // set zero to PWM + driver->setPwm(0, 0, 0); + +} + +/** + FOC functions +*/ +// FOC initialization function +int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction ) { + int exit_flag = 1; + // align motor if necessary + // alignment necessary for encoders! + if(zero_electric_offset != NOT_SET){ + // abosolute zero offset provided - no need to align + zero_electric_angle = zero_electric_offset; + // set the sensor direction - default CW + sensor->natural_direction = sensor_direction; + }else{ + // sensor and motor alignment + _delay(500); + exit_flag = alignSensor(); + _delay(500); + } + if(monitor_port) monitor_port->println("MOT: Motor ready."); + + return exit_flag; +} +// Encoder alignment to electrical 0 angle +int BLDCMotor::alignSensor() { + if(monitor_port) monitor_port->println("MOT: Align sensor."); + // align the electrical phases of the motor and sensor + // set angle -90 degrees + + float start_angle = shaftAngle(); + for (int i = 0; i <=5; i++ ) { + float angle = _3PI_2 + _2PI * i / 6.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(200); + } + float mid_angle = shaftAngle(); + for (int i = 5; i >=0; i-- ) { + float angle = _3PI_2 + _2PI * i / 6.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(200); + } + if (mid_angle < start_angle) { + if(monitor_port) monitor_port->println("MOT: natural_direction==CCW"); + sensor->natural_direction = Direction::CCW; + } else if (mid_angle == start_angle) { + if(monitor_port) monitor_port->println("MOT: Sensor failed to notice movement"); + } + + // let the motor stabilize for 2 sec + _delay(2000); + // set sensor to zero + sensor->initRelativeZero(); + _delay(500); + setPhaseVoltage(0, 0, 0); + _delay(200); + + // find the index if available + int exit_flag = absoluteZeroAlign(); + _delay(500); + if(monitor_port){ + if(exit_flag< 0 ) monitor_port->println("MOT: Error: Not found!"); + if(exit_flag> 0 ) monitor_port->println("MOT: Success!"); + else monitor_port->println("MOT: Not available!"); + } + return exit_flag; +} + + +// Encoder alignment the absolute zero angle +// - to the index +int BLDCMotor::absoluteZeroAlign() { + + if(monitor_port) monitor_port->println("MOT: Absolute zero align."); + // if no absolute zero return + if(!sensor->hasAbsoluteZero()) return 0; + + + if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println("MOT: Searching..."); + // search the absolute zero with small velocity + while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){ + loopFOC(); + voltage_q = PID_velocity(velocity_index_search - shaftVelocity()); + } + voltage_q = 0; + // disable motor + setPhaseVoltage(0, 0, 0); + + // align absolute zero if it has been found + if(!sensor->needsAbsoluteZeroSearch()){ + // align the sensor with the absolute zero + float zero_offset = sensor->initAbsoluteZero(); + // remember zero electric angle + zero_electric_angle = _normalizeAngle(_electricalAngle(zero_offset, pole_pairs)); + } + // return bool if zero found + return !sensor->needsAbsoluteZeroSearch() ? 1 : -1; +} + +// Iterative function looping FOC algorithm, setting Uq on the Motor +// The faster it can be run the better +void BLDCMotor::loopFOC() { + // shaft angle + shaft_angle = shaftAngle(); + // set the phase voltage - FOC heart function :) + setPhaseVoltage(voltage_q, voltage_d, _electricalAngle(shaft_angle,pole_pairs)); +} + +// Iterative function running outer loop of the FOC algorithm +// Behavior of this function is determined by the motor.controller variable +// It runs either angle, velocity or voltage loop +// - needs to be called iteratively it is asynchronous function +// - if target is not set it uses motor.target value +void BLDCMotor::move(float new_target) { + // set internal target variable + if( new_target != NOT_SET ) target = new_target; + // get angular velocity + shaft_velocity = shaftVelocity(); + // choose control loop + switch (controller) { + case ControlType::voltage: + voltage_q = target; + break; + case ControlType::angle: + // angle set point + // include angle loop + shaft_angle_sp = target; + shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); + voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + break; + case ControlType::velocity: + // velocity set point + // include velocity loop + shaft_velocity_sp = target; + voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + break; + case ControlType::velocity_openloop: + // velocity control in open loop + // loopFOC should not be called + shaft_velocity_sp = target; + velocityOpenloop(shaft_velocity_sp); + break; + case ControlType::angle_openloop: + // angle control in open loop + // loopFOC should not be called + shaft_angle_sp = target; + angleOpenloop(shaft_angle_sp); + break; + } +} + + +// Method using FOC to set Uq and Ud to the motor at the optimal angle +// Function implementing Space Vector PWM and Sine PWM algorithms +// +// Function using sine approximation +// regular sin + cos ~300us (no memory usaage) +// approx _sin + _cos ~110us (400Byte ~ 20% of memory) +void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { + + const bool centered = true; + int sector; + float _ca,_sa; + + switch (foc_modulation) + { + case FOCModulationType::Trapezoid_120 : + // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5 + static int trap_120_map[6][3] = { + {0,1,-1},{-1,1,0},{-1,0,1},{0,-1,1},{1,-1,0},{1,0,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + }; + // static int trap_120_state = 0; + sector = 6 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + + Ua = Uq + trap_120_map[sector][0] * Uq; + Ub = Uq + trap_120_map[sector][1] * Uq; + Uc = Uq + trap_120_map[sector][2] * Uq; + + if (centered) { + Ua += (driver->voltage_limit)/2 -Uq; + Ub += (driver->voltage_limit)/2 -Uq; + Uc += (driver->voltage_limit)/2 -Uq; + } + break; + + case FOCModulationType::Trapezoid_150 : + // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8 + static int trap_150_map[12][3] = { + {0,1,-1},{-1,1,-1},{-1,1,0},{-1,1,1},{-1,0,1},{-1,-1,1},{0,-1,1},{1,-1,1},{1,-1,0},{1,-1,-1},{1,0,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + }; + // static int trap_150_state = 0; + sector = 12 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + + Ua = Uq + trap_150_map[sector][0] * Uq; + Ub = Uq + trap_150_map[sector][1] * Uq; + Uc = Uq + trap_150_map[sector][2] * Uq; + + //center + if (centered) { + Ua += (driver->voltage_limit)/2 -Uq; + Ub += (driver->voltage_limit)/2 -Uq; + Uc += (driver->voltage_limit)/2 -Uq; + } + + break; + + case FOCModulationType::SinePWM : + // Sinusoidal PWM modulation + // Inverse Park + Clarke transformation + + // angle normalization in between 0 and 2pi + // only necessary if using _sin and _cos - approximation functions + angle_el = _normalizeAngle(angle_el + zero_electric_angle); + _ca = _cos(angle_el); + _sa = _sin(angle_el); + // Inverse park transform + Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; + Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; + + // Clarke transform + Ua = Ualpha + driver->voltage_limit/2; + Ub = -0.5 * Ualpha + _SQRT3_2 * Ubeta + driver->voltage_limit/2; + Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + driver->voltage_limit/2; + + if (!centered) { + float Umin = min(Ua, min(Ub, Uc)); + Ua -= Umin; + Ub -= Umin; + Uc -= Umin; + } + + break; + + case FOCModulationType::SpaceVectorPWM : + // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm + // https://www.youtube.com/watch?v=QMSWUMEAejg + + // if negative voltages change inverse the phase + // angle + 180degrees + if(Uq < 0) angle_el += _PI; + Uq = abs(Uq); + + // angle normalisation in between 0 and 2pi + // only necessary if using _sin and _cos - approximation functions + angle_el = _normalizeAngle(angle_el + zero_electric_angle + _PI_2); + + // find the sector we are in currently + sector = floor(angle_el / _PI_3) + 1; + // calculate the duty cycles + float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_limit; + float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_limit; + // two versions possible + float T0 = 0; // pulled to 0 - better for low power supply voltage + if (centered) { + T0 = 1 - T1 - T2; //centered around driver->voltage_limit/2 + } + + // calculate the duty cycles(times) + float Ta,Tb,Tc; + switch(sector){ + case 1: + Ta = T1 + T2 + T0/2; + Tb = T2 + T0/2; + Tc = T0/2; + break; + case 2: + Ta = T1 + T0/2; + Tb = T1 + T2 + T0/2; + Tc = T0/2; + break; + case 3: + Ta = T0/2; + Tb = T1 + T2 + T0/2; + Tc = T2 + T0/2; + break; + case 4: + Ta = T0/2; + Tb = T1+ T0/2; + Tc = T1 + T2 + T0/2; + break; + case 5: + Ta = T2 + T0/2; + Tb = T0/2; + Tc = T1 + T2 + T0/2; + break; + case 6: + Ta = T1 + T2 + T0/2; + Tb = T0/2; + Tc = T1 + T0/2; + break; + default: + // possible error state + Ta = 0; + Tb = 0; + Tc = 0; + } + + // calculate the phase voltages and center + Ua = Ta*driver->voltage_limit; + Ub = Tb*driver->voltage_limit; + Uc = Tc*driver->voltage_limit; + break; + + } + + // set the voltages in driver + driver->setPwm(Ua, Ub, Uc); +} + + + +// Function (iterative) generating open loop movement for target velocity +// - target_velocity - rad/s +// it uses voltage_limit variable +void BLDCMotor::velocityOpenloop(float target_velocity){ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6; + + // calculate the necessary angle to achieve target velocity + shaft_angle += target_velocity*Ts; + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; +} + +// Function (iterative) generating open loop movement towards the target angle +// - target_angle - rad +// it uses voltage_limit and velocity_limit variables +void BLDCMotor::angleOpenloop(float target_angle){ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6; + + // calculate the necessary angle to move from current position towards target angle + // with maximal velocity (velocity_limit) + if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)) + shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; + else + shaft_angle = target_angle; + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/BLDCMotor.h b/minimal_project_examples/atmega328_bldc_encoder/src/BLDCMotor.h new file mode 100644 index 00000000..164cc108 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/BLDCMotor.h @@ -0,0 +1,109 @@ +#ifndef BLDCMotor_h +#define BLDCMotor_h + +#include "Arduino.h" +#include "common/base_classes/FOCMotor.h" +#include "common/base_classes/Sensor.h" +#include "common/base_classes/BLDCDriver.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" +#include "common/defaults.h" + +/** + BLDC motor class +*/ +class BLDCMotor: public FOCMotor +{ + public: + /** + BLDCMotor class constructor + @param pp pole pairs number + */ + BLDCMotor(int pp); + + /** + * Function linking a motor and a foc driver + * + * @param driver BLDCDriver class implementing all the hardware specific functions necessary PWM setting + */ + void linkDriver(BLDCDriver* driver); + + /** + * BLDCDriver link: + * - 3PWM + * - 6PWM + */ + BLDCDriver* driver; + + /** Motor hardware init function */ + void init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + */ + int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override; + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + void loopFOC() override; + + /** + * Function executing the control loops set by the controller parameter of the BLDCMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + void move(float target = NOT_SET) override; + + float Ua,Ub,Uc;//!< Current phase voltages Ua,Ub and Uc set to motor + float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform + + + private: + // FOC methods + /** + * Method using FOC to set Uq to the motor at the optimal angle + * Heart of the FOC algorithm + * + * @param Uq Current voltage in q axis to set to the motor + * @param Ud Current voltage in d axis to set to the motor + * @param angle_el current electrical angle of the motor + */ + void setPhaseVoltage(float Uq, float Ud, float angle_el); + /** Sensor alignment to electrical 0 angle of the motor */ + int alignSensor(); + /** Motor and sensor alignment to the sensors absolute 0 angle */ + int absoluteZeroAlign(); + + + // Open loop motion control + /** + * Function (iterative) generating open loop movement for target velocity + * it uses voltage_limit variable + * + * @param target_velocity - rad/s + */ + void velocityOpenloop(float target_velocity); + /** + * Function (iterative) generating open loop movement towards the target angle + * it uses voltage_limit and velocity_limit variables + * + * @param target_angle - rad + */ + void angleOpenloop(float target_angle); + // open loop variables + long open_loop_timestamp; +}; + + +#endif diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/common/base_classes/BLDCDriver.h b/minimal_project_examples/atmega328_bldc_encoder/src/common/base_classes/BLDCDriver.h new file mode 100644 index 00000000..708323fb --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/common/base_classes/BLDCDriver.h @@ -0,0 +1,28 @@ +#ifndef BLDCDRIVER_H +#define BLDCDRIVER_H + +class BLDCDriver{ + public: + + /** Initialise hardware */ + virtual int init(); + /** Enable hardware */ + virtual void enable(); + /** Disable hardware */ + virtual void disable(); + + long pwm_frequency; //!< pwm frequency value in hertz + float voltage_power_supply; //!< power supply voltage + float voltage_limit; //!< limiting voltage set to the motor + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + virtual void setPwm(float Ua, float Ub, float Uc); +}; + +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/common/base_classes/FOCMotor.cpp b/minimal_project_examples/atmega328_bldc_encoder/src/common/base_classes/FOCMotor.cpp new file mode 100644 index 00000000..1d1a0f43 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/common/base_classes/FOCMotor.cpp @@ -0,0 +1,241 @@ +#include "FOCMotor.h" + +/** + * Default constructor - setting all variabels to default values + */ +FOCMotor::FOCMotor() +{ + // maximum angular velocity to be used for positioning + velocity_limit = DEF_VEL_LIM; + // maximum voltage to be set to the motor + voltage_limit = DEF_POWER_SUPPLY; + + // index search velocity + velocity_index_search = DEF_INDEX_SEARCH_TARGET_VELOCITY; + // sensor and motor align voltage + voltage_sensor_align = DEF_VOLTAGE_SENSOR_ALIGN; + + // electric angle of comthe zero angle + zero_electric_angle = 0; + + // default modulation is SinePWM + foc_modulation = FOCModulationType::SinePWM; + + // default target value + target = 0; + voltage_d = 0; + voltage_q = 0; + + //monitor_port + monitor_port = nullptr; + //sensor + sensor = nullptr; +} + + +/** + Sensor communication methods +*/ +void FOCMotor::linkSensor(Sensor* _sensor) { + sensor = _sensor; +} +// shaft angle calculation +float FOCMotor::shaftAngle() { + // if no sensor linked return 0 + if(!sensor) return shaft_angle; + return sensor->getAngle(); +} +// shaft velocity calculation +float FOCMotor::shaftVelocity() { + // if no sensor linked return 0 + if(!sensor) return 0; + return LPF_velocity(sensor->getVelocity()); +} + +/** + * Monitoring functions + */ +// function implementing the monitor_port setter +void FOCMotor::useMonitoring(Print &print){ + monitor_port = &print; //operate on the address of print + if(monitor_port ) monitor_port->println("MOT: Monitor enabled!"); +} +// utility function intended to be used with serial plotter to monitor motor variables +// significantly slowing the execution down!!!! +void FOCMotor::monitor() { + if(!monitor_port) return; + switch (controller) { + case ControlType::velocity_openloop: + case ControlType::velocity: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_velocity_sp); + monitor_port->print("\t"); + monitor_port->println(shaft_velocity); + break; + case ControlType::angle_openloop: + case ControlType::angle: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_angle_sp); + monitor_port->print("\t"); + monitor_port->println(shaft_angle); + break; + case ControlType::voltage: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_angle); + monitor_port->print("\t"); + monitor_port->println(shaft_velocity); + break; + } +} + +int FOCMotor::command(String user_command) { + // error flag + int errorFlag = 1; + // if empty string + if(user_command.length() < 1) return errorFlag; + + // parse command letter + char cmd = user_command.charAt(0); + // check if get command + char GET = user_command.charAt(1) == '\n'; + // parse command values + float value = user_command.substring(1).toFloat(); + + // a bit of optimisation of variable memory for Arduino UNO (atmega328) + switch(cmd){ + case 'P': // velocity P gain change + case 'I': // velocity I gain change + case 'D': // velocity D gain change + case 'R': // velocity voltage ramp change + if(monitor_port) monitor_port->print(" PID velocity| "); + break; + case 'F': // velocity Tf low pass filter change + if(monitor_port) monitor_port->print(" LPF velocity| "); + break; + case 'K': // angle loop gain P change + if(monitor_port) monitor_port->print(" P angle| "); + break; + case 'L': // velocity voltage limit change + case 'N': // angle loop gain velocity_limit change + if(monitor_port) monitor_port->print(" Limits| "); + break; + } + + // apply the the command + switch(cmd){ + case 'P': // velocity P gain change + if(monitor_port) monitor_port->print("P: "); + if(!GET) PID_velocity.P = value; + if(monitor_port) monitor_port->println(PID_velocity.P); + break; + case 'I': // velocity I gain change + if(monitor_port) monitor_port->print("I: "); + if(!GET) PID_velocity.I = value; + if(monitor_port) monitor_port->println(PID_velocity.I); + break; + case 'D': // velocity D gain change + if(monitor_port) monitor_port->print("D: "); + if(!GET) PID_velocity.D = value; + if(monitor_port) monitor_port->println(PID_velocity.D); + break; + case 'R': // velocity voltage ramp change + if(monitor_port) monitor_port->print("volt_ramp: "); + if(!GET) PID_velocity.output_ramp = value; + if(monitor_port) monitor_port->println(PID_velocity.output_ramp); + break; + case 'L': // velocity voltage limit change + if(monitor_port) monitor_port->print("volt_limit: "); + if(!GET) { + voltage_limit = value; + PID_velocity.limit = value; + } + if(monitor_port) monitor_port->println(voltage_limit); + break; + case 'F': // velocity Tf low pass filter change + if(monitor_port) monitor_port->print("Tf: "); + if(!GET) LPF_velocity.Tf = value; + if(monitor_port) monitor_port->println(LPF_velocity.Tf); + break; + case 'K': // angle loop gain P change + if(monitor_port) monitor_port->print(" P: "); + if(!GET) P_angle.P = value; + if(monitor_port) monitor_port->println(P_angle.P); + break; + case 'N': // angle loop gain velocity_limit change + if(monitor_port) monitor_port->print("vel_limit: "); + if(!GET){ + velocity_limit = value; + P_angle.limit = value; + } + if(monitor_port) monitor_port->println(velocity_limit); + break; + case 'C': + // change control type + if(monitor_port) monitor_port->print("Control: "); + + if(GET){ // if get command + switch(controller){ + case ControlType::voltage: + if(monitor_port) monitor_port->println("voltage"); + break; + case ControlType::velocity: + if(monitor_port) monitor_port->println("velocity"); + break; + case ControlType::angle: + if(monitor_port) monitor_port->println("angle"); + break; + default: + if(monitor_port) monitor_port->println("open loop"); + } + }else{ // if set command + switch((int)value){ + case 0: + if(monitor_port) monitor_port->println("voltage"); + controller = ControlType::voltage; + break; + case 1: + if(monitor_port) monitor_port->println("velocity"); + controller = ControlType::velocity; + break; + case 2: + if(monitor_port) monitor_port->println("angle"); + controller = ControlType::angle; + break; + default: // not valid command + errorFlag = 0; + } + } + break; + case 'V': // get current values of the state variables + switch((int)value){ + case 0: // get voltage + if(monitor_port) monitor_port->print("Uq: "); + if(monitor_port) monitor_port->println(voltage_q); + break; + case 1: // get velocity + if(monitor_port) monitor_port->print("Velocity: "); + if(monitor_port) monitor_port->println(shaft_velocity); + break; + case 2: // get angle + if(monitor_port) monitor_port->print("Angle: "); + if(monitor_port) monitor_port->println(shaft_angle); + break; + case 3: // get angle + if(monitor_port) monitor_port->print("Target: "); + if(monitor_port) monitor_port->println(target); + break; + default: // not valid command + errorFlag = 0; + } + break; + default: // target change + if(monitor_port) monitor_port->print("Target : "); + target = user_command.toFloat(); + if(monitor_port) monitor_port->println(target); + } + // return 0 if error and 1 if ok + return errorFlag; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/common/base_classes/FOCMotor.h b/minimal_project_examples/atmega328_bldc_encoder/src/common/base_classes/FOCMotor.h new file mode 100644 index 00000000..986e1ab2 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/common/base_classes/FOCMotor.h @@ -0,0 +1,197 @@ +#ifndef FOCMOTOR_H +#define FOCMOTOR_H + +#include "Arduino.h" +#include "Sensor.h" +#include "BLDCDriver.h" + +#include "../time_utils.h" +#include "../foc_utils.h" +#include "../defaults.h" +#include "../pid.h" +#include "../lowpass_filter.h" + + +/** + * Motiron control type + */ +enum ControlType{ + voltage,//!< Torque control using voltage + velocity,//!< Velocity motion control + angle,//!< Position/angle motion control + velocity_openloop, + angle_openloop +}; + +/** + * FOC modulation type + */ +enum FOCModulationType{ + SinePWM, //!< Sinusoidal PWM modulation + SpaceVectorPWM, //!< Space vector modulation method + Trapezoid_120, + Trapezoid_150 +}; + +/** + Generic motor class +*/ +class FOCMotor +{ + public: + /** + * Default constructor - setting all variabels to default values + */ + FOCMotor(); + + /** Motor hardware init function */ + virtual void init()=0; + /** Motor disable function */ + virtual void disable()=0; + /** Motor enable function */ + virtual void enable()=0; + + /** + * Function linking a motor and a sensor + * + * @param sensor Sensor class wrapper for the FOC algorihtm to read the motor angle and velocity + */ + void linkSensor(Sensor* sensor); + + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + * + * - If zero_electric_offset parameter is set the alignment procedure is skipped + * + * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. + * @param sensor_direction sensor natural direction - default is CW + * + */ + virtual int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW)=0; + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + virtual void loopFOC()=0; + /** + * Function executing the control loops set by the controller parameter of the BLDCMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + virtual void move(float target = NOT_SET)=0; + + // State calculation methods + /** Shaft angle calculation in radians [rad] */ + float shaftAngle(); + /** + * Shaft angle calculation function in radian per second [rad/s] + * It implements low pass filtering + */ + float shaftVelocity(); + + // state variables + float target; //!< current target value - depends of the controller + float shaft_angle;//!< current motor angle + float shaft_velocity;//!< current motor velocity + float shaft_velocity_sp;//!< current target velocity + float shaft_angle_sp;//!< current target angle + float voltage_q;//!< current voltage u_q set + float voltage_d;//!< current voltage u_d set + + // motor configuration parameters + float voltage_sensor_align;//!< sensor and motor align voltage parameter + float velocity_index_search;//!< target velocity for index search + int pole_pairs;//!< Motor pole pairs number + + // limiting variables + float voltage_limit; //!< Voltage limitting variable - global limit + float velocity_limit; //!< Velocity limitting variable - global limit + + float zero_electric_angle;//! _2PI ? a_sin - _2PI : a_sin; + return _sin(a_sin); +} + + +// normalizing radian angle to [0,2PI] +float _normalizeAngle(float angle){ + float a = fmod(angle, _2PI); + return a >= 0 ? a : (a + _2PI); +} + +// Electrical angle calculation +float _electricalAngle(float shaft_angle, int pole_pairs) { + return (shaft_angle * pole_pairs); +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/common/foc_utils.h b/minimal_project_examples/atmega328_bldc_encoder/src/common/foc_utils.h new file mode 100644 index 00000000..7da3f7bc --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/common/foc_utils.h @@ -0,0 +1,55 @@ +#ifndef FOCUTILS_LIB_H +#define FOCUTILS_LIB_H + +#include "Arduino.h" + +// sign function +#define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) ) +#define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) +#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) + +// utility defines +#define _2_SQRT3 1.15470053838 +#define _SQRT3 1.73205080757 +#define _1_SQRT3 0.57735026919 +#define _SQRT3_2 0.86602540378 +#define _SQRT2 1.41421356237 +#define _120_D2R 2.09439510239 +#define _PI 3.14159265359 +#define _PI_2 1.57079632679 +#define _PI_3 1.0471975512 +#define _2PI 6.28318530718 +#define _3PI_2 4.71238898038 + +#define NOT_SET -12345.0 + +/** + * Function approximating the sine calculation by using fixed size array + * - execution time ~40us (Arduino UNO) + * + * @param a angle in between 0 and 2PI + */ +float _sin(float a); +/** + * Function approximating cosine calculation by using fixed size array + * - execution time ~50us (Arduino UNO) + * + * @param a angle in between 0 and 2PI + */ +float _cos(float a); + +/** + * normalizing radian angle to [0,2PI] + * @param angle - angle to be normalized + */ +float _normalizeAngle(float angle); + + +/** + * Electrical angle calculation + * + * @param shaft_angle - shaft angle of the motor + * @param pole_pairs - number of pole pairs + */ +float _electricalAngle(float shaft_angle, int pole_pairs); +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/common/lowpass_filter.cpp b/minimal_project_examples/atmega328_bldc_encoder/src/common/lowpass_filter.cpp new file mode 100644 index 00000000..9bf4ab7a --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/common/lowpass_filter.cpp @@ -0,0 +1,25 @@ +#include "lowpass_filter.h" + +LowPassFilter::LowPassFilter(float time_constant) + : Tf(time_constant) + , y_prev(0.0f) +{ + timestamp_prev = _micros(); +} + + +float LowPassFilter::operator() (float x) +{ + unsigned long timestamp = _micros(); + float dt = (timestamp - timestamp_prev)*1e-6f; + + if (dt < 0.0f || dt > 0.5f) + dt = 1e-3f; + + float alpha = Tf/(Tf + dt); + float y = alpha*y_prev + (1.0f - alpha)*x; + + y_prev = y; + timestamp_prev = timestamp; + return y; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/common/lowpass_filter.h b/minimal_project_examples/atmega328_bldc_encoder/src/common/lowpass_filter.h new file mode 100644 index 00000000..7c51be54 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/common/lowpass_filter.h @@ -0,0 +1,29 @@ +#ifndef LOWPASS_FILTER_H +#define LOWPASS_FILTER_H + + +#include "time_utils.h" +#include "foc_utils.h" + + +/** + * Low pass filter class + */ +class LowPassFilter +{ +public: + /** + * @param Tf - Low pass filter time constant + */ + LowPassFilter(float Tf); + ~LowPassFilter() = default; + + float operator() (float x); + float Tf; //!< Low pass filter time constant + +protected: + unsigned long timestamp_prev; //!< Last execution timestamp + float y_prev; //!< filtered value in previous execution step +}; + +#endif // LOWPASS_FILTER_H \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/common/pid.cpp b/minimal_project_examples/atmega328_bldc_encoder/src/common/pid.cpp new file mode 100644 index 00000000..1a0a9828 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/common/pid.cpp @@ -0,0 +1,56 @@ +#include "pid.h" + +PIDController::PIDController(float P, float I, float D, float ramp, float limit) + : P(P) + , I(I) + , D(D) + , output_ramp(ramp) // output derivative limit [volts/second] + , limit(limit) // output supply limit [volts] + , integral_prev(0.0) + , error_prev(0.0) + , output_prev(0.0) +{ + timestamp_prev = _micros()*1e-6; +} + +// PID controller function +float PIDController::operator() (float error){ + // calculate the time from the last call + unsigned long timestamp_now = _micros(); + float Ts = (timestamp_now - timestamp_prev) * 1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + + // u(s) = (P + I/s + Ds)e(s) + // Discrete implementations + // proportional part + // u_p = P *e(k) + float proportional = P * error; + // Tustin transform of the integral part + // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) + float integral = integral_prev + I*Ts*0.5*(error + error_prev); + // antiwindup - limit the output voltage_q + integral = _constrain(integral, -limit, limit); + // Discrete derivation + // u_dk = D(ek - ek_1)/Ts + float derivative = D*(error - error_prev)/Ts; + + // sum all the components + float output = proportional + integral + derivative; + // antiwindup - limit the output variable + output = _constrain(output, -limit, limit); + + // limit the acceleration by ramping the output + float output_rate = (output - output_prev)/Ts; + if (output_rate > output_ramp) + output = output_prev + output_ramp*Ts; + else if (output_rate < -output_ramp) + output = output_prev - output_ramp*Ts; + + // saving for the next pass + integral_prev = integral; + output_prev = output; + error_prev = error; + timestamp_prev = timestamp_now; + return output; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/common/pid.h b/minimal_project_examples/atmega328_bldc_encoder/src/common/pid.h new file mode 100644 index 00000000..0e9ddf54 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/common/pid.h @@ -0,0 +1,40 @@ +#ifndef PID_H +#define PID_H + + +#include "time_utils.h" +#include "foc_utils.h" + +/** + * PID controller class + */ +class PIDController +{ +public: + /** + * + * @param P - Proportional gain + * @param I - Integral gain + * @param D - Derivative gain + * @param ramp - Maximum speed of change of the output value + * @param limit - Maximum output value + */ + PIDController(float P, float I, float D, float ramp, float limit); + ~PIDController() = default; + + float operator() (float error); + + float P; //!< Proportional gain + float I; //!< Integral gain + float D; //!< Derivative gain + float output_ramp; //!< Maximum speed of change of the output value + float limit; //!< Maximum output value + +protected: + float integral_prev; //!< last integral component value + float error_prev; //!< last tracking error value + float timestamp_prev; //!< Last execution timestamp + float output_prev; //!< last pid output value +}; + +#endif // PID_H \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/common/time_utils.cpp b/minimal_project_examples/atmega328_bldc_encoder/src/common/time_utils.cpp new file mode 100644 index 00000000..181d03cf --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/common/time_utils.cpp @@ -0,0 +1,31 @@ +#include "time_utils.h" + +// function buffering delay() +// arduino uno function doesn't work well with interrupts +void _delay(unsigned long ms){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) + // if arduino uno and other atmega328p chips + // use while instad of delay, + // due to wrong measurement based on changed timer0 + unsigned long t = _micros() + ms*1000; + while( _micros() < t ){}; +#else + // regular micros + delay(ms); +#endif +} + + +// function buffering _micros() +// arduino function doesn't work well with interrupts +unsigned long _micros(){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) +// if arduino uno and other atmega328p chips + //return the value based on the prescaler + if((TCCR0B & 0b00000111) == 0x01) return (micros()/32); + else return (micros()); +#else + // regular micros + return micros(); +#endif +} diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/common/time_utils.h b/minimal_project_examples/atmega328_bldc_encoder/src/common/time_utils.h new file mode 100644 index 00000000..143d4853 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/common/time_utils.h @@ -0,0 +1,22 @@ +#ifndef TIME_UTILS_H +#define TIME_UTILS_H + +#include "foc_utils.h" + +/** + * Function implementing delay() function in milliseconds + * - blocking function + * - hardware specific + + * @param ms number of milliseconds to wait + */ +void _delay(unsigned long ms); + +/** + * Function implementing timestamp getting function in microseconds + * hardware specific + */ +unsigned long _micros(); + + +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/drivers/BLDCDriver3PWM.cpp b/minimal_project_examples/atmega328_bldc_encoder/src/drivers/BLDCDriver3PWM.cpp new file mode 100644 index 00000000..bacb4ec5 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/drivers/BLDCDriver3PWM.cpp @@ -0,0 +1,70 @@ +#include "BLDCDriver3PWM.h" + +BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en){ + // Pin initialization + pwmA = phA; + pwmB = phB; + pwmC = phC; + + // enable_pin pin + enable_pin = en; + + // default power-supply value + voltage_power_supply = DEF_POWER_SUPPLY; + voltage_limit = NOT_SET; + +} + +// enable motor driver +void BLDCDriver3PWM::enable(){ + // enable_pin the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, HIGH); + // set zero to PWM + setPwm(0,0,0); +} + +// disable motor driver +void BLDCDriver3PWM::disable() +{ + // set zero to PWM + setPwm(0, 0, 0); + // disable the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, LOW); + +} + +// init hardware pins +int BLDCDriver3PWM::init() { + + // PWM pins + pinMode(pwmA, OUTPUT); + pinMode(pwmB, OUTPUT); + pinMode(pwmC, OUTPUT); + if(enable_pin != NOT_SET) pinMode(enable_pin, OUTPUT); + + + // sanity check for the voltage limit configuration + if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + + // Set the pwm frequency to the pins + // hardware specific function - depending on driver and mcu + _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC); + return 0; +} + + +// Set voltage to the pwm pin +void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { + // limit the voltage in driver + Ua = _constrain(Ua, -voltage_limit, voltage_limit); + Ub = _constrain(Ub, -voltage_limit, voltage_limit); + Uc = _constrain(Uc, -voltage_limit, voltage_limit); + // calculate duty cycle + // limited in [0,1] + float dc_a = _constrain(Ua / voltage_power_supply, 0 , 1 ); + float dc_b = _constrain(Ub / voltage_power_supply, 0 , 1 ); + float dc_c = _constrain(Uc / voltage_power_supply, 0 , 1 ); + // hardware specific writing + // hardware specific function - depending on driver and mcu + _writeDutyCycle3PWM(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC); +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/drivers/BLDCDriver3PWM.h b/minimal_project_examples/atmega328_bldc_encoder/src/drivers/BLDCDriver3PWM.h new file mode 100644 index 00000000..b6f7d7f8 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/drivers/BLDCDriver3PWM.h @@ -0,0 +1,52 @@ +#ifndef BLDCDriver3PWM_h +#define BLDCDriver3PWM_h + +#include "../common/base_classes/BLDCDriver.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "hardware_api.h" + +/** + 3 pwm bldc driver class +*/ +class BLDCDriver3PWM: public BLDCDriver +{ + public: + /** + BLDCDriver class constructor + @param phA A phase pwm pin + @param phB B phase pwm pin + @param phC C phase pwm pin + @param en enable pin (optional input) + */ + BLDCDriver3PWM(int phA,int phB,int phC, int en = NOT_SET); + + /** Motor hardware init function */ + int init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + // hardware variables + int pwmA; //!< phase A pwm pin number + int pwmB; //!< phase B pwm pin number + int pwmC; //!< phase C pwm pin number + int enable_pin; //!< enable pin number + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + void setPwm(float Ua, float Ub, float Uc) override; + + private: + +}; + + +#endif diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/drivers/hardware_api.h b/minimal_project_examples/atmega328_bldc_encoder/src/drivers/hardware_api.h new file mode 100644 index 00000000..da5ddf51 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/drivers/hardware_api.h @@ -0,0 +1,100 @@ +#ifndef HARDWARE_UTILS_H +#define HARDWARE_UTILS_H + +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + +/** + * Configuring PWM frequency, resolution and alignment + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA bldc driver + * @param pinB pinB bldc driver + * @param pinC pinC bldc driver + */ +void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC); + +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 4PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pin1A pin1A stepper driver + * @param pin1B pin1B stepper driver + * @param pin2A pin2A stepper driver + * @param pin2B pin2B stepper driver + */ +void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B); + +/** + * Configuring PWM frequency, resolution and alignment + * - BLDC driver - 6PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low - if applicable + * @param pinA_h pinA high-side bldc driver + * @param pinA_l pinA low-side bldc driver + * @param pinB_h pinA high-side bldc driver + * @param pinB_l pinA low-side bldc driver + * @param pinC_h pinA high-side bldc driver + * @param pinC_l pinA low-side bldc driver + * + * @return 0 if config good, -1 if failed + */ +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param pinA phase A hardware pin number + * @param pinB phase B hardware pin number + * @param pinC phase C hardware pin number + */ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 4PWM setting + * - hardware specific + * + * @param dc_1a duty cycle phase 1A [0, 1] + * @param dc_1b duty cycle phase 1B [0, 1] + * @param dc_2a duty cycle phase 2A [0, 1] + * @param dc_2b duty cycle phase 2B [0, 1] + * @param pin1A phase 1A hardware pin number + * @param pin1B phase 1B hardware pin number + * @param pin2A phase 2A hardware pin number + * @param pin2B phase 2B hardware pin number + */ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B); + + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 6PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low + * @param pinA_h phase A high-side hardware pin number + * @param pinA_l phase A low-side hardware pin number + * @param pinB_h phase B high-side hardware pin number + * @param pinB_l phase B low-side hardware pin number + * @param pinC_h phase C high-side hardware pin number + * @param pinC_l phase C low-side hardware pin number + * + */ +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l); + +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/drivers/hardware_specific/atmega328_mcu.cpp b/minimal_project_examples/atmega328_bldc_encoder/src/drivers/hardware_specific/atmega328_mcu.cpp new file mode 100644 index 00000000..58b368f7 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/drivers/hardware_specific/atmega328_mcu.cpp @@ -0,0 +1,133 @@ +#include "../hardware_api.h" + +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) + +// set pwm frequency to 32KHz +void _pinHighFrequency(const int pin){ + // High PWM frequency + // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328 + if (pin == 5 || pin == 6 ) { + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 + } + if (pin == 9 || pin == 10 ) + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 + if (pin == 3 || pin == 11) + TCCR2B = ((TCCR2B & 0b11111000) | 0x01);// set prescaler to 1 + +} + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); + _pinHighFrequency(pinC); +} + + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); + analogWrite(pinC, 255.0*dc_c); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pin1A); + _pinHighFrequency(pin1B); + _pinHighFrequency(pin2A); + _pinHighFrequency(pin2B); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0*dc_1a); + analogWrite(pin1B, 255.0*dc_1b); + analogWrite(pin2A, 255.0*dc_2a); + analogWrite(pin2B, 255.0*dc_2b); +} + + +// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm +int _configureComplementaryPair(int pinH, int pinL) { + if( (pinH == 5 && pinL == 6 ) || (pinH == 6 && pinL == 5 ) ){ + // configure the pwm phase-corrected mode + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); + // configure complementary pwm on low side + if(pinH == 6 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ; + else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ; + // set prescaler to 1 - 32kHz freq + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); + }else if( (pinH == 9 && pinL == 10 ) || (pinH == 10 && pinL == 9 ) ){ + // set prescaler to 1 - 32kHz freq + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); + // configure complementary pwm on low side + if(pinH == 9 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ; + else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ; + }else if((pinH == 3 && pinL == 11 ) || (pinH == 11 && pinL == 3 ) ){ + // set prescaler to 1 - 32kHz freq + TCCR2B = ((TCCR2B & 0b11111000) | 0x01); + // configure complementary pwm on low side + if(pinH == 11 ) TCCR2A = 0b10110000 | (TCCR2A & 0b00001111) ; + else TCCR2A = 0b11100000 | (TCCR2A & 0b00001111) ; + }else{ + return -1; + } + return 0; +} + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arudino/ATmega328 +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + // High PWM frequency + // - always max 32kHz + int ret_flag = 0; + ret_flag += _configureComplementaryPair(pinA_h, pinA_l); + ret_flag += _configureComplementaryPair(pinB_h, pinB_l); + ret_flag += _configureComplementaryPair(pinC_h, pinC_l); + return ret_flag; // returns -1 if not well configured +} + +// function setting the +void _setPwmPair(int pinH, int pinL, float val, int dead_time) +{ + int pwm_h = _constrain(val-dead_time/2,0,255); + int pwm_l = _constrain(val+dead_time/2,0,255); + + analogWrite(pinH, pwm_h); + if(pwm_l == 255 || pwm_l == 0) + digitalWrite(pinL, pwm_l ? LOW : HIGH); + else + analogWrite(pinL, pwm_l); +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arudino/ATmega328 +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); + _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); + _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0); +} + +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/drivers/hardware_specific/esp32_mcu.cpp b/minimal_project_examples/atmega328_bldc_encoder/src/drivers/hardware_specific/esp32_mcu.cpp new file mode 100644 index 00000000..3c07a445 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/drivers/hardware_specific/esp32_mcu.cpp @@ -0,0 +1,296 @@ +#include "../hardware_api.h" + +#if defined(ESP_H) + +#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 + +// 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_motor_slots_t; + +typedef struct { + int pinAH; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator1; + mcpwm_operator_t mcpwm_operator2; + mcpwm_io_signals_t mcpwm_ah; + mcpwm_io_signals_t mcpwm_bh; + mcpwm_io_signals_t mcpwm_ch; + mcpwm_io_signals_t mcpwm_al; + mcpwm_io_signals_t mcpwm_bl; + mcpwm_io_signals_t mcpwm_cl; +} bldc_6pwm_motor_slots_t; + +// define bldc motor slots array +bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}, // 2nd motor will be MCPWM0 channel B + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 3rd motor will be MCPWM1 channel A + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B} // 4th motor will be MCPWM1 channel B + }; + +// define stepper motor slots array +stepper_motor_slots_t esp32_stepper_motor_slots[2] = { + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0 + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1 + }; + +// define stepper motor slots array +bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 + }; + +// configuring high frequency pwm timer +// a lot of help from this post from Paul Gauld +// https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller +void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit, float dead_zone = NOT_SET){ + + mcpwm_config_t pwm_config; + pwm_config.frequency = pwm_frequency; //frequency + pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave) + pwm_config.duty_mode = MCPWM_DUTY_MODE_0; // Active HIGH + mcpwm_init(mcpwm_unit, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings + mcpwm_init(mcpwm_unit, MCPWM_TIMER_1, &pwm_config); //Configure PWM0A & PWM0B with above settings + mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &pwm_config); //Configure PWM0A & PWM0B with above settings + + if (dead_zone != NOT_SET){ + // dead zone is configured in dead_time x 100 nanoseconds + float dead_time = (float)(1e7 / pwm_frequency) * dead_zone; + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time, dead_time); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time, dead_time); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time, dead_time); + } + + _delay(100); + + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SELECT_SYNC_INT0, 0); + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_SELECT_SYNC_INT0, 0); + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_SELECT_SYNC_INT0, 0); + _delay(1); + mcpwm_num->timer[0].sync.out_sel = 1; + _delay(1); + mcpwm_num->timer[0].sync.out_sel = 0; +} + + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// supports Arudino/ATmega328, STM32 and ESP32 +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency + else pwm_frequency = _constrain(2*pwm_frequency, 0, 100000); // constrain to 50kHz max - centered pwm has twice lower frequency + + bldc_3pwm_motor_slots_t m_slot = {}; + + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 4; slot_num++){ + if(esp32_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_bldc_3pwm_motor_slots[slot_num].pinA = pinA; + m_slot = esp32_bldc_3pwm_motor_slots[slot_num]; + break; + } + } + // disable all the slots with the same MCPWM + if( slot_num < 2 ){ + // slot 0 of the stepper + esp32_stepper_motor_slots[0].pin1A = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + }else{ + // slot 1 of the stepper + esp32_stepper_motor_slots[1].pin1A = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + } + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_c, pinC); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency + else pwm_frequency = _constrain(2*pwm_frequency, 0, 100000); // constrain to 50kHz max - centered pwm has twice lower frequency + stepper_motor_slots_t m_slot = {}; + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 2; slot_num++){ + if(esp32_stepper_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_stepper_motor_slots[slot_num].pin1A = pinA; + m_slot = esp32_stepper_motor_slots[slot_num]; + break; + } + } + // disable all the slots with the same MCPWM + if( slot_num == 0 ){ + // slots 0 and 1 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + }else{ + // slots 2 and 3 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + } + + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1a, pinA); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1b, pinB); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2a, pinC); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2b, pinD); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +// ESP32 uses MCPWM +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // determine which motor slot is the motor connected to + for(int i = 0; i < 4; i++){ + if(esp32_bldc_3pwm_motor_slots[i].pinA == pinA){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100] + mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_a*100.0); + mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_b*100.0); + mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_c*100.0); + break; + } + } +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +// ESP32 uses MCPWM +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // determine which motor slot is the motor connected to + for(int i = 0; i < 2; i++){ + if(esp32_stepper_motor_slots[i].pin1A == pin1A){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100] + mcpwm_set_duty(esp32_stepper_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_motor_slots[i].mcpwm_operator1, dc_1a*100.0); + mcpwm_set_duty(esp32_stepper_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_motor_slots[i].mcpwm_operator1, dc_1b*100.0); + mcpwm_set_duty(esp32_stepper_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_motor_slots[i].mcpwm_operator2, dc_2a*100.0); + mcpwm_set_duty(esp32_stepper_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_motor_slots[i].mcpwm_operator2, dc_2b*100.0); + break; + } + } +} + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 40000; // default frequency 20khz - centered pwm has twice lower frequency + else pwm_frequency = _constrain(2*pwm_frequency, 0, 60000); // constrain to 30kHz max - centered pwm has twice lower frequency + bldc_6pwm_motor_slots_t m_slot = {}; + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 2; slot_num++){ + if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h; + m_slot = esp32_bldc_6pwm_motor_slots[slot_num]; + break; + } + } + // if no slots available + if(slot_num >= 2) return -1; + + // disable all the slots with the same MCPWM + if( slot_num == 0 ){ + // slots 0 and 1 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + esp32_stepper_motor_slots[0].pin1A = _TAKEN_SLOT; + }else{ + // slots 2 and 3 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + esp32_stepper_motor_slots[1].pin1A = _TAKEN_SLOT; + } + + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_al, pinA_l); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bh, pinB_h); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bl, pinB_l); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ch, pinC_h); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_cl, pinC_l); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit, dead_zone); + // return + return 0; +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + // determine which motor slot is the motor connected to + for(int i = 0; i < 2; i++){ + if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100.0] + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0); + break; + } + } +} +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/drivers/hardware_specific/generic_mcu.cpp b/minimal_project_examples/atmega328_bldc_encoder/src/drivers/hardware_specific/generic_mcu.cpp new file mode 100644 index 00000000..dc322746 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/drivers/hardware_specific/generic_mcu.cpp @@ -0,0 +1,68 @@ +#include "../hardware_api.h" + +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) // if mcu is not atmega328 + +#elif defined(__arm__) && defined(CORE_TEENSY) // or teensy + +#elif defined(ESP_H) // or esp32 + +#elif defined(_STM32_DEF_) // or stm32 + +#else + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// in generic case dont do anything +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + return; +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +// in generic case dont do anything +void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + return; +} + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + return -1; +} + + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); + analogWrite(pinC, 255.0*dc_c); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0*dc_1a); + analogWrite(pin1B, 255.0*dc_1b); + analogWrite(pin2A, 255.0*dc_2a); + analogWrite(pin2B, 255.0*dc_2b); +} + + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + return; +} + + +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/drivers/hardware_specific/stm32_mcu.cpp b/minimal_project_examples/atmega328_bldc_encoder/src/drivers/hardware_specific/stm32_mcu.cpp new file mode 100644 index 00000000..85d0bd65 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/drivers/hardware_specific/stm32_mcu.cpp @@ -0,0 +1,291 @@ + +#include "../hardware_api.h" + +#if defined(_STM32_DEF_) + +#define _PWM_RESOLUTION 12 // 12bit +#define _PWM_RANGE 4095.0// 2^12 -1 = 4095 +#define _PWM_FREQUENCY 50000 // 50khz + + +#define _HARDWARE_6PWM 1 +#define _SOFTWARE_6PWM 0 +#define _ERROR_6PWM -1 + + +// setting pwm to hardware pin - instead analogWrite() +void _setPwm(int ulPin, uint32_t value, int resolution) +{ + PinName pin = digitalPinToPinName(ulPin); + TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); + uint32_t index = get_timer_index(Instance); + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + + uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + HT->setCaptureCompare(channel, value, (TimerCompareFormat_t)resolution); +} + + +// init pin pwm +HardwareTimer* _initPinPWM(uint32_t PWM_freq, int ulPin) +{ + PinName pin = digitalPinToPinName(ulPin); + TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); + + uint32_t index = get_timer_index(Instance); + if (HardwareTimer_Handle[index] == NULL) { + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); + HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); + } + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, pin); + HT->setOverflow(PWM_freq, HERTZ_FORMAT); + HT->pause(); + HT->refresh(); + return HT; +} + + +// init high side pin +HardwareTimer* _initPinPWMHigh(uint32_t PWM_freq, int ulPin) +{ + return _initPinPWM(PWM_freq, ulPin); +} + +// init low side pin +HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) +{ + PinName pin = digitalPinToPinName(ulPin); + TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); + uint32_t index = get_timer_index(Instance); + + if (HardwareTimer_Handle[index] == NULL) { + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); + HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); + TIM_OC_InitTypeDef sConfigOC = TIM_OC_InitTypeDef(); + sConfigOC.OCMode = TIM_OCMODE_PWM2; + sConfigOC.Pulse = 100; + sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; + sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; + sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; + sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET; + sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; + uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel); + } + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, pin); + HT->setOverflow(PWM_freq, HERTZ_FORMAT); + HT->pause(); + HT->refresh(); + return HT; +} + + +// align the timers to end the init +void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3) +{ + HT1->pause(); + HT1->refresh(); + HT2->pause(); + HT2->refresh(); + HT3->pause(); + HT3->refresh(); + HT1->resume(); + HT2->resume(); + HT3->resume(); +} + +// align the timers to end the init +void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3,HardwareTimer *HT4) +{ + HT1->pause(); + HT1->refresh(); + HT2->pause(); + HT2->refresh(); + HT3->pause(); + HT3->refresh(); + HT4->pause(); + HT4->refresh(); + HT1->resume(); + HT2->resume(); + HT3->resume(); + HT4->resume(); +} + +HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) +{ + PinName uhPinName = digitalPinToPinName(pinA_h); + PinName ulPinName = digitalPinToPinName(pinA_l); + PinName vhPinName = digitalPinToPinName(pinB_h); + PinName vlPinName = digitalPinToPinName(pinB_l); + PinName whPinName = digitalPinToPinName(pinC_h); + PinName wlPinName = digitalPinToPinName(pinC_l); + + TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM); + + uint32_t index = get_timer_index(Instance); + + if (HardwareTimer_Handle[index] == NULL) { + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM)); + HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); + ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT); + } + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + + HT->setMode(STM_PIN_CHANNEL(pinmap_function(uhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, uhPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(ulPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, ulPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(vhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vhPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(vlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vlPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(whPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, whPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, wlPinName); + + // dead time is set in nanoseconds + uint32_t dead_time_ns = (float)(1e9/PWM_freq)*dead_zone; + uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns); + LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! + LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N); + + HT->pause(); + HT->refresh(); + HT->resume(); + return HT; +} + + +// returns 0 if each pair of pwm channels has same channel +// returns 1 all the channels belong to the same timer - hardware inverted channels +// returns -1 if neither - avoid configuring - error!!! +int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + PinName nameAH = digitalPinToPinName(pinA_h); + PinName nameAL = digitalPinToPinName(pinA_l); + PinName nameBH = digitalPinToPinName(pinB_h); + PinName nameBL = digitalPinToPinName(pinB_l); + PinName nameCH = digitalPinToPinName(pinC_h); + PinName nameCL = digitalPinToPinName(pinC_l); + int tim1 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAH, PinMap_PWM)); + int tim2 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAL, PinMap_PWM)); + int tim3 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBH, PinMap_PWM)); + int tim4 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBL, PinMap_PWM)); + int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM)); + int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM)); + if(tim1 == tim2 && tim2==tim3 && tim3==tim4 && tim4==tim5 && tim5==tim6) + return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer + else if(tim1 == tim2 && tim3==tim4 && tim5==tim6) + return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer + else + return _ERROR_6PWM; // might be error avoid configuration +} + + + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY); // constrain to 50kHz max + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); + HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); + HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); + // allign the timers + _alignPWMTimers(HT1, HT2, HT3); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY); // constrain to 50kHz max + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); + HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); + HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); + HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinD); + // allign the timers + _alignPWMTimers(HT1, HT2, HT3, HT4); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +//- hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,4095] + _setPwm(pinA, _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(pinB, _PWM_RANGE*dc_b, _PWM_RESOLUTION); + _setPwm(pinC, _PWM_RANGE*dc_c, _PWM_RESOLUTION); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +//- hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,4095] + _setPwm(pin1A, _PWM_RANGE*dc_1a, _PWM_RESOLUTION); + _setPwm(pin1B, _PWM_RANGE*dc_1b, _PWM_RESOLUTION); + _setPwm(pin2A, _PWM_RANGE*dc_2a, _PWM_RESOLUTION); + _setPwm(pin2B, _PWM_RANGE*dc_2b, _PWM_RESOLUTION); +} + + + + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, 100000); // constrain to 100kHz max + + // find configuration + int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); + // configure accordingly + switch(config){ + case _ERROR_6PWM: + return -1; // fail + break; + case _HARDWARE_6PWM: + _initHardware6PWMInterface(pwm_frequency, dead_zone, pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); + break; + case _SOFTWARE_6PWM: + HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinA_h); + _initPinPWMLow(pwm_frequency, pinA_l); + HardwareTimer* HT2 = _initPinPWMHigh(pwm_frequency,pinB_h); + _initPinPWMLow(pwm_frequency, pinB_l); + HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency,pinC_h); + _initPinPWMLow(pwm_frequency, pinC_l); + _alignPWMTimers(HT1, HT2, HT3); + break; + } + return 0; // success +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + // find configuration + int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); + // set pwm accordingly + switch(config){ + case _HARDWARE_6PWM: + _setPwm(pinA_h, _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(pinB_h, _PWM_RANGE*dc_b, _PWM_RESOLUTION); + _setPwm(pinC_h, _PWM_RANGE*dc_c, _PWM_RESOLUTION); + break; + case _SOFTWARE_6PWM: + _setPwm(pinA_l, _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinA_h, _constrain(dc_a - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinB_l, _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinB_h, _constrain(dc_b - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinC_l, _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinC_h, _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + break; + } +} +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/drivers/hardware_specific/teensy_mcu.cpp b/minimal_project_examples/atmega328_bldc_encoder/src/drivers/hardware_specific/teensy_mcu.cpp new file mode 100644 index 00000000..10bd24c5 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/drivers/hardware_specific/teensy_mcu.cpp @@ -0,0 +1,71 @@ +#include "../hardware_api.h" + +#if defined(__arm__) && defined(CORE_TEENSY) + +// configure High PWM frequency +void _setHighFrequency(const long freq, const int pin){ + analogWrite(pin, 0); + analogWriteFrequency(pin, freq); +} + + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); + _setHighFrequency(pwm_frequency, pinC); + if(pinD != NOT_SET) _setHighFrequency(pwm_frequency, pinD); // stepper motor +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 50000; // default frequency 50khz + else pwm_frequency = _constrain(pwm_frequency, 0, 50000); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); + _setHighFrequency(pwm_frequency, pinC); + _setHighFrequency(pwm_frequency, pinD); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); + analogWrite(pinC, 255.0*dc_c); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0*dc_1a); + analogWrite(pin1B, 255.0*dc_1b); + analogWrite(pin2A, 255.0*dc_2a); + analogWrite(pin2B, 255.0*dc_2b); +} + + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + return -1; +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + return; +} +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/sensors/Encoder.cpp b/minimal_project_examples/atmega328_bldc_encoder/src/sensors/Encoder.cpp new file mode 100644 index 00000000..1965c015 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/sensors/Encoder.cpp @@ -0,0 +1,228 @@ +#include "Encoder.h" + + +/* + Encoder(int encA, int encB , int cpr, int index) + - encA, encB - encoder A and B pins + - cpr - counts per rotation number (cpm=ppm*4) + - index pin - (optional input) +*/ + +Encoder::Encoder(int _encA, int _encB , float _ppr, int _index){ + + // Encoder measurement structure init + // hardware pins + pinA = _encA; + pinB = _encB; + // counter setup + pulse_counter = 0; + pulse_timestamp = 0; + + cpr = _ppr; + A_active = 0; + B_active = 0; + I_active = 0; + // index pin + index_pin = _index; // its 0 if not used + index_pulse_counter = 0; + + // velocity calculation variables + prev_Th = 0; + pulse_per_second = 0; + prev_pulse_counter = 0; + prev_timestamp_us = _micros(); + + // extern pullup as default + pullup = Pullup::EXTERN; + // enable quadrature encoder by default + quadrature = Quadrature::ON; +} + +// Encoder interrupt callback functions +// A channel +void Encoder::handleA() { + int A = digitalRead(pinA); + switch (quadrature){ + case Quadrature::ON: + // CPR = 4xPPR + if ( A != A_active ) { + pulse_counter += (A_active == B_active) ? 1 : -1; + pulse_timestamp = _micros(); + A_active = A; + } + break; + case Quadrature::OFF: + // CPR = PPR + if(A && !digitalRead(pinB)){ + pulse_counter++; + pulse_timestamp = _micros(); + } + break; + } +} +// B channel +void Encoder::handleB() { + int B = digitalRead(pinB); + switch (quadrature){ + case Quadrature::ON: + // // CPR = 4xPPR + if ( B != B_active ) { + pulse_counter += (A_active != B_active) ? 1 : -1; + pulse_timestamp = _micros(); + B_active = B; + } + break; + case Quadrature::OFF: + // CPR = PPR + if(B && !digitalRead(pinA)){ + pulse_counter--; + pulse_timestamp = _micros(); + } + break; + } +} + +// Index channel +void Encoder::handleIndex() { + if(hasIndex()){ + int I = digitalRead(index_pin); + if(I && !I_active){ + // align encoder on each index + if(index_pulse_counter){ + long tmp = pulse_counter; + // corrent the counter value + pulse_counter = round((double)pulse_counter/(double)cpr)*cpr; + // preserve relative speed + prev_pulse_counter += pulse_counter - tmp; + } else { + // initial offset + index_pulse_counter = pulse_counter; + } + } + I_active = I; + } +} + +/* + Shaft angle calculation +*/ +float Encoder::getAngle(){ + return natural_direction * _2PI * (pulse_counter) / ((float)cpr); +} +/* + Shaft velocity calculation + function using mixed time and frequency measurement technique +*/ +float Encoder::getVelocity(){ + // timestamp + long timestamp_us = _micros(); + // sampling time calculation + float Ts = (timestamp_us - prev_timestamp_us) * 1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + + // time from last impulse + float Th = (timestamp_us - pulse_timestamp) * 1e-6; + long dN = pulse_counter - prev_pulse_counter; + + // Pulse per second calculation (Eq.3.) + // dN - impulses received + // Ts - sampling time - time in between function calls + // Th - time from last impulse + // Th_1 - time form last impulse of the previous call + // only increment if some impulses received + float dt = Ts + prev_Th - Th; + pulse_per_second = (dN != 0 && dt > Ts/2) ? dN / dt : pulse_per_second; + + // if more than 0.05 passed in between impulses + if ( Th > 0.1) pulse_per_second = 0; + + // velocity calculation + float velocity = pulse_per_second / ((float)cpr) * (_2PI); + + // save variables for next pass + prev_timestamp_us = timestamp_us; + // save velocity calculation variables + prev_Th = Th; + prev_pulse_counter = pulse_counter; + return natural_direction * (velocity); +} + +// getter for index pin +// return -1 if no index +int Encoder::needsAbsoluteZeroSearch(){ + return index_pulse_counter == 0; +} +// getter for index pin +int Encoder::hasAbsoluteZero(){ + return hasIndex(); +} +// initialize counter to zero +float Encoder::initRelativeZero(){ + long angle_offset = -pulse_counter; + pulse_counter = 0; + pulse_timestamp = _micros(); + return _2PI * (angle_offset) / ((float)cpr); +} +// initialize index to zero +float Encoder::initAbsoluteZero(){ + pulse_counter -= index_pulse_counter; + prev_pulse_counter = pulse_counter; + return (index_pulse_counter) / ((float)cpr) * (_2PI); +} +// private function used to determine if encoder has index +int Encoder::hasIndex(){ + return index_pin != 0; +} + + +// encoder initialisation of the hardware pins +// and calculation variables +void Encoder::init(){ + + // Encoder - check if pullup needed for your encoder + if(pullup == Pullup::INTERN){ + pinMode(pinA, INPUT_PULLUP); + pinMode(pinB, INPUT_PULLUP); + if(hasIndex()) pinMode(index_pin,INPUT_PULLUP); + }else{ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if(hasIndex()) pinMode(index_pin,INPUT); + } + + // counter setup + pulse_counter = 0; + pulse_timestamp = _micros(); + // velocity calculation variables + prev_Th = 0; + pulse_per_second = 0; + prev_pulse_counter = 0; + prev_timestamp_us = _micros(); + + // initial cpr = PPR + // change it if the mode is quadrature + if(quadrature == Quadrature::ON) cpr = 4*cpr; + +} + +// function enabling hardware interrupts of the for the callback provided +// if callback is not provided then the interrupt is not enabled +void Encoder::enableInterrupts(void (*doA)(), void(*doB)(), void(*doIndex)()){ + // attach interrupt if functions provided + switch(quadrature){ + case Quadrature::ON: + // A callback and B callback + if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE); + if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE); + break; + case Quadrature::OFF: + // A callback and B callback + if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, RISING); + if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, RISING); + break; + } + + // if index used initialize the index interrupt + if(hasIndex() && doIndex != nullptr) attachInterrupt(digitalPinToInterrupt(index_pin), doIndex, CHANGE); +} diff --git a/minimal_project_examples/atmega328_bldc_encoder/src/sensors/Encoder.h b/minimal_project_examples/atmega328_bldc_encoder/src/sensors/Encoder.h new file mode 100644 index 00000000..29122888 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_encoder/src/sensors/Encoder.h @@ -0,0 +1,105 @@ +#ifndef ENCODER_LIB_H +#define ENCODER_LIB_H + +#include "Arduino.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/base_classes/Sensor.h" + + +/** + * Quadrature mode configuration structure + */ +enum Quadrature{ + ON, //!< Enable quadrature mode CPR = 4xPPR + OFF //!< Disable quadrature mode / CPR = PPR +}; + +class Encoder: public Sensor{ + public: + /** + Encoder class constructor + @param encA encoder B pin + @param encB encoder B pin + @param ppr impulses per rotation (cpr=ppr*4) + @param index index pin number (optional input) + */ + Encoder(int encA, int encB , float ppr, int index = 0); + + /** encoder initialise pins */ + void init(); + /** + * function enabling hardware interrupts for the encoder channels with provided callback functions + * if callback is not provided then the interrupt is not enabled + * + * @param doA pointer to the A channel interrupt handler function + * @param doB pointer to the B channel interrupt handler function + * @param doIndex pointer to the Index channel interrupt handler function + * + */ + void enableInterrupts(void (*doA)() = nullptr, void(*doB)() = nullptr, void(*doIndex)() = nullptr); + + // Encoder interrupt callback functions + /** A channel callback function */ + void handleA(); + /** B channel callback function */ + void handleB(); + /** Index channel callback function */ + void handleIndex(); + + + // pins A and B + int pinA; //!< encoder hardware pin A + int pinB; //!< encoder hardware pin B + int index_pin; //!< index pin + + // Encoder configuration + Pullup pullup; //!< Configuration parameter internal or external pullups + Quadrature quadrature;//!< Configuration parameter enable or disable quadrature mode + float cpr;//!< encoder cpr number + + // Abstract functions of the Sensor class implementation + /** get current angle (rad) */ + float getAngle() override; + /** get current angular velocity (rad/s) */ + float getVelocity() override; + /** + * set current angle as zero angle + * return the angle [rad] difference + */ + float initRelativeZero() override; + /** + * set index angle as zero angle + * return the angle [rad] difference + */ + float initAbsoluteZero() override; + /** + * returns 0 if it has no index + * 0 - encoder without index + * 1 - encoder with index + */ + int hasAbsoluteZero() override; + /** + * returns 0 if it does need search for absolute zero + * 0 - encoder without index + * 1 - ecoder with index + */ + int needsAbsoluteZeroSearch() override; + + private: + int hasIndex(); //!< function returning 1 if encoder has index pin and 0 if not. + + volatile long pulse_counter;//!< current pulse counter + volatile long pulse_timestamp;//!< last impulse timestamp in us + volatile int A_active; //!< current active states of A channel + volatile int B_active; //!< current active states of B channel + volatile int I_active; //!< current active states of Index channel + volatile long index_pulse_counter; //!< impulse counter number upon first index interrupt + + // velocity calculation variables + float prev_Th, pulse_per_second; + volatile long prev_pulse_counter, prev_timestamp_us; +}; + + +#endif diff --git a/minimal_project_examples/atmega328_bldc_magnetic_i2c/atmega328_bldc_magnetic_i2c.ino b/minimal_project_examples/atmega328_bldc_magnetic_i2c/atmega328_bldc_magnetic_i2c.ino new file mode 100644 index 00000000..cf1142c5 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_magnetic_i2c/atmega328_bldc_magnetic_i2c.ino @@ -0,0 +1,145 @@ +/** + * Comprehensive BLDC motor control example using magnetic sensor + * + * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: + * - configure PID controller constants + * - change motion control loops + * - monitor motor variabels + * - set target values + * - check all the configuration values + * + * To check the config value just enter the command letter. + * For example: - to read velocity PI controller P gain run: P + * - to set velocity PI controller P gain to 1.2 run: P1.2 + * + * To change the target value just enter a number in the terminal: + * For example: - to change the target value to -0.1453 enter: -0.1453 + * - to get the current target value enter: V3 + * + * List of commands: + * - P: velocity PID controller P gain + * - I: velocity PID controller I gain + * - D: velocity PID controller D gain + * - R: velocity PID controller voltage ramp + * - F: velocity Low pass filter time constant + * - K: angle P controller P gain + * - N: angle P controller velocity limit + * - L: system voltage limit + * - C: control loop + * - 0: voltage + * - 1: velocity + * - 2: angle + * - V: get motor variables + * - 0: currently set voltage + * - 1: current velocity + * - 2: current angle + * - 3: current target value + * + */ +#include "src/BLDCMotor.h" +#include "src/drivers/BLDCDriver3PWM.h" +#include "src/sensors/MagneticSensorI2C.h" + +// I2C magnetic sensor instance +MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0x0E, 4); + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); + +void setup() { + + // initialise magnetic sensor hardware + sensor.init(); + // link the motor to the sensor + motor.linkSensor(&sensor); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link driver + motor.linkDriver(&driver); + + // choose FOC modulation + motor.foc_modulation = FOCModulationType::SpaceVectorPWM; + + // set control loop type to be used + motor.controller = ControlType::voltage; + + // contoller configuration based on the control type + motor.PID_velocity.P = 0.2; + motor.PID_velocity.I = 20; + motor.PID_velocity.D = 0; + // default voltage_power_supply + motor.voltage_limit = 12; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01; + + // angle loop controller + motor.P_angle.P = 20; + // angle loop velocity limit + motor.velocity_limit = 50; + + // use monitoring with serial for motor init + // monitoring port + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialise motor + motor.init(); + // align encoder and start FOC + motor.initFOC(); + + // set the inital target value + motor.target = 2; + + + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + Serial.println("Motor commands sketch | Initial motion control > torque/voltage : target 2V."); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor.loopFOC(); + + // iterative function setting the outter loop target + // velocity, position or voltage + // if tatget not set in parameter uses motor.target variable + motor.move(); + + // user communication + motor.command(serialReceiveUserCommand()); +} + +// utility function enabling serial communication the user +String serialReceiveUserCommand() { + + // a string to hold incoming data + static String received_chars; + + String command = ""; + + while (Serial.available()) { + // get the new byte: + char inChar = (char)Serial.read(); + // add it to the string buffer: + received_chars += inChar; + + // end of user input + if (inChar == '\n') { + + // execute the user command + command = received_chars; + + // reset the command buffer + received_chars = ""; + } + } + return command; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/BLDCMotor.cpp b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/BLDCMotor.cpp new file mode 100644 index 00000000..bed4cd6a --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/BLDCMotor.cpp @@ -0,0 +1,419 @@ +#include "BLDCMotor.h" + +// // BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en) +// // - phA, phB, phC - motor A,B,C phase pwm pins +// // - pp - pole pair number +// // - cpr - counts per rotation number (cpm=ppm*4) +// // - enable pin - (optional input) +// BLDCMotor::BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en){ +// noop; +// } + +// BLDCMotor( int pp) +// - phA, phB, phC - motor A,B,C phase pwm pins +// - pp - pole pair number +// - cpr - counts per rotation number (cpm=ppm*4) +// - enable pin - (optional input) +BLDCMotor::BLDCMotor(int pp) +: FOCMotor() +{ + // save pole pairs number + pole_pairs = pp; +} + + +/** + Link the driver which controls the motor +*/ +void BLDCMotor::linkDriver(BLDCDriver* _driver) { + driver = _driver; +} + +// init hardware pins +void BLDCMotor::init() { + if(monitor_port) monitor_port->println("MOT: Initialise variables."); + // sanity check for the voltage limit configuration + if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; + // constrain voltage for sensor alignment + if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; + // update the controller limits + PID_velocity.limit = voltage_limit; + P_angle.limit = velocity_limit; + + _delay(500); + // enable motor + if(monitor_port) monitor_port->println("MOT: Enable driver."); + enable(); + _delay(500); +} + + +// disable motor driver +void BLDCMotor::disable() +{ + // set zero to PWM + driver->setPwm(0, 0, 0); + // disable the driver + driver->disable(); +} +// enable motor driver +void BLDCMotor::enable() +{ + // enable the driver + driver->enable(); + // set zero to PWM + driver->setPwm(0, 0, 0); + +} + +/** + FOC functions +*/ +// FOC initialization function +int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction ) { + int exit_flag = 1; + // align motor if necessary + // alignment necessary for encoders! + if(zero_electric_offset != NOT_SET){ + // abosolute zero offset provided - no need to align + zero_electric_angle = zero_electric_offset; + // set the sensor direction - default CW + sensor->natural_direction = sensor_direction; + }else{ + // sensor and motor alignment + _delay(500); + exit_flag = alignSensor(); + _delay(500); + } + if(monitor_port) monitor_port->println("MOT: Motor ready."); + + return exit_flag; +} +// Encoder alignment to electrical 0 angle +int BLDCMotor::alignSensor() { + if(monitor_port) monitor_port->println("MOT: Align sensor."); + // align the electrical phases of the motor and sensor + // set angle -90 degrees + + float start_angle = shaftAngle(); + for (int i = 0; i <=5; i++ ) { + float angle = _3PI_2 + _2PI * i / 6.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(200); + } + float mid_angle = shaftAngle(); + for (int i = 5; i >=0; i-- ) { + float angle = _3PI_2 + _2PI * i / 6.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(200); + } + if (mid_angle < start_angle) { + if(monitor_port) monitor_port->println("MOT: natural_direction==CCW"); + sensor->natural_direction = Direction::CCW; + } else if (mid_angle == start_angle) { + if(monitor_port) monitor_port->println("MOT: Sensor failed to notice movement"); + } + + // let the motor stabilize for 2 sec + _delay(2000); + // set sensor to zero + sensor->initRelativeZero(); + _delay(500); + setPhaseVoltage(0, 0, 0); + _delay(200); + + // find the index if available + int exit_flag = absoluteZeroAlign(); + _delay(500); + if(monitor_port){ + if(exit_flag< 0 ) monitor_port->println("MOT: Error: Not found!"); + if(exit_flag> 0 ) monitor_port->println("MOT: Success!"); + else monitor_port->println("MOT: Not available!"); + } + return exit_flag; +} + + +// Encoder alignment the absolute zero angle +// - to the index +int BLDCMotor::absoluteZeroAlign() { + + if(monitor_port) monitor_port->println("MOT: Absolute zero align."); + // if no absolute zero return + if(!sensor->hasAbsoluteZero()) return 0; + + + if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println("MOT: Searching..."); + // search the absolute zero with small velocity + while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){ + loopFOC(); + voltage_q = PID_velocity(velocity_index_search - shaftVelocity()); + } + voltage_q = 0; + // disable motor + setPhaseVoltage(0, 0, 0); + + // align absolute zero if it has been found + if(!sensor->needsAbsoluteZeroSearch()){ + // align the sensor with the absolute zero + float zero_offset = sensor->initAbsoluteZero(); + // remember zero electric angle + zero_electric_angle = _normalizeAngle(_electricalAngle(zero_offset, pole_pairs)); + } + // return bool if zero found + return !sensor->needsAbsoluteZeroSearch() ? 1 : -1; +} + +// Iterative function looping FOC algorithm, setting Uq on the Motor +// The faster it can be run the better +void BLDCMotor::loopFOC() { + // shaft angle + shaft_angle = shaftAngle(); + // set the phase voltage - FOC heart function :) + setPhaseVoltage(voltage_q, voltage_d, _electricalAngle(shaft_angle,pole_pairs)); +} + +// Iterative function running outer loop of the FOC algorithm +// Behavior of this function is determined by the motor.controller variable +// It runs either angle, velocity or voltage loop +// - needs to be called iteratively it is asynchronous function +// - if target is not set it uses motor.target value +void BLDCMotor::move(float new_target) { + // set internal target variable + if( new_target != NOT_SET ) target = new_target; + // get angular velocity + shaft_velocity = shaftVelocity(); + // choose control loop + switch (controller) { + case ControlType::voltage: + voltage_q = target; + break; + case ControlType::angle: + // angle set point + // include angle loop + shaft_angle_sp = target; + shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); + voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + break; + case ControlType::velocity: + // velocity set point + // include velocity loop + shaft_velocity_sp = target; + voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + break; + case ControlType::velocity_openloop: + // velocity control in open loop + // loopFOC should not be called + shaft_velocity_sp = target; + velocityOpenloop(shaft_velocity_sp); + break; + case ControlType::angle_openloop: + // angle control in open loop + // loopFOC should not be called + shaft_angle_sp = target; + angleOpenloop(shaft_angle_sp); + break; + } +} + + +// Method using FOC to set Uq and Ud to the motor at the optimal angle +// Function implementing Space Vector PWM and Sine PWM algorithms +// +// Function using sine approximation +// regular sin + cos ~300us (no memory usaage) +// approx _sin + _cos ~110us (400Byte ~ 20% of memory) +void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { + + const bool centered = true; + int sector; + float _ca,_sa; + + switch (foc_modulation) + { + case FOCModulationType::Trapezoid_120 : + // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5 + static int trap_120_map[6][3] = { + {0,1,-1},{-1,1,0},{-1,0,1},{0,-1,1},{1,-1,0},{1,0,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + }; + // static int trap_120_state = 0; + sector = 6 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + + Ua = Uq + trap_120_map[sector][0] * Uq; + Ub = Uq + trap_120_map[sector][1] * Uq; + Uc = Uq + trap_120_map[sector][2] * Uq; + + if (centered) { + Ua += (driver->voltage_limit)/2 -Uq; + Ub += (driver->voltage_limit)/2 -Uq; + Uc += (driver->voltage_limit)/2 -Uq; + } + break; + + case FOCModulationType::Trapezoid_150 : + // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8 + static int trap_150_map[12][3] = { + {0,1,-1},{-1,1,-1},{-1,1,0},{-1,1,1},{-1,0,1},{-1,-1,1},{0,-1,1},{1,-1,1},{1,-1,0},{1,-1,-1},{1,0,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + }; + // static int trap_150_state = 0; + sector = 12 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + + Ua = Uq + trap_150_map[sector][0] * Uq; + Ub = Uq + trap_150_map[sector][1] * Uq; + Uc = Uq + trap_150_map[sector][2] * Uq; + + //center + if (centered) { + Ua += (driver->voltage_limit)/2 -Uq; + Ub += (driver->voltage_limit)/2 -Uq; + Uc += (driver->voltage_limit)/2 -Uq; + } + + break; + + case FOCModulationType::SinePWM : + // Sinusoidal PWM modulation + // Inverse Park + Clarke transformation + + // angle normalization in between 0 and 2pi + // only necessary if using _sin and _cos - approximation functions + angle_el = _normalizeAngle(angle_el + zero_electric_angle); + _ca = _cos(angle_el); + _sa = _sin(angle_el); + // Inverse park transform + Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; + Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; + + // Clarke transform + Ua = Ualpha + driver->voltage_limit/2; + Ub = -0.5 * Ualpha + _SQRT3_2 * Ubeta + driver->voltage_limit/2; + Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + driver->voltage_limit/2; + + if (!centered) { + float Umin = min(Ua, min(Ub, Uc)); + Ua -= Umin; + Ub -= Umin; + Uc -= Umin; + } + + break; + + case FOCModulationType::SpaceVectorPWM : + // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm + // https://www.youtube.com/watch?v=QMSWUMEAejg + + // if negative voltages change inverse the phase + // angle + 180degrees + if(Uq < 0) angle_el += _PI; + Uq = abs(Uq); + + // angle normalisation in between 0 and 2pi + // only necessary if using _sin and _cos - approximation functions + angle_el = _normalizeAngle(angle_el + zero_electric_angle + _PI_2); + + // find the sector we are in currently + sector = floor(angle_el / _PI_3) + 1; + // calculate the duty cycles + float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_limit; + float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_limit; + // two versions possible + float T0 = 0; // pulled to 0 - better for low power supply voltage + if (centered) { + T0 = 1 - T1 - T2; //centered around driver->voltage_limit/2 + } + + // calculate the duty cycles(times) + float Ta,Tb,Tc; + switch(sector){ + case 1: + Ta = T1 + T2 + T0/2; + Tb = T2 + T0/2; + Tc = T0/2; + break; + case 2: + Ta = T1 + T0/2; + Tb = T1 + T2 + T0/2; + Tc = T0/2; + break; + case 3: + Ta = T0/2; + Tb = T1 + T2 + T0/2; + Tc = T2 + T0/2; + break; + case 4: + Ta = T0/2; + Tb = T1+ T0/2; + Tc = T1 + T2 + T0/2; + break; + case 5: + Ta = T2 + T0/2; + Tb = T0/2; + Tc = T1 + T2 + T0/2; + break; + case 6: + Ta = T1 + T2 + T0/2; + Tb = T0/2; + Tc = T1 + T0/2; + break; + default: + // possible error state + Ta = 0; + Tb = 0; + Tc = 0; + } + + // calculate the phase voltages and center + Ua = Ta*driver->voltage_limit; + Ub = Tb*driver->voltage_limit; + Uc = Tc*driver->voltage_limit; + break; + + } + + // set the voltages in driver + driver->setPwm(Ua, Ub, Uc); +} + + + +// Function (iterative) generating open loop movement for target velocity +// - target_velocity - rad/s +// it uses voltage_limit variable +void BLDCMotor::velocityOpenloop(float target_velocity){ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6; + + // calculate the necessary angle to achieve target velocity + shaft_angle += target_velocity*Ts; + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; +} + +// Function (iterative) generating open loop movement towards the target angle +// - target_angle - rad +// it uses voltage_limit and velocity_limit variables +void BLDCMotor::angleOpenloop(float target_angle){ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6; + + // calculate the necessary angle to move from current position towards target angle + // with maximal velocity (velocity_limit) + if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)) + shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; + else + shaft_angle = target_angle; + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/BLDCMotor.h b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/BLDCMotor.h new file mode 100644 index 00000000..164cc108 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/BLDCMotor.h @@ -0,0 +1,109 @@ +#ifndef BLDCMotor_h +#define BLDCMotor_h + +#include "Arduino.h" +#include "common/base_classes/FOCMotor.h" +#include "common/base_classes/Sensor.h" +#include "common/base_classes/BLDCDriver.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" +#include "common/defaults.h" + +/** + BLDC motor class +*/ +class BLDCMotor: public FOCMotor +{ + public: + /** + BLDCMotor class constructor + @param pp pole pairs number + */ + BLDCMotor(int pp); + + /** + * Function linking a motor and a foc driver + * + * @param driver BLDCDriver class implementing all the hardware specific functions necessary PWM setting + */ + void linkDriver(BLDCDriver* driver); + + /** + * BLDCDriver link: + * - 3PWM + * - 6PWM + */ + BLDCDriver* driver; + + /** Motor hardware init function */ + void init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + */ + int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override; + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + void loopFOC() override; + + /** + * Function executing the control loops set by the controller parameter of the BLDCMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + void move(float target = NOT_SET) override; + + float Ua,Ub,Uc;//!< Current phase voltages Ua,Ub and Uc set to motor + float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform + + + private: + // FOC methods + /** + * Method using FOC to set Uq to the motor at the optimal angle + * Heart of the FOC algorithm + * + * @param Uq Current voltage in q axis to set to the motor + * @param Ud Current voltage in d axis to set to the motor + * @param angle_el current electrical angle of the motor + */ + void setPhaseVoltage(float Uq, float Ud, float angle_el); + /** Sensor alignment to electrical 0 angle of the motor */ + int alignSensor(); + /** Motor and sensor alignment to the sensors absolute 0 angle */ + int absoluteZeroAlign(); + + + // Open loop motion control + /** + * Function (iterative) generating open loop movement for target velocity + * it uses voltage_limit variable + * + * @param target_velocity - rad/s + */ + void velocityOpenloop(float target_velocity); + /** + * Function (iterative) generating open loop movement towards the target angle + * it uses voltage_limit and velocity_limit variables + * + * @param target_angle - rad + */ + void angleOpenloop(float target_angle); + // open loop variables + long open_loop_timestamp; +}; + + +#endif diff --git a/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/base_classes/BLDCDriver.h b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/base_classes/BLDCDriver.h new file mode 100644 index 00000000..708323fb --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/base_classes/BLDCDriver.h @@ -0,0 +1,28 @@ +#ifndef BLDCDRIVER_H +#define BLDCDRIVER_H + +class BLDCDriver{ + public: + + /** Initialise hardware */ + virtual int init(); + /** Enable hardware */ + virtual void enable(); + /** Disable hardware */ + virtual void disable(); + + long pwm_frequency; //!< pwm frequency value in hertz + float voltage_power_supply; //!< power supply voltage + float voltage_limit; //!< limiting voltage set to the motor + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + virtual void setPwm(float Ua, float Ub, float Uc); +}; + +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/base_classes/FOCMotor.cpp b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/base_classes/FOCMotor.cpp new file mode 100644 index 00000000..1d1a0f43 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/base_classes/FOCMotor.cpp @@ -0,0 +1,241 @@ +#include "FOCMotor.h" + +/** + * Default constructor - setting all variabels to default values + */ +FOCMotor::FOCMotor() +{ + // maximum angular velocity to be used for positioning + velocity_limit = DEF_VEL_LIM; + // maximum voltage to be set to the motor + voltage_limit = DEF_POWER_SUPPLY; + + // index search velocity + velocity_index_search = DEF_INDEX_SEARCH_TARGET_VELOCITY; + // sensor and motor align voltage + voltage_sensor_align = DEF_VOLTAGE_SENSOR_ALIGN; + + // electric angle of comthe zero angle + zero_electric_angle = 0; + + // default modulation is SinePWM + foc_modulation = FOCModulationType::SinePWM; + + // default target value + target = 0; + voltage_d = 0; + voltage_q = 0; + + //monitor_port + monitor_port = nullptr; + //sensor + sensor = nullptr; +} + + +/** + Sensor communication methods +*/ +void FOCMotor::linkSensor(Sensor* _sensor) { + sensor = _sensor; +} +// shaft angle calculation +float FOCMotor::shaftAngle() { + // if no sensor linked return 0 + if(!sensor) return shaft_angle; + return sensor->getAngle(); +} +// shaft velocity calculation +float FOCMotor::shaftVelocity() { + // if no sensor linked return 0 + if(!sensor) return 0; + return LPF_velocity(sensor->getVelocity()); +} + +/** + * Monitoring functions + */ +// function implementing the monitor_port setter +void FOCMotor::useMonitoring(Print &print){ + monitor_port = &print; //operate on the address of print + if(monitor_port ) monitor_port->println("MOT: Monitor enabled!"); +} +// utility function intended to be used with serial plotter to monitor motor variables +// significantly slowing the execution down!!!! +void FOCMotor::monitor() { + if(!monitor_port) return; + switch (controller) { + case ControlType::velocity_openloop: + case ControlType::velocity: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_velocity_sp); + monitor_port->print("\t"); + monitor_port->println(shaft_velocity); + break; + case ControlType::angle_openloop: + case ControlType::angle: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_angle_sp); + monitor_port->print("\t"); + monitor_port->println(shaft_angle); + break; + case ControlType::voltage: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_angle); + monitor_port->print("\t"); + monitor_port->println(shaft_velocity); + break; + } +} + +int FOCMotor::command(String user_command) { + // error flag + int errorFlag = 1; + // if empty string + if(user_command.length() < 1) return errorFlag; + + // parse command letter + char cmd = user_command.charAt(0); + // check if get command + char GET = user_command.charAt(1) == '\n'; + // parse command values + float value = user_command.substring(1).toFloat(); + + // a bit of optimisation of variable memory for Arduino UNO (atmega328) + switch(cmd){ + case 'P': // velocity P gain change + case 'I': // velocity I gain change + case 'D': // velocity D gain change + case 'R': // velocity voltage ramp change + if(monitor_port) monitor_port->print(" PID velocity| "); + break; + case 'F': // velocity Tf low pass filter change + if(monitor_port) monitor_port->print(" LPF velocity| "); + break; + case 'K': // angle loop gain P change + if(monitor_port) monitor_port->print(" P angle| "); + break; + case 'L': // velocity voltage limit change + case 'N': // angle loop gain velocity_limit change + if(monitor_port) monitor_port->print(" Limits| "); + break; + } + + // apply the the command + switch(cmd){ + case 'P': // velocity P gain change + if(monitor_port) monitor_port->print("P: "); + if(!GET) PID_velocity.P = value; + if(monitor_port) monitor_port->println(PID_velocity.P); + break; + case 'I': // velocity I gain change + if(monitor_port) monitor_port->print("I: "); + if(!GET) PID_velocity.I = value; + if(monitor_port) monitor_port->println(PID_velocity.I); + break; + case 'D': // velocity D gain change + if(monitor_port) monitor_port->print("D: "); + if(!GET) PID_velocity.D = value; + if(monitor_port) monitor_port->println(PID_velocity.D); + break; + case 'R': // velocity voltage ramp change + if(monitor_port) monitor_port->print("volt_ramp: "); + if(!GET) PID_velocity.output_ramp = value; + if(monitor_port) monitor_port->println(PID_velocity.output_ramp); + break; + case 'L': // velocity voltage limit change + if(monitor_port) monitor_port->print("volt_limit: "); + if(!GET) { + voltage_limit = value; + PID_velocity.limit = value; + } + if(monitor_port) monitor_port->println(voltage_limit); + break; + case 'F': // velocity Tf low pass filter change + if(monitor_port) monitor_port->print("Tf: "); + if(!GET) LPF_velocity.Tf = value; + if(monitor_port) monitor_port->println(LPF_velocity.Tf); + break; + case 'K': // angle loop gain P change + if(monitor_port) monitor_port->print(" P: "); + if(!GET) P_angle.P = value; + if(monitor_port) monitor_port->println(P_angle.P); + break; + case 'N': // angle loop gain velocity_limit change + if(monitor_port) monitor_port->print("vel_limit: "); + if(!GET){ + velocity_limit = value; + P_angle.limit = value; + } + if(monitor_port) monitor_port->println(velocity_limit); + break; + case 'C': + // change control type + if(monitor_port) monitor_port->print("Control: "); + + if(GET){ // if get command + switch(controller){ + case ControlType::voltage: + if(monitor_port) monitor_port->println("voltage"); + break; + case ControlType::velocity: + if(monitor_port) monitor_port->println("velocity"); + break; + case ControlType::angle: + if(monitor_port) monitor_port->println("angle"); + break; + default: + if(monitor_port) monitor_port->println("open loop"); + } + }else{ // if set command + switch((int)value){ + case 0: + if(monitor_port) monitor_port->println("voltage"); + controller = ControlType::voltage; + break; + case 1: + if(monitor_port) monitor_port->println("velocity"); + controller = ControlType::velocity; + break; + case 2: + if(monitor_port) monitor_port->println("angle"); + controller = ControlType::angle; + break; + default: // not valid command + errorFlag = 0; + } + } + break; + case 'V': // get current values of the state variables + switch((int)value){ + case 0: // get voltage + if(monitor_port) monitor_port->print("Uq: "); + if(monitor_port) monitor_port->println(voltage_q); + break; + case 1: // get velocity + if(monitor_port) monitor_port->print("Velocity: "); + if(monitor_port) monitor_port->println(shaft_velocity); + break; + case 2: // get angle + if(monitor_port) monitor_port->print("Angle: "); + if(monitor_port) monitor_port->println(shaft_angle); + break; + case 3: // get angle + if(monitor_port) monitor_port->print("Target: "); + if(monitor_port) monitor_port->println(target); + break; + default: // not valid command + errorFlag = 0; + } + break; + default: // target change + if(monitor_port) monitor_port->print("Target : "); + target = user_command.toFloat(); + if(monitor_port) monitor_port->println(target); + } + // return 0 if error and 1 if ok + return errorFlag; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/base_classes/FOCMotor.h b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/base_classes/FOCMotor.h new file mode 100644 index 00000000..986e1ab2 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/base_classes/FOCMotor.h @@ -0,0 +1,197 @@ +#ifndef FOCMOTOR_H +#define FOCMOTOR_H + +#include "Arduino.h" +#include "Sensor.h" +#include "BLDCDriver.h" + +#include "../time_utils.h" +#include "../foc_utils.h" +#include "../defaults.h" +#include "../pid.h" +#include "../lowpass_filter.h" + + +/** + * Motiron control type + */ +enum ControlType{ + voltage,//!< Torque control using voltage + velocity,//!< Velocity motion control + angle,//!< Position/angle motion control + velocity_openloop, + angle_openloop +}; + +/** + * FOC modulation type + */ +enum FOCModulationType{ + SinePWM, //!< Sinusoidal PWM modulation + SpaceVectorPWM, //!< Space vector modulation method + Trapezoid_120, + Trapezoid_150 +}; + +/** + Generic motor class +*/ +class FOCMotor +{ + public: + /** + * Default constructor - setting all variabels to default values + */ + FOCMotor(); + + /** Motor hardware init function */ + virtual void init()=0; + /** Motor disable function */ + virtual void disable()=0; + /** Motor enable function */ + virtual void enable()=0; + + /** + * Function linking a motor and a sensor + * + * @param sensor Sensor class wrapper for the FOC algorihtm to read the motor angle and velocity + */ + void linkSensor(Sensor* sensor); + + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + * + * - If zero_electric_offset parameter is set the alignment procedure is skipped + * + * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. + * @param sensor_direction sensor natural direction - default is CW + * + */ + virtual int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW)=0; + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + virtual void loopFOC()=0; + /** + * Function executing the control loops set by the controller parameter of the BLDCMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + virtual void move(float target = NOT_SET)=0; + + // State calculation methods + /** Shaft angle calculation in radians [rad] */ + float shaftAngle(); + /** + * Shaft angle calculation function in radian per second [rad/s] + * It implements low pass filtering + */ + float shaftVelocity(); + + // state variables + float target; //!< current target value - depends of the controller + float shaft_angle;//!< current motor angle + float shaft_velocity;//!< current motor velocity + float shaft_velocity_sp;//!< current target velocity + float shaft_angle_sp;//!< current target angle + float voltage_q;//!< current voltage u_q set + float voltage_d;//!< current voltage u_d set + + // motor configuration parameters + float voltage_sensor_align;//!< sensor and motor align voltage parameter + float velocity_index_search;//!< target velocity for index search + int pole_pairs;//!< Motor pole pairs number + + // limiting variables + float voltage_limit; //!< Voltage limitting variable - global limit + float velocity_limit; //!< Velocity limitting variable - global limit + + float zero_electric_angle;//! _2PI ? a_sin - _2PI : a_sin; + return _sin(a_sin); +} + + +// normalizing radian angle to [0,2PI] +float _normalizeAngle(float angle){ + float a = fmod(angle, _2PI); + return a >= 0 ? a : (a + _2PI); +} + +// Electrical angle calculation +float _electricalAngle(float shaft_angle, int pole_pairs) { + return (shaft_angle * pole_pairs); +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/foc_utils.h b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/foc_utils.h new file mode 100644 index 00000000..7da3f7bc --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/foc_utils.h @@ -0,0 +1,55 @@ +#ifndef FOCUTILS_LIB_H +#define FOCUTILS_LIB_H + +#include "Arduino.h" + +// sign function +#define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) ) +#define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) +#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) + +// utility defines +#define _2_SQRT3 1.15470053838 +#define _SQRT3 1.73205080757 +#define _1_SQRT3 0.57735026919 +#define _SQRT3_2 0.86602540378 +#define _SQRT2 1.41421356237 +#define _120_D2R 2.09439510239 +#define _PI 3.14159265359 +#define _PI_2 1.57079632679 +#define _PI_3 1.0471975512 +#define _2PI 6.28318530718 +#define _3PI_2 4.71238898038 + +#define NOT_SET -12345.0 + +/** + * Function approximating the sine calculation by using fixed size array + * - execution time ~40us (Arduino UNO) + * + * @param a angle in between 0 and 2PI + */ +float _sin(float a); +/** + * Function approximating cosine calculation by using fixed size array + * - execution time ~50us (Arduino UNO) + * + * @param a angle in between 0 and 2PI + */ +float _cos(float a); + +/** + * normalizing radian angle to [0,2PI] + * @param angle - angle to be normalized + */ +float _normalizeAngle(float angle); + + +/** + * Electrical angle calculation + * + * @param shaft_angle - shaft angle of the motor + * @param pole_pairs - number of pole pairs + */ +float _electricalAngle(float shaft_angle, int pole_pairs); +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/lowpass_filter.cpp b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/lowpass_filter.cpp new file mode 100644 index 00000000..9bf4ab7a --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/lowpass_filter.cpp @@ -0,0 +1,25 @@ +#include "lowpass_filter.h" + +LowPassFilter::LowPassFilter(float time_constant) + : Tf(time_constant) + , y_prev(0.0f) +{ + timestamp_prev = _micros(); +} + + +float LowPassFilter::operator() (float x) +{ + unsigned long timestamp = _micros(); + float dt = (timestamp - timestamp_prev)*1e-6f; + + if (dt < 0.0f || dt > 0.5f) + dt = 1e-3f; + + float alpha = Tf/(Tf + dt); + float y = alpha*y_prev + (1.0f - alpha)*x; + + y_prev = y; + timestamp_prev = timestamp; + return y; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/lowpass_filter.h b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/lowpass_filter.h new file mode 100644 index 00000000..7c51be54 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/lowpass_filter.h @@ -0,0 +1,29 @@ +#ifndef LOWPASS_FILTER_H +#define LOWPASS_FILTER_H + + +#include "time_utils.h" +#include "foc_utils.h" + + +/** + * Low pass filter class + */ +class LowPassFilter +{ +public: + /** + * @param Tf - Low pass filter time constant + */ + LowPassFilter(float Tf); + ~LowPassFilter() = default; + + float operator() (float x); + float Tf; //!< Low pass filter time constant + +protected: + unsigned long timestamp_prev; //!< Last execution timestamp + float y_prev; //!< filtered value in previous execution step +}; + +#endif // LOWPASS_FILTER_H \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/pid.cpp b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/pid.cpp new file mode 100644 index 00000000..1a0a9828 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/pid.cpp @@ -0,0 +1,56 @@ +#include "pid.h" + +PIDController::PIDController(float P, float I, float D, float ramp, float limit) + : P(P) + , I(I) + , D(D) + , output_ramp(ramp) // output derivative limit [volts/second] + , limit(limit) // output supply limit [volts] + , integral_prev(0.0) + , error_prev(0.0) + , output_prev(0.0) +{ + timestamp_prev = _micros()*1e-6; +} + +// PID controller function +float PIDController::operator() (float error){ + // calculate the time from the last call + unsigned long timestamp_now = _micros(); + float Ts = (timestamp_now - timestamp_prev) * 1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + + // u(s) = (P + I/s + Ds)e(s) + // Discrete implementations + // proportional part + // u_p = P *e(k) + float proportional = P * error; + // Tustin transform of the integral part + // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) + float integral = integral_prev + I*Ts*0.5*(error + error_prev); + // antiwindup - limit the output voltage_q + integral = _constrain(integral, -limit, limit); + // Discrete derivation + // u_dk = D(ek - ek_1)/Ts + float derivative = D*(error - error_prev)/Ts; + + // sum all the components + float output = proportional + integral + derivative; + // antiwindup - limit the output variable + output = _constrain(output, -limit, limit); + + // limit the acceleration by ramping the output + float output_rate = (output - output_prev)/Ts; + if (output_rate > output_ramp) + output = output_prev + output_ramp*Ts; + else if (output_rate < -output_ramp) + output = output_prev - output_ramp*Ts; + + // saving for the next pass + integral_prev = integral; + output_prev = output; + error_prev = error; + timestamp_prev = timestamp_now; + return output; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/pid.h b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/pid.h new file mode 100644 index 00000000..0e9ddf54 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/pid.h @@ -0,0 +1,40 @@ +#ifndef PID_H +#define PID_H + + +#include "time_utils.h" +#include "foc_utils.h" + +/** + * PID controller class + */ +class PIDController +{ +public: + /** + * + * @param P - Proportional gain + * @param I - Integral gain + * @param D - Derivative gain + * @param ramp - Maximum speed of change of the output value + * @param limit - Maximum output value + */ + PIDController(float P, float I, float D, float ramp, float limit); + ~PIDController() = default; + + float operator() (float error); + + float P; //!< Proportional gain + float I; //!< Integral gain + float D; //!< Derivative gain + float output_ramp; //!< Maximum speed of change of the output value + float limit; //!< Maximum output value + +protected: + float integral_prev; //!< last integral component value + float error_prev; //!< last tracking error value + float timestamp_prev; //!< Last execution timestamp + float output_prev; //!< last pid output value +}; + +#endif // PID_H \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/time_utils.cpp b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/time_utils.cpp new file mode 100644 index 00000000..181d03cf --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/time_utils.cpp @@ -0,0 +1,31 @@ +#include "time_utils.h" + +// function buffering delay() +// arduino uno function doesn't work well with interrupts +void _delay(unsigned long ms){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) + // if arduino uno and other atmega328p chips + // use while instad of delay, + // due to wrong measurement based on changed timer0 + unsigned long t = _micros() + ms*1000; + while( _micros() < t ){}; +#else + // regular micros + delay(ms); +#endif +} + + +// function buffering _micros() +// arduino function doesn't work well with interrupts +unsigned long _micros(){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) +// if arduino uno and other atmega328p chips + //return the value based on the prescaler + if((TCCR0B & 0b00000111) == 0x01) return (micros()/32); + else return (micros()); +#else + // regular micros + return micros(); +#endif +} diff --git a/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/time_utils.h b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/time_utils.h new file mode 100644 index 00000000..143d4853 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/common/time_utils.h @@ -0,0 +1,22 @@ +#ifndef TIME_UTILS_H +#define TIME_UTILS_H + +#include "foc_utils.h" + +/** + * Function implementing delay() function in milliseconds + * - blocking function + * - hardware specific + + * @param ms number of milliseconds to wait + */ +void _delay(unsigned long ms); + +/** + * Function implementing timestamp getting function in microseconds + * hardware specific + */ +unsigned long _micros(); + + +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/drivers/BLDCDriver3PWM.cpp b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/drivers/BLDCDriver3PWM.cpp new file mode 100644 index 00000000..a3fc052d --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/drivers/BLDCDriver3PWM.cpp @@ -0,0 +1,72 @@ +#include "BLDCDriver3PWM.h" + +BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en){ + // Pin initialization + pwmA = phA; + pwmB = phB; + pwmC = phC; + + // enable_pin pin + enable_pin = en; + + // default power-supply value + voltage_power_supply = DEF_POWER_SUPPLY; + voltage_limit = NOT_SET; + +} + +// enable motor driver +void BLDCDriver3PWM::enable(){ + // enable_pin the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, HIGH); + // set zero to PWM + setPwm(0,0,0); +} + +// disable motor driver +void BLDCDriver3PWM::disable() +{ + // set zero to PWM + setPwm(0, 0, 0); + // disable the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, LOW); + +} + +// init hardware pins +int BLDCDriver3PWM::init() { + // a bit of separation + _delay(1000); + + // PWM pins + pinMode(pwmA, OUTPUT); + pinMode(pwmB, OUTPUT); + pinMode(pwmC, OUTPUT); + if(enable_pin != NOT_SET) pinMode(enable_pin, OUTPUT); + + + // sanity check for the voltage limit configuration + if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + + // Set the pwm frequency to the pins + // hardware specific function - depending on driver and mcu + _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC); + return 0; +} + + +// Set voltage to the pwm pin +void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { + // limit the voltage in driver + Ua = _constrain(Ua, 0.0, voltage_limit); + Ub = _constrain(Ub, 0.0, voltage_limit); + Uc = _constrain(Uc, 0.0, voltage_limit); + // calculate duty cycle + // limited in [0,1] + float dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 ); + float dc_b = _constrain(Ub / voltage_power_supply, 0.0 , 1.0 ); + float dc_c = _constrain(Uc / voltage_power_supply, 0.0 , 1.0 ); + // hardware specific writing + // hardware specific function - depending on driver and mcu + _writeDutyCycle3PWM(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC); +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/drivers/BLDCDriver3PWM.h b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/drivers/BLDCDriver3PWM.h new file mode 100644 index 00000000..b6f7d7f8 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/drivers/BLDCDriver3PWM.h @@ -0,0 +1,52 @@ +#ifndef BLDCDriver3PWM_h +#define BLDCDriver3PWM_h + +#include "../common/base_classes/BLDCDriver.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "hardware_api.h" + +/** + 3 pwm bldc driver class +*/ +class BLDCDriver3PWM: public BLDCDriver +{ + public: + /** + BLDCDriver class constructor + @param phA A phase pwm pin + @param phB B phase pwm pin + @param phC C phase pwm pin + @param en enable pin (optional input) + */ + BLDCDriver3PWM(int phA,int phB,int phC, int en = NOT_SET); + + /** Motor hardware init function */ + int init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + // hardware variables + int pwmA; //!< phase A pwm pin number + int pwmB; //!< phase B pwm pin number + int pwmC; //!< phase C pwm pin number + int enable_pin; //!< enable pin number + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + void setPwm(float Ua, float Ub, float Uc) override; + + private: + +}; + + +#endif diff --git a/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/drivers/hardware_api.h b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/drivers/hardware_api.h new file mode 100644 index 00000000..01c03867 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/drivers/hardware_api.h @@ -0,0 +1,123 @@ +#ifndef HARDWARE_UTILS_H +#define HARDWARE_UTILS_H + +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA bldc driver + * @param pinB pinB bldc driver + */ +void _configure2PWM(long pwm_frequency, const int pinA, const int pinB); + +/** + * Configuring PWM frequency, resolution and alignment + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA bldc driver + * @param pinB pinB bldc driver + * @param pinC pinC bldc driver + */ +void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC); + +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 4PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pin1A pin1A stepper driver + * @param pin1B pin1B stepper driver + * @param pin2A pin2A stepper driver + * @param pin2B pin2B stepper driver + */ +void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B); + +/** + * Configuring PWM frequency, resolution and alignment + * - BLDC driver - 6PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low - if applicable + * @param pinA_h pinA high-side bldc driver + * @param pinA_l pinA low-side bldc driver + * @param pinB_h pinA high-side bldc driver + * @param pinB_l pinA low-side bldc driver + * @param pinC_h pinA high-side bldc driver + * @param pinC_l pinA low-side bldc driver + * + * @return 0 if config good, -1 if failed + */ +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param pinA phase A hardware pin number + * @param pinB phase B hardware pin number + */ +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param pinA phase A hardware pin number + * @param pinB phase B hardware pin number + * @param pinC phase C hardware pin number + */ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 4PWM setting + * - hardware specific + * + * @param dc_1a duty cycle phase 1A [0, 1] + * @param dc_1b duty cycle phase 1B [0, 1] + * @param dc_2a duty cycle phase 2A [0, 1] + * @param dc_2b duty cycle phase 2B [0, 1] + * @param pin1A phase 1A hardware pin number + * @param pin1B phase 1B hardware pin number + * @param pin2A phase 2A hardware pin number + * @param pin2B phase 2B hardware pin number + */ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B); + + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 6PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low + * @param pinA_h phase A high-side hardware pin number + * @param pinA_l phase A low-side hardware pin number + * @param pinB_h phase B high-side hardware pin number + * @param pinB_l phase B low-side hardware pin number + * @param pinC_h phase C high-side hardware pin number + * @param pinC_l phase C low-side hardware pin number + * + */ +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l); + +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/drivers/hardware_specific/atmega328_mcu.cpp b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/drivers/hardware_specific/atmega328_mcu.cpp new file mode 100644 index 00000000..24fe563e --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/drivers/hardware_specific/atmega328_mcu.cpp @@ -0,0 +1,153 @@ +#include "../hardware_api.h" + +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) + +// set pwm frequency to 32KHz +void _pinHighFrequency(const int pin){ + // High PWM frequency + // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328 + if (pin == 5 || pin == 6 ) { + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 + } + if (pin == 9 || pin == 10 ) + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 + if (pin == 3 || pin == 11) + TCCR2B = ((TCCR2B & 0b11111000) | 0x01);// set prescaler to 1 + +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); +} + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); + _pinHighFrequency(pinC); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); + analogWrite(pinC, 255.0*dc_c); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pin1A); + _pinHighFrequency(pin1B); + _pinHighFrequency(pin2A); + _pinHighFrequency(pin2B); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0*dc_1a); + analogWrite(pin1B, 255.0*dc_1b); + analogWrite(pin2A, 255.0*dc_2a); + analogWrite(pin2B, 255.0*dc_2b); +} + + +// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm +int _configureComplementaryPair(int pinH, int pinL) { + if( (pinH == 5 && pinL == 6 ) || (pinH == 6 && pinL == 5 ) ){ + // configure the pwm phase-corrected mode + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); + // configure complementary pwm on low side + if(pinH == 6 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ; + else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ; + // set prescaler to 1 - 32kHz freq + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); + }else if( (pinH == 9 && pinL == 10 ) || (pinH == 10 && pinL == 9 ) ){ + // set prescaler to 1 - 32kHz freq + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); + // configure complementary pwm on low side + if(pinH == 9 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ; + else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ; + }else if((pinH == 3 && pinL == 11 ) || (pinH == 11 && pinL == 3 ) ){ + // set prescaler to 1 - 32kHz freq + TCCR2B = ((TCCR2B & 0b11111000) | 0x01); + // configure complementary pwm on low side + if(pinH == 11 ) TCCR2A = 0b10110000 | (TCCR2A & 0b00001111) ; + else TCCR2A = 0b11100000 | (TCCR2A & 0b00001111) ; + }else{ + return -1; + } + return 0; +} + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arudino/ATmega328 +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + // High PWM frequency + // - always max 32kHz + int ret_flag = 0; + ret_flag += _configureComplementaryPair(pinA_h, pinA_l); + ret_flag += _configureComplementaryPair(pinB_h, pinB_l); + ret_flag += _configureComplementaryPair(pinC_h, pinC_l); + return ret_flag; // returns -1 if not well configured +} + +// function setting the +void _setPwmPair(int pinH, int pinL, float val, int dead_time) +{ + int pwm_h = _constrain(val-dead_time/2,0,255); + int pwm_l = _constrain(val+dead_time/2,0,255); + + analogWrite(pinH, pwm_h); + if(pwm_l == 255 || pwm_l == 0) + digitalWrite(pinL, pwm_l ? LOW : HIGH); + else + analogWrite(pinL, pwm_l); +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arudino/ATmega328 +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); + _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); + _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0); +} + +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/sensors/MagneticSensorI2C.cpp b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/sensors/MagneticSensorI2C.cpp new file mode 100644 index 00000000..accce08a --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/sensors/MagneticSensorI2C.cpp @@ -0,0 +1,232 @@ +#include "MagneticSensorI2C.h" + +/** Typical configuration for the 12bit AMS AS5600 magnetic sensor over I2C interface */ +MagneticSensorI2CConfig_s AS5600_I2C = { + .chip_address = 0x36, + .bit_resolution = 12, + .angle_register = 0x0E, + .data_start_bit = 11 +}; + +/** Typical configuration for the 12bit AMS AS5048 magnetic sensor over I2C interface */ +MagneticSensorI2CConfig_s AS5048_I2C = { + .chip_address = 0x40, // highly configurable. if A1 and A2 are held low, this is probable value + .bit_resolution = 14, + .angle_register = 0xFE, + .data_start_bit = 15 +}; + + +// MagneticSensorI2C(uint8_t _chip_address, float _cpr, uint8_t _angle_register_msb) +// @param _chip_address I2C chip address +// @param _bit_resolution bit resolution of the sensor +// @param _angle_register_msb angle read register +// @param _bits_used_msb number of used bits in msb +MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _bits_used_msb){ + // chip I2C address + chip_address = _chip_address; + // angle read register of the magnetic sensor + angle_register_msb = _angle_register_msb; + // register maximum value (counts per revolution) + cpr = pow(2, _bit_resolution); + + // depending on the sensor architecture there are different combinations of + // LSB and MSB register used bits + // AS5600 uses 0..7 LSB and 8..11 MSB + // AS5048 uses 0..5 LSB and 6..13 MSB + // used bits in LSB + lsb_used = _bit_resolution - _bits_used_msb; + // extraction masks + lsb_mask = (uint8_t)( (2 << lsb_used) - 1 ); + msb_mask = (uint8_t)( (2 << _bits_used_msb) - 1 ); + wire = &Wire; +} + +MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ + chip_address = config.chip_address; + + // angle read register of the magnetic sensor + angle_register_msb = config.angle_register; + // register maximum value (counts per revolution) + cpr = pow(2, config.bit_resolution); + + int bits_used_msb = config.data_start_bit - 7; + lsb_used = config.bit_resolution - bits_used_msb; + // extraction masks + lsb_mask = (uint8_t)( (2 << lsb_used) - 1 ); + msb_mask = (uint8_t)( (2 << bits_used_msb) - 1 ); + wire = &Wire; +} + +void MagneticSensorI2C::init(TwoWire* _wire){ + + wire = _wire; + + // I2C communication begin + wire->begin(); + + // velocity calculation init + angle_prev = 0; + velocity_calc_timestamp = _micros(); + + // full rotations tracking number + full_rotation_offset = 0; + angle_data_prev = getRawCount(); + zero_offset = 0; +} + +// Shaft angle calculation +// angle is in radians [rad] +float MagneticSensorI2C::getAngle(){ + // raw data from the sensor + float angle_data = getRawCount(); + + // tracking the number of rotations + // in order to expand angle range form [0,2PI] + // to basically infinity + float d_angle = angle_data - angle_data_prev; + // if overflow happened track it as full rotation + if(abs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; + // save the current angle value for the next steps + // in order to know if overflow happened + angle_data_prev = angle_data; + + // zero offset adding + angle_data -= (int)zero_offset; + // return the full angle + // (number of full rotations)*2PI + current sensor angle + return natural_direction * (full_rotation_offset + ( angle_data / (float)cpr) * _2PI); +} + +// Shaft velocity calculation +float MagneticSensorI2C::getVelocity(){ + // calculate sample time + unsigned long now_us = _micros(); + float Ts = (now_us - velocity_calc_timestamp)*1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + + // current angle + float angle_c = getAngle(); + // velocity calculation + float vel = (angle_c - angle_prev)/Ts; + + // save variables for future pass + angle_prev = angle_c; + velocity_calc_timestamp = now_us; + return vel; +} + +// set current angle as zero angle +// return the angle [rad] difference +float MagneticSensorI2C::initRelativeZero(){ + float angle_offset = -getAngle(); + zero_offset = natural_direction * getRawCount(); + + // angle tracking variables + full_rotation_offset = 0; + return angle_offset; +} +// set absolute zero angle as zero angle +// return the angle [rad] difference +float MagneticSensorI2C::initAbsoluteZero(){ + float rotation = -(int)zero_offset; + // init absolute zero + zero_offset = 0; + + // angle tracking variables + full_rotation_offset = 0; + // return offset in radians + return rotation / (float)cpr * _2PI; +} +// returns 0 if it has no absolute 0 measurement +// 0 - incremental encoder without index +// 1 - encoder with index & magnetic sensors +int MagneticSensorI2C::hasAbsoluteZero(){ + return 1; +} +// returns 0 if it does need search for absolute zero +// 0 - magnetic sensor +// 1 - ecoder with index +int MagneticSensorI2C::needsAbsoluteZeroSearch(){ + return 0; +} + + +// function reading the raw counter of the magnetic sensor +int MagneticSensorI2C::getRawCount(){ + return (int)MagneticSensorI2C::read(angle_register_msb); +} + +// I2C functions +/* +* Read a register from the sensor +* Takes the address of the register as a uint8_t +* Returns the value of the register +*/ +int MagneticSensorI2C::read(uint8_t angle_reg_msb) { + // read the angle register first MSB then LSB + byte readArray[2]; + uint16_t readValue = 0; + // notify the device that is aboout to be read + wire->beginTransmission(chip_address); + wire->write(angle_reg_msb); + wire->endTransmission(false); + + // read the data msb and lsb + wire->requestFrom(chip_address, (uint8_t)2); + for (byte i=0; i < 2; i++) { + readArray[i] = wire->read(); + } + + // depending on the sensor architecture there are different combinations of + // LSB and MSB register used bits + // AS5600 uses 0..7 LSB and 8..11 MSB + // AS5048 uses 0..5 LSB and 6..13 MSB + readValue = ( readArray[1] & lsb_mask ); + readValue += ( ( readArray[0] & msb_mask ) << lsb_used ); + return readValue; +} + +/* +* Checks whether other devices have locked the bus. Can clear SDA locks. +* This should be called before sensor.init() on devices that suffer i2c slaves locking sda +* e.g some stm32 boards with AS5600 chips +* Takes the sda_pin and scl_pin +* Returns 0 for OK, 1 for other master and 2 for unfixable sda locked LOW +*/ +int MagneticSensorI2C::checkBus(byte sda_pin, byte scl_pin) { + + pinMode(scl_pin, INPUT_PULLUP); + pinMode(sda_pin, INPUT_PULLUP); + delay(250); + + if (digitalRead(scl_pin) == LOW) { + // Someone else has claimed master!"); + return 1; + } + + if(digitalRead(sda_pin) == LOW) { + // slave is communicating and awaiting clocks, we are blocked + pinMode(scl_pin, OUTPUT); + for (byte i = 0; i < 16; i++) { + // toggle clock for 2 bytes of data + digitalWrite(scl_pin, LOW); + delayMicroseconds(20); + digitalWrite(scl_pin, HIGH); + delayMicroseconds(20); + } + pinMode(sda_pin, INPUT); + delayMicroseconds(20); + if (digitalRead(sda_pin) == LOW) { + // SDA still blocked + return 2; + } + _delay(1000); + } + // SDA is clear (HIGH) + pinMode(sda_pin, INPUT); + pinMode(scl_pin, INPUT); + + return 0; +} diff --git a/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/sensors/MagneticSensorI2C.h b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/sensors/MagneticSensorI2C.h new file mode 100644 index 00000000..7fe4efeb --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_magnetic_i2c/src/sensors/MagneticSensorI2C.h @@ -0,0 +1,101 @@ +#ifndef MAGNETICSENSORI2C_LIB_H +#define MAGNETICSENSORI2C_LIB_H + +#include "Arduino.h" +#include +#include "../common/base_classes/Sensor.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + +struct MagneticSensorI2CConfig_s { + int chip_address; + int bit_resolution; + int angle_register; + int data_start_bit; +}; +// some predefined structures +extern MagneticSensorI2CConfig_s AS5600_I2C,AS5048_I2C; + +class MagneticSensorI2C: public Sensor{ + public: + /** + * MagneticSensorI2C class constructor + * @param chip_address I2C chip address + * @param bits number of bits of the sensor resolution + * @param angle_register_msb angle read register msb + * @param _bits_used_msb number of used bits in msb + */ + MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _msb_bits_used); + + /** + * MagneticSensorI2C class constructor + * @param config I2C config + */ + MagneticSensorI2C(MagneticSensorI2CConfig_s config); + + static MagneticSensorI2C AS5600(); + + /** sensor initialise pins */ + void init(TwoWire* _wire = &Wire); + + // implementation of abstract functions of the Sensor class + /** get current angle (rad) */ + float getAngle() override; + /** get current angular velocity (rad/s) **/ + float getVelocity() override; + /** + * set current angle as zero angle + * return the angle [rad] difference + */ + float initRelativeZero() override; + /** + * set absolute zero angle as zero angle + * return the angle [rad] difference + */ + float initAbsoluteZero() override; + /** returns 1 because it is the absolute sensor */ + int hasAbsoluteZero() override; + /** returns 0 maning it doesn't need search for absolute zero */ + + int needsAbsoluteZeroSearch() override; + + /** experimental function to check and fix SDA locked LOW issues */ + int checkBus(byte sda_pin = SDA, byte scl_pin = SCL); + + private: + float cpr; //!< Maximum range of the magnetic sensor + uint16_t lsb_used; //!< Number of bits used in LSB register + uint8_t lsb_mask; + uint8_t msb_mask; + + // I2C variables + uint8_t angle_register_msb; //!< I2C angle register to read + uint8_t chip_address; //!< I2C chip select pins + + // I2C functions + /** Read one I2C register value */ + int read(uint8_t angle_register_msb); + + word zero_offset; //!< user defined zero offset + /** + * Function getting current angle register value + * it uses angle_register variable + */ + int getRawCount(); + + // total angle tracking variables + float full_rotation_offset; //!println("MOT: Initialise variables."); + // sanity check for the voltage limit configuration + if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; + // constrain voltage for sensor alignment + if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; + // update the controller limits + PID_velocity.limit = voltage_limit; + P_angle.limit = velocity_limit; + + _delay(500); + // enable motor + if(monitor_port) monitor_port->println("MOT: Enable driver."); + enable(); + _delay(500); +} + + +// disable motor driver +void BLDCMotor::disable() +{ + // set zero to PWM + driver->setPwm(0, 0, 0); + // disable the driver + driver->disable(); +} +// enable motor driver +void BLDCMotor::enable() +{ + // enable the driver + driver->enable(); + // set zero to PWM + driver->setPwm(0, 0, 0); + +} + +/** + FOC functions +*/ +// FOC initialization function +int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction ) { + int exit_flag = 1; + // align motor if necessary + // alignment necessary for encoders! + if(zero_electric_offset != NOT_SET){ + // abosolute zero offset provided - no need to align + zero_electric_angle = zero_electric_offset; + // set the sensor direction - default CW + sensor->natural_direction = sensor_direction; + }else{ + // sensor and motor alignment + _delay(500); + exit_flag = alignSensor(); + _delay(500); + } + if(monitor_port) monitor_port->println("MOT: Motor ready."); + + return exit_flag; +} +// Encoder alignment to electrical 0 angle +int BLDCMotor::alignSensor() { + if(monitor_port) monitor_port->println("MOT: Align sensor."); + // align the electrical phases of the motor and sensor + // set angle -90 degrees + + float start_angle = shaftAngle(); + for (int i = 0; i <=5; i++ ) { + float angle = _3PI_2 + _2PI * i / 6.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(200); + } + float mid_angle = shaftAngle(); + for (int i = 5; i >=0; i-- ) { + float angle = _3PI_2 + _2PI * i / 6.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(200); + } + if (mid_angle < start_angle) { + if(monitor_port) monitor_port->println("MOT: natural_direction==CCW"); + sensor->natural_direction = Direction::CCW; + } else if (mid_angle == start_angle) { + if(monitor_port) monitor_port->println("MOT: Sensor failed to notice movement"); + } + + // let the motor stabilize for 2 sec + _delay(2000); + // set sensor to zero + sensor->initRelativeZero(); + _delay(500); + setPhaseVoltage(0, 0, 0); + _delay(200); + + // find the index if available + int exit_flag = absoluteZeroAlign(); + _delay(500); + if(monitor_port){ + if(exit_flag< 0 ) monitor_port->println("MOT: Error: Not found!"); + if(exit_flag> 0 ) monitor_port->println("MOT: Success!"); + else monitor_port->println("MOT: Not available!"); + } + return exit_flag; +} + + +// Encoder alignment the absolute zero angle +// - to the index +int BLDCMotor::absoluteZeroAlign() { + + if(monitor_port) monitor_port->println("MOT: Absolute zero align."); + // if no absolute zero return + if(!sensor->hasAbsoluteZero()) return 0; + + + if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println("MOT: Searching..."); + // search the absolute zero with small velocity + while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){ + loopFOC(); + voltage_q = PID_velocity(velocity_index_search - shaftVelocity()); + } + voltage_q = 0; + // disable motor + setPhaseVoltage(0, 0, 0); + + // align absolute zero if it has been found + if(!sensor->needsAbsoluteZeroSearch()){ + // align the sensor with the absolute zero + float zero_offset = sensor->initAbsoluteZero(); + // remember zero electric angle + zero_electric_angle = _normalizeAngle(_electricalAngle(zero_offset, pole_pairs)); + } + // return bool if zero found + return !sensor->needsAbsoluteZeroSearch() ? 1 : -1; +} + +// Iterative function looping FOC algorithm, setting Uq on the Motor +// The faster it can be run the better +void BLDCMotor::loopFOC() { + // shaft angle + shaft_angle = shaftAngle(); + // set the phase voltage - FOC heart function :) + setPhaseVoltage(voltage_q, voltage_d, _electricalAngle(shaft_angle,pole_pairs)); +} + +// Iterative function running outer loop of the FOC algorithm +// Behavior of this function is determined by the motor.controller variable +// It runs either angle, velocity or voltage loop +// - needs to be called iteratively it is asynchronous function +// - if target is not set it uses motor.target value +void BLDCMotor::move(float new_target) { + // set internal target variable + if( new_target != NOT_SET ) target = new_target; + // get angular velocity + shaft_velocity = shaftVelocity(); + // choose control loop + switch (controller) { + case ControlType::voltage: + voltage_q = target; + break; + case ControlType::angle: + // angle set point + // include angle loop + shaft_angle_sp = target; + shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); + voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + break; + case ControlType::velocity: + // velocity set point + // include velocity loop + shaft_velocity_sp = target; + voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + break; + case ControlType::velocity_openloop: + // velocity control in open loop + // loopFOC should not be called + shaft_velocity_sp = target; + velocityOpenloop(shaft_velocity_sp); + break; + case ControlType::angle_openloop: + // angle control in open loop + // loopFOC should not be called + shaft_angle_sp = target; + angleOpenloop(shaft_angle_sp); + break; + } +} + + +// Method using FOC to set Uq and Ud to the motor at the optimal angle +// Function implementing Space Vector PWM and Sine PWM algorithms +// +// Function using sine approximation +// regular sin + cos ~300us (no memory usaage) +// approx _sin + _cos ~110us (400Byte ~ 20% of memory) +void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { + + const bool centered = true; + int sector; + float _ca,_sa; + + switch (foc_modulation) + { + case FOCModulationType::Trapezoid_120 : + // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5 + static int trap_120_map[6][3] = { + {0,1,-1},{-1,1,0},{-1,0,1},{0,-1,1},{1,-1,0},{1,0,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + }; + // static int trap_120_state = 0; + sector = 6 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + + Ua = Uq + trap_120_map[sector][0] * Uq; + Ub = Uq + trap_120_map[sector][1] * Uq; + Uc = Uq + trap_120_map[sector][2] * Uq; + + if (centered) { + Ua += (driver->voltage_limit)/2 -Uq; + Ub += (driver->voltage_limit)/2 -Uq; + Uc += (driver->voltage_limit)/2 -Uq; + } + break; + + case FOCModulationType::Trapezoid_150 : + // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8 + static int trap_150_map[12][3] = { + {0,1,-1},{-1,1,-1},{-1,1,0},{-1,1,1},{-1,0,1},{-1,-1,1},{0,-1,1},{1,-1,1},{1,-1,0},{1,-1,-1},{1,0,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + }; + // static int trap_150_state = 0; + sector = 12 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + + Ua = Uq + trap_150_map[sector][0] * Uq; + Ub = Uq + trap_150_map[sector][1] * Uq; + Uc = Uq + trap_150_map[sector][2] * Uq; + + //center + if (centered) { + Ua += (driver->voltage_limit)/2 -Uq; + Ub += (driver->voltage_limit)/2 -Uq; + Uc += (driver->voltage_limit)/2 -Uq; + } + + break; + + case FOCModulationType::SinePWM : + // Sinusoidal PWM modulation + // Inverse Park + Clarke transformation + + // angle normalization in between 0 and 2pi + // only necessary if using _sin and _cos - approximation functions + angle_el = _normalizeAngle(angle_el + zero_electric_angle); + _ca = _cos(angle_el); + _sa = _sin(angle_el); + // Inverse park transform + Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; + Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; + + // Clarke transform + Ua = Ualpha + driver->voltage_limit/2; + Ub = -0.5 * Ualpha + _SQRT3_2 * Ubeta + driver->voltage_limit/2; + Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + driver->voltage_limit/2; + + if (!centered) { + float Umin = min(Ua, min(Ub, Uc)); + Ua -= Umin; + Ub -= Umin; + Uc -= Umin; + } + + break; + + case FOCModulationType::SpaceVectorPWM : + // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm + // https://www.youtube.com/watch?v=QMSWUMEAejg + + // if negative voltages change inverse the phase + // angle + 180degrees + if(Uq < 0) angle_el += _PI; + Uq = abs(Uq); + + // angle normalisation in between 0 and 2pi + // only necessary if using _sin and _cos - approximation functions + angle_el = _normalizeAngle(angle_el + zero_electric_angle + _PI_2); + + // find the sector we are in currently + sector = floor(angle_el / _PI_3) + 1; + // calculate the duty cycles + float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_limit; + float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_limit; + // two versions possible + float T0 = 0; // pulled to 0 - better for low power supply voltage + if (centered) { + T0 = 1 - T1 - T2; //centered around driver->voltage_limit/2 + } + + // calculate the duty cycles(times) + float Ta,Tb,Tc; + switch(sector){ + case 1: + Ta = T1 + T2 + T0/2; + Tb = T2 + T0/2; + Tc = T0/2; + break; + case 2: + Ta = T1 + T0/2; + Tb = T1 + T2 + T0/2; + Tc = T0/2; + break; + case 3: + Ta = T0/2; + Tb = T1 + T2 + T0/2; + Tc = T2 + T0/2; + break; + case 4: + Ta = T0/2; + Tb = T1+ T0/2; + Tc = T1 + T2 + T0/2; + break; + case 5: + Ta = T2 + T0/2; + Tb = T0/2; + Tc = T1 + T2 + T0/2; + break; + case 6: + Ta = T1 + T2 + T0/2; + Tb = T0/2; + Tc = T1 + T0/2; + break; + default: + // possible error state + Ta = 0; + Tb = 0; + Tc = 0; + } + + // calculate the phase voltages and center + Ua = Ta*driver->voltage_limit; + Ub = Tb*driver->voltage_limit; + Uc = Tc*driver->voltage_limit; + break; + + } + + // set the voltages in driver + driver->setPwm(Ua, Ub, Uc); +} + + + +// Function (iterative) generating open loop movement for target velocity +// - target_velocity - rad/s +// it uses voltage_limit variable +void BLDCMotor::velocityOpenloop(float target_velocity){ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6; + + // calculate the necessary angle to achieve target velocity + shaft_angle += target_velocity*Ts; + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; +} + +// Function (iterative) generating open loop movement towards the target angle +// - target_angle - rad +// it uses voltage_limit and velocity_limit variables +void BLDCMotor::angleOpenloop(float target_angle){ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6; + + // calculate the necessary angle to move from current position towards target angle + // with maximal velocity (velocity_limit) + if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)) + shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; + else + shaft_angle = target_angle; + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_openloop/src/BLDCMotor.h b/minimal_project_examples/atmega328_bldc_openloop/src/BLDCMotor.h new file mode 100644 index 00000000..164cc108 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_openloop/src/BLDCMotor.h @@ -0,0 +1,109 @@ +#ifndef BLDCMotor_h +#define BLDCMotor_h + +#include "Arduino.h" +#include "common/base_classes/FOCMotor.h" +#include "common/base_classes/Sensor.h" +#include "common/base_classes/BLDCDriver.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" +#include "common/defaults.h" + +/** + BLDC motor class +*/ +class BLDCMotor: public FOCMotor +{ + public: + /** + BLDCMotor class constructor + @param pp pole pairs number + */ + BLDCMotor(int pp); + + /** + * Function linking a motor and a foc driver + * + * @param driver BLDCDriver class implementing all the hardware specific functions necessary PWM setting + */ + void linkDriver(BLDCDriver* driver); + + /** + * BLDCDriver link: + * - 3PWM + * - 6PWM + */ + BLDCDriver* driver; + + /** Motor hardware init function */ + void init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + */ + int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override; + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + void loopFOC() override; + + /** + * Function executing the control loops set by the controller parameter of the BLDCMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + void move(float target = NOT_SET) override; + + float Ua,Ub,Uc;//!< Current phase voltages Ua,Ub and Uc set to motor + float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform + + + private: + // FOC methods + /** + * Method using FOC to set Uq to the motor at the optimal angle + * Heart of the FOC algorithm + * + * @param Uq Current voltage in q axis to set to the motor + * @param Ud Current voltage in d axis to set to the motor + * @param angle_el current electrical angle of the motor + */ + void setPhaseVoltage(float Uq, float Ud, float angle_el); + /** Sensor alignment to electrical 0 angle of the motor */ + int alignSensor(); + /** Motor and sensor alignment to the sensors absolute 0 angle */ + int absoluteZeroAlign(); + + + // Open loop motion control + /** + * Function (iterative) generating open loop movement for target velocity + * it uses voltage_limit variable + * + * @param target_velocity - rad/s + */ + void velocityOpenloop(float target_velocity); + /** + * Function (iterative) generating open loop movement towards the target angle + * it uses voltage_limit and velocity_limit variables + * + * @param target_angle - rad + */ + void angleOpenloop(float target_angle); + // open loop variables + long open_loop_timestamp; +}; + + +#endif diff --git a/minimal_project_examples/atmega328_bldc_openloop/src/common/base_classes/BLDCDriver.h b/minimal_project_examples/atmega328_bldc_openloop/src/common/base_classes/BLDCDriver.h new file mode 100644 index 00000000..708323fb --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_openloop/src/common/base_classes/BLDCDriver.h @@ -0,0 +1,28 @@ +#ifndef BLDCDRIVER_H +#define BLDCDRIVER_H + +class BLDCDriver{ + public: + + /** Initialise hardware */ + virtual int init(); + /** Enable hardware */ + virtual void enable(); + /** Disable hardware */ + virtual void disable(); + + long pwm_frequency; //!< pwm frequency value in hertz + float voltage_power_supply; //!< power supply voltage + float voltage_limit; //!< limiting voltage set to the motor + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + virtual void setPwm(float Ua, float Ub, float Uc); +}; + +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_openloop/src/common/base_classes/FOCMotor.cpp b/minimal_project_examples/atmega328_bldc_openloop/src/common/base_classes/FOCMotor.cpp new file mode 100644 index 00000000..1d1a0f43 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_openloop/src/common/base_classes/FOCMotor.cpp @@ -0,0 +1,241 @@ +#include "FOCMotor.h" + +/** + * Default constructor - setting all variabels to default values + */ +FOCMotor::FOCMotor() +{ + // maximum angular velocity to be used for positioning + velocity_limit = DEF_VEL_LIM; + // maximum voltage to be set to the motor + voltage_limit = DEF_POWER_SUPPLY; + + // index search velocity + velocity_index_search = DEF_INDEX_SEARCH_TARGET_VELOCITY; + // sensor and motor align voltage + voltage_sensor_align = DEF_VOLTAGE_SENSOR_ALIGN; + + // electric angle of comthe zero angle + zero_electric_angle = 0; + + // default modulation is SinePWM + foc_modulation = FOCModulationType::SinePWM; + + // default target value + target = 0; + voltage_d = 0; + voltage_q = 0; + + //monitor_port + monitor_port = nullptr; + //sensor + sensor = nullptr; +} + + +/** + Sensor communication methods +*/ +void FOCMotor::linkSensor(Sensor* _sensor) { + sensor = _sensor; +} +// shaft angle calculation +float FOCMotor::shaftAngle() { + // if no sensor linked return 0 + if(!sensor) return shaft_angle; + return sensor->getAngle(); +} +// shaft velocity calculation +float FOCMotor::shaftVelocity() { + // if no sensor linked return 0 + if(!sensor) return 0; + return LPF_velocity(sensor->getVelocity()); +} + +/** + * Monitoring functions + */ +// function implementing the monitor_port setter +void FOCMotor::useMonitoring(Print &print){ + monitor_port = &print; //operate on the address of print + if(monitor_port ) monitor_port->println("MOT: Monitor enabled!"); +} +// utility function intended to be used with serial plotter to monitor motor variables +// significantly slowing the execution down!!!! +void FOCMotor::monitor() { + if(!monitor_port) return; + switch (controller) { + case ControlType::velocity_openloop: + case ControlType::velocity: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_velocity_sp); + monitor_port->print("\t"); + monitor_port->println(shaft_velocity); + break; + case ControlType::angle_openloop: + case ControlType::angle: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_angle_sp); + monitor_port->print("\t"); + monitor_port->println(shaft_angle); + break; + case ControlType::voltage: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_angle); + monitor_port->print("\t"); + monitor_port->println(shaft_velocity); + break; + } +} + +int FOCMotor::command(String user_command) { + // error flag + int errorFlag = 1; + // if empty string + if(user_command.length() < 1) return errorFlag; + + // parse command letter + char cmd = user_command.charAt(0); + // check if get command + char GET = user_command.charAt(1) == '\n'; + // parse command values + float value = user_command.substring(1).toFloat(); + + // a bit of optimisation of variable memory for Arduino UNO (atmega328) + switch(cmd){ + case 'P': // velocity P gain change + case 'I': // velocity I gain change + case 'D': // velocity D gain change + case 'R': // velocity voltage ramp change + if(monitor_port) monitor_port->print(" PID velocity| "); + break; + case 'F': // velocity Tf low pass filter change + if(monitor_port) monitor_port->print(" LPF velocity| "); + break; + case 'K': // angle loop gain P change + if(monitor_port) monitor_port->print(" P angle| "); + break; + case 'L': // velocity voltage limit change + case 'N': // angle loop gain velocity_limit change + if(monitor_port) monitor_port->print(" Limits| "); + break; + } + + // apply the the command + switch(cmd){ + case 'P': // velocity P gain change + if(monitor_port) monitor_port->print("P: "); + if(!GET) PID_velocity.P = value; + if(monitor_port) monitor_port->println(PID_velocity.P); + break; + case 'I': // velocity I gain change + if(monitor_port) monitor_port->print("I: "); + if(!GET) PID_velocity.I = value; + if(monitor_port) monitor_port->println(PID_velocity.I); + break; + case 'D': // velocity D gain change + if(monitor_port) monitor_port->print("D: "); + if(!GET) PID_velocity.D = value; + if(monitor_port) monitor_port->println(PID_velocity.D); + break; + case 'R': // velocity voltage ramp change + if(monitor_port) monitor_port->print("volt_ramp: "); + if(!GET) PID_velocity.output_ramp = value; + if(monitor_port) monitor_port->println(PID_velocity.output_ramp); + break; + case 'L': // velocity voltage limit change + if(monitor_port) monitor_port->print("volt_limit: "); + if(!GET) { + voltage_limit = value; + PID_velocity.limit = value; + } + if(monitor_port) monitor_port->println(voltage_limit); + break; + case 'F': // velocity Tf low pass filter change + if(monitor_port) monitor_port->print("Tf: "); + if(!GET) LPF_velocity.Tf = value; + if(monitor_port) monitor_port->println(LPF_velocity.Tf); + break; + case 'K': // angle loop gain P change + if(monitor_port) monitor_port->print(" P: "); + if(!GET) P_angle.P = value; + if(monitor_port) monitor_port->println(P_angle.P); + break; + case 'N': // angle loop gain velocity_limit change + if(monitor_port) monitor_port->print("vel_limit: "); + if(!GET){ + velocity_limit = value; + P_angle.limit = value; + } + if(monitor_port) monitor_port->println(velocity_limit); + break; + case 'C': + // change control type + if(monitor_port) monitor_port->print("Control: "); + + if(GET){ // if get command + switch(controller){ + case ControlType::voltage: + if(monitor_port) monitor_port->println("voltage"); + break; + case ControlType::velocity: + if(monitor_port) monitor_port->println("velocity"); + break; + case ControlType::angle: + if(monitor_port) monitor_port->println("angle"); + break; + default: + if(monitor_port) monitor_port->println("open loop"); + } + }else{ // if set command + switch((int)value){ + case 0: + if(monitor_port) monitor_port->println("voltage"); + controller = ControlType::voltage; + break; + case 1: + if(monitor_port) monitor_port->println("velocity"); + controller = ControlType::velocity; + break; + case 2: + if(monitor_port) monitor_port->println("angle"); + controller = ControlType::angle; + break; + default: // not valid command + errorFlag = 0; + } + } + break; + case 'V': // get current values of the state variables + switch((int)value){ + case 0: // get voltage + if(monitor_port) monitor_port->print("Uq: "); + if(monitor_port) monitor_port->println(voltage_q); + break; + case 1: // get velocity + if(monitor_port) monitor_port->print("Velocity: "); + if(monitor_port) monitor_port->println(shaft_velocity); + break; + case 2: // get angle + if(monitor_port) monitor_port->print("Angle: "); + if(monitor_port) monitor_port->println(shaft_angle); + break; + case 3: // get angle + if(monitor_port) monitor_port->print("Target: "); + if(monitor_port) monitor_port->println(target); + break; + default: // not valid command + errorFlag = 0; + } + break; + default: // target change + if(monitor_port) monitor_port->print("Target : "); + target = user_command.toFloat(); + if(monitor_port) monitor_port->println(target); + } + // return 0 if error and 1 if ok + return errorFlag; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_openloop/src/common/base_classes/FOCMotor.h b/minimal_project_examples/atmega328_bldc_openloop/src/common/base_classes/FOCMotor.h new file mode 100644 index 00000000..986e1ab2 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_openloop/src/common/base_classes/FOCMotor.h @@ -0,0 +1,197 @@ +#ifndef FOCMOTOR_H +#define FOCMOTOR_H + +#include "Arduino.h" +#include "Sensor.h" +#include "BLDCDriver.h" + +#include "../time_utils.h" +#include "../foc_utils.h" +#include "../defaults.h" +#include "../pid.h" +#include "../lowpass_filter.h" + + +/** + * Motiron control type + */ +enum ControlType{ + voltage,//!< Torque control using voltage + velocity,//!< Velocity motion control + angle,//!< Position/angle motion control + velocity_openloop, + angle_openloop +}; + +/** + * FOC modulation type + */ +enum FOCModulationType{ + SinePWM, //!< Sinusoidal PWM modulation + SpaceVectorPWM, //!< Space vector modulation method + Trapezoid_120, + Trapezoid_150 +}; + +/** + Generic motor class +*/ +class FOCMotor +{ + public: + /** + * Default constructor - setting all variabels to default values + */ + FOCMotor(); + + /** Motor hardware init function */ + virtual void init()=0; + /** Motor disable function */ + virtual void disable()=0; + /** Motor enable function */ + virtual void enable()=0; + + /** + * Function linking a motor and a sensor + * + * @param sensor Sensor class wrapper for the FOC algorihtm to read the motor angle and velocity + */ + void linkSensor(Sensor* sensor); + + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + * + * - If zero_electric_offset parameter is set the alignment procedure is skipped + * + * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. + * @param sensor_direction sensor natural direction - default is CW + * + */ + virtual int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW)=0; + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + virtual void loopFOC()=0; + /** + * Function executing the control loops set by the controller parameter of the BLDCMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + virtual void move(float target = NOT_SET)=0; + + // State calculation methods + /** Shaft angle calculation in radians [rad] */ + float shaftAngle(); + /** + * Shaft angle calculation function in radian per second [rad/s] + * It implements low pass filtering + */ + float shaftVelocity(); + + // state variables + float target; //!< current target value - depends of the controller + float shaft_angle;//!< current motor angle + float shaft_velocity;//!< current motor velocity + float shaft_velocity_sp;//!< current target velocity + float shaft_angle_sp;//!< current target angle + float voltage_q;//!< current voltage u_q set + float voltage_d;//!< current voltage u_d set + + // motor configuration parameters + float voltage_sensor_align;//!< sensor and motor align voltage parameter + float velocity_index_search;//!< target velocity for index search + int pole_pairs;//!< Motor pole pairs number + + // limiting variables + float voltage_limit; //!< Voltage limitting variable - global limit + float velocity_limit; //!< Velocity limitting variable - global limit + + float zero_electric_angle;//! _2PI ? a_sin - _2PI : a_sin; + return _sin(a_sin); +} + + +// normalizing radian angle to [0,2PI] +float _normalizeAngle(float angle){ + float a = fmod(angle, _2PI); + return a >= 0 ? a : (a + _2PI); +} + +// Electrical angle calculation +float _electricalAngle(float shaft_angle, int pole_pairs) { + return (shaft_angle * pole_pairs); +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_openloop/src/common/foc_utils.h b/minimal_project_examples/atmega328_bldc_openloop/src/common/foc_utils.h new file mode 100644 index 00000000..7da3f7bc --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_openloop/src/common/foc_utils.h @@ -0,0 +1,55 @@ +#ifndef FOCUTILS_LIB_H +#define FOCUTILS_LIB_H + +#include "Arduino.h" + +// sign function +#define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) ) +#define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) +#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) + +// utility defines +#define _2_SQRT3 1.15470053838 +#define _SQRT3 1.73205080757 +#define _1_SQRT3 0.57735026919 +#define _SQRT3_2 0.86602540378 +#define _SQRT2 1.41421356237 +#define _120_D2R 2.09439510239 +#define _PI 3.14159265359 +#define _PI_2 1.57079632679 +#define _PI_3 1.0471975512 +#define _2PI 6.28318530718 +#define _3PI_2 4.71238898038 + +#define NOT_SET -12345.0 + +/** + * Function approximating the sine calculation by using fixed size array + * - execution time ~40us (Arduino UNO) + * + * @param a angle in between 0 and 2PI + */ +float _sin(float a); +/** + * Function approximating cosine calculation by using fixed size array + * - execution time ~50us (Arduino UNO) + * + * @param a angle in between 0 and 2PI + */ +float _cos(float a); + +/** + * normalizing radian angle to [0,2PI] + * @param angle - angle to be normalized + */ +float _normalizeAngle(float angle); + + +/** + * Electrical angle calculation + * + * @param shaft_angle - shaft angle of the motor + * @param pole_pairs - number of pole pairs + */ +float _electricalAngle(float shaft_angle, int pole_pairs); +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_openloop/src/common/lowpass_filter.cpp b/minimal_project_examples/atmega328_bldc_openloop/src/common/lowpass_filter.cpp new file mode 100644 index 00000000..9bf4ab7a --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_openloop/src/common/lowpass_filter.cpp @@ -0,0 +1,25 @@ +#include "lowpass_filter.h" + +LowPassFilter::LowPassFilter(float time_constant) + : Tf(time_constant) + , y_prev(0.0f) +{ + timestamp_prev = _micros(); +} + + +float LowPassFilter::operator() (float x) +{ + unsigned long timestamp = _micros(); + float dt = (timestamp - timestamp_prev)*1e-6f; + + if (dt < 0.0f || dt > 0.5f) + dt = 1e-3f; + + float alpha = Tf/(Tf + dt); + float y = alpha*y_prev + (1.0f - alpha)*x; + + y_prev = y; + timestamp_prev = timestamp; + return y; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_openloop/src/common/lowpass_filter.h b/minimal_project_examples/atmega328_bldc_openloop/src/common/lowpass_filter.h new file mode 100644 index 00000000..7c51be54 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_openloop/src/common/lowpass_filter.h @@ -0,0 +1,29 @@ +#ifndef LOWPASS_FILTER_H +#define LOWPASS_FILTER_H + + +#include "time_utils.h" +#include "foc_utils.h" + + +/** + * Low pass filter class + */ +class LowPassFilter +{ +public: + /** + * @param Tf - Low pass filter time constant + */ + LowPassFilter(float Tf); + ~LowPassFilter() = default; + + float operator() (float x); + float Tf; //!< Low pass filter time constant + +protected: + unsigned long timestamp_prev; //!< Last execution timestamp + float y_prev; //!< filtered value in previous execution step +}; + +#endif // LOWPASS_FILTER_H \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_openloop/src/common/pid.cpp b/minimal_project_examples/atmega328_bldc_openloop/src/common/pid.cpp new file mode 100644 index 00000000..1a0a9828 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_openloop/src/common/pid.cpp @@ -0,0 +1,56 @@ +#include "pid.h" + +PIDController::PIDController(float P, float I, float D, float ramp, float limit) + : P(P) + , I(I) + , D(D) + , output_ramp(ramp) // output derivative limit [volts/second] + , limit(limit) // output supply limit [volts] + , integral_prev(0.0) + , error_prev(0.0) + , output_prev(0.0) +{ + timestamp_prev = _micros()*1e-6; +} + +// PID controller function +float PIDController::operator() (float error){ + // calculate the time from the last call + unsigned long timestamp_now = _micros(); + float Ts = (timestamp_now - timestamp_prev) * 1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + + // u(s) = (P + I/s + Ds)e(s) + // Discrete implementations + // proportional part + // u_p = P *e(k) + float proportional = P * error; + // Tustin transform of the integral part + // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) + float integral = integral_prev + I*Ts*0.5*(error + error_prev); + // antiwindup - limit the output voltage_q + integral = _constrain(integral, -limit, limit); + // Discrete derivation + // u_dk = D(ek - ek_1)/Ts + float derivative = D*(error - error_prev)/Ts; + + // sum all the components + float output = proportional + integral + derivative; + // antiwindup - limit the output variable + output = _constrain(output, -limit, limit); + + // limit the acceleration by ramping the output + float output_rate = (output - output_prev)/Ts; + if (output_rate > output_ramp) + output = output_prev + output_ramp*Ts; + else if (output_rate < -output_ramp) + output = output_prev - output_ramp*Ts; + + // saving for the next pass + integral_prev = integral; + output_prev = output; + error_prev = error; + timestamp_prev = timestamp_now; + return output; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_openloop/src/common/pid.h b/minimal_project_examples/atmega328_bldc_openloop/src/common/pid.h new file mode 100644 index 00000000..0e9ddf54 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_openloop/src/common/pid.h @@ -0,0 +1,40 @@ +#ifndef PID_H +#define PID_H + + +#include "time_utils.h" +#include "foc_utils.h" + +/** + * PID controller class + */ +class PIDController +{ +public: + /** + * + * @param P - Proportional gain + * @param I - Integral gain + * @param D - Derivative gain + * @param ramp - Maximum speed of change of the output value + * @param limit - Maximum output value + */ + PIDController(float P, float I, float D, float ramp, float limit); + ~PIDController() = default; + + float operator() (float error); + + float P; //!< Proportional gain + float I; //!< Integral gain + float D; //!< Derivative gain + float output_ramp; //!< Maximum speed of change of the output value + float limit; //!< Maximum output value + +protected: + float integral_prev; //!< last integral component value + float error_prev; //!< last tracking error value + float timestamp_prev; //!< Last execution timestamp + float output_prev; //!< last pid output value +}; + +#endif // PID_H \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_openloop/src/common/time_utils.cpp b/minimal_project_examples/atmega328_bldc_openloop/src/common/time_utils.cpp new file mode 100644 index 00000000..181d03cf --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_openloop/src/common/time_utils.cpp @@ -0,0 +1,31 @@ +#include "time_utils.h" + +// function buffering delay() +// arduino uno function doesn't work well with interrupts +void _delay(unsigned long ms){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) + // if arduino uno and other atmega328p chips + // use while instad of delay, + // due to wrong measurement based on changed timer0 + unsigned long t = _micros() + ms*1000; + while( _micros() < t ){}; +#else + // regular micros + delay(ms); +#endif +} + + +// function buffering _micros() +// arduino function doesn't work well with interrupts +unsigned long _micros(){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) +// if arduino uno and other atmega328p chips + //return the value based on the prescaler + if((TCCR0B & 0b00000111) == 0x01) return (micros()/32); + else return (micros()); +#else + // regular micros + return micros(); +#endif +} diff --git a/minimal_project_examples/atmega328_bldc_openloop/src/common/time_utils.h b/minimal_project_examples/atmega328_bldc_openloop/src/common/time_utils.h new file mode 100644 index 00000000..143d4853 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_openloop/src/common/time_utils.h @@ -0,0 +1,22 @@ +#ifndef TIME_UTILS_H +#define TIME_UTILS_H + +#include "foc_utils.h" + +/** + * Function implementing delay() function in milliseconds + * - blocking function + * - hardware specific + + * @param ms number of milliseconds to wait + */ +void _delay(unsigned long ms); + +/** + * Function implementing timestamp getting function in microseconds + * hardware specific + */ +unsigned long _micros(); + + +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_openloop/src/drivers/BLDCDriver3PWM.cpp b/minimal_project_examples/atmega328_bldc_openloop/src/drivers/BLDCDriver3PWM.cpp new file mode 100644 index 00000000..a3fc052d --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_openloop/src/drivers/BLDCDriver3PWM.cpp @@ -0,0 +1,72 @@ +#include "BLDCDriver3PWM.h" + +BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en){ + // Pin initialization + pwmA = phA; + pwmB = phB; + pwmC = phC; + + // enable_pin pin + enable_pin = en; + + // default power-supply value + voltage_power_supply = DEF_POWER_SUPPLY; + voltage_limit = NOT_SET; + +} + +// enable motor driver +void BLDCDriver3PWM::enable(){ + // enable_pin the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, HIGH); + // set zero to PWM + setPwm(0,0,0); +} + +// disable motor driver +void BLDCDriver3PWM::disable() +{ + // set zero to PWM + setPwm(0, 0, 0); + // disable the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, LOW); + +} + +// init hardware pins +int BLDCDriver3PWM::init() { + // a bit of separation + _delay(1000); + + // PWM pins + pinMode(pwmA, OUTPUT); + pinMode(pwmB, OUTPUT); + pinMode(pwmC, OUTPUT); + if(enable_pin != NOT_SET) pinMode(enable_pin, OUTPUT); + + + // sanity check for the voltage limit configuration + if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + + // Set the pwm frequency to the pins + // hardware specific function - depending on driver and mcu + _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC); + return 0; +} + + +// Set voltage to the pwm pin +void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { + // limit the voltage in driver + Ua = _constrain(Ua, 0.0, voltage_limit); + Ub = _constrain(Ub, 0.0, voltage_limit); + Uc = _constrain(Uc, 0.0, voltage_limit); + // calculate duty cycle + // limited in [0,1] + float dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 ); + float dc_b = _constrain(Ub / voltage_power_supply, 0.0 , 1.0 ); + float dc_c = _constrain(Uc / voltage_power_supply, 0.0 , 1.0 ); + // hardware specific writing + // hardware specific function - depending on driver and mcu + _writeDutyCycle3PWM(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC); +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_openloop/src/drivers/BLDCDriver3PWM.h b/minimal_project_examples/atmega328_bldc_openloop/src/drivers/BLDCDriver3PWM.h new file mode 100644 index 00000000..b6f7d7f8 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_openloop/src/drivers/BLDCDriver3PWM.h @@ -0,0 +1,52 @@ +#ifndef BLDCDriver3PWM_h +#define BLDCDriver3PWM_h + +#include "../common/base_classes/BLDCDriver.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "hardware_api.h" + +/** + 3 pwm bldc driver class +*/ +class BLDCDriver3PWM: public BLDCDriver +{ + public: + /** + BLDCDriver class constructor + @param phA A phase pwm pin + @param phB B phase pwm pin + @param phC C phase pwm pin + @param en enable pin (optional input) + */ + BLDCDriver3PWM(int phA,int phB,int phC, int en = NOT_SET); + + /** Motor hardware init function */ + int init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + // hardware variables + int pwmA; //!< phase A pwm pin number + int pwmB; //!< phase B pwm pin number + int pwmC; //!< phase C pwm pin number + int enable_pin; //!< enable pin number + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + void setPwm(float Ua, float Ub, float Uc) override; + + private: + +}; + + +#endif diff --git a/minimal_project_examples/atmega328_bldc_openloop/src/drivers/hardware_api.h b/minimal_project_examples/atmega328_bldc_openloop/src/drivers/hardware_api.h new file mode 100644 index 00000000..01c03867 --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_openloop/src/drivers/hardware_api.h @@ -0,0 +1,123 @@ +#ifndef HARDWARE_UTILS_H +#define HARDWARE_UTILS_H + +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA bldc driver + * @param pinB pinB bldc driver + */ +void _configure2PWM(long pwm_frequency, const int pinA, const int pinB); + +/** + * Configuring PWM frequency, resolution and alignment + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA bldc driver + * @param pinB pinB bldc driver + * @param pinC pinC bldc driver + */ +void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC); + +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 4PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pin1A pin1A stepper driver + * @param pin1B pin1B stepper driver + * @param pin2A pin2A stepper driver + * @param pin2B pin2B stepper driver + */ +void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B); + +/** + * Configuring PWM frequency, resolution and alignment + * - BLDC driver - 6PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low - if applicable + * @param pinA_h pinA high-side bldc driver + * @param pinA_l pinA low-side bldc driver + * @param pinB_h pinA high-side bldc driver + * @param pinB_l pinA low-side bldc driver + * @param pinC_h pinA high-side bldc driver + * @param pinC_l pinA low-side bldc driver + * + * @return 0 if config good, -1 if failed + */ +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param pinA phase A hardware pin number + * @param pinB phase B hardware pin number + */ +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param pinA phase A hardware pin number + * @param pinB phase B hardware pin number + * @param pinC phase C hardware pin number + */ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 4PWM setting + * - hardware specific + * + * @param dc_1a duty cycle phase 1A [0, 1] + * @param dc_1b duty cycle phase 1B [0, 1] + * @param dc_2a duty cycle phase 2A [0, 1] + * @param dc_2b duty cycle phase 2B [0, 1] + * @param pin1A phase 1A hardware pin number + * @param pin1B phase 1B hardware pin number + * @param pin2A phase 2A hardware pin number + * @param pin2B phase 2B hardware pin number + */ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B); + + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 6PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low + * @param pinA_h phase A high-side hardware pin number + * @param pinA_l phase A low-side hardware pin number + * @param pinB_h phase B high-side hardware pin number + * @param pinB_l phase B low-side hardware pin number + * @param pinC_h phase C high-side hardware pin number + * @param pinC_l phase C low-side hardware pin number + * + */ +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l); + +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_bldc_openloop/src/drivers/hardware_specific/atmega328_mcu.cpp b/minimal_project_examples/atmega328_bldc_openloop/src/drivers/hardware_specific/atmega328_mcu.cpp new file mode 100644 index 00000000..24fe563e --- /dev/null +++ b/minimal_project_examples/atmega328_bldc_openloop/src/drivers/hardware_specific/atmega328_mcu.cpp @@ -0,0 +1,153 @@ +#include "../hardware_api.h" + +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) + +// set pwm frequency to 32KHz +void _pinHighFrequency(const int pin){ + // High PWM frequency + // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328 + if (pin == 5 || pin == 6 ) { + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 + } + if (pin == 9 || pin == 10 ) + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 + if (pin == 3 || pin == 11) + TCCR2B = ((TCCR2B & 0b11111000) | 0x01);// set prescaler to 1 + +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); +} + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); + _pinHighFrequency(pinC); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); + analogWrite(pinC, 255.0*dc_c); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pin1A); + _pinHighFrequency(pin1B); + _pinHighFrequency(pin2A); + _pinHighFrequency(pin2B); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0*dc_1a); + analogWrite(pin1B, 255.0*dc_1b); + analogWrite(pin2A, 255.0*dc_2a); + analogWrite(pin2B, 255.0*dc_2b); +} + + +// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm +int _configureComplementaryPair(int pinH, int pinL) { + if( (pinH == 5 && pinL == 6 ) || (pinH == 6 && pinL == 5 ) ){ + // configure the pwm phase-corrected mode + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); + // configure complementary pwm on low side + if(pinH == 6 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ; + else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ; + // set prescaler to 1 - 32kHz freq + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); + }else if( (pinH == 9 && pinL == 10 ) || (pinH == 10 && pinL == 9 ) ){ + // set prescaler to 1 - 32kHz freq + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); + // configure complementary pwm on low side + if(pinH == 9 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ; + else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ; + }else if((pinH == 3 && pinL == 11 ) || (pinH == 11 && pinL == 3 ) ){ + // set prescaler to 1 - 32kHz freq + TCCR2B = ((TCCR2B & 0b11111000) | 0x01); + // configure complementary pwm on low side + if(pinH == 11 ) TCCR2A = 0b10110000 | (TCCR2A & 0b00001111) ; + else TCCR2A = 0b11100000 | (TCCR2A & 0b00001111) ; + }else{ + return -1; + } + return 0; +} + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arudino/ATmega328 +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + // High PWM frequency + // - always max 32kHz + int ret_flag = 0; + ret_flag += _configureComplementaryPair(pinA_h, pinA_l); + ret_flag += _configureComplementaryPair(pinB_h, pinB_l); + ret_flag += _configureComplementaryPair(pinC_h, pinC_l); + return ret_flag; // returns -1 if not well configured +} + +// function setting the +void _setPwmPair(int pinH, int pinL, float val, int dead_time) +{ + int pwm_h = _constrain(val-dead_time/2,0,255); + int pwm_l = _constrain(val+dead_time/2,0,255); + + analogWrite(pinH, pwm_h); + if(pwm_l == 255 || pwm_l == 0) + digitalWrite(pinL, pwm_l ? LOW : HIGH); + else + analogWrite(pinL, pwm_l); +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arudino/ATmega328 +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); + _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); + _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0); +} + +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_driver_standalone/atmega328_driver_standalone.ino b/minimal_project_examples/atmega328_driver_standalone/atmega328_driver_standalone.ino new file mode 100644 index 00000000..e5d64dd2 --- /dev/null +++ b/minimal_project_examples/atmega328_driver_standalone/atmega328_driver_standalone.ino @@ -0,0 +1,32 @@ +#include "src/drivers/BLDCDriver3PWM.h" + +// BLDC driver instance +BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8); + +void setup() { + + // pwm frequency to be used [Hz] + // for atmega328 fixed to 32kHz + // esp32/stm32/teensy configurable + driver.pwm_frequency = 50000; + // power supply voltage [V] + driver.voltage_power_supply = 12; + // Max DC voltage allowed - default voltage_power_supply + driver.voltage_limit = 12; + + // driver init + driver.init(); + + // enable driver + driver.enable(); + + _delay(1000); +} + +void loop() { + // setting pwm + // phase A: 3V + // phase B: 6V + // phase C: 5V + driver.setPwm(3,6,5); +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_driver_standalone/src/common/base_classes/BLDCDriver.h b/minimal_project_examples/atmega328_driver_standalone/src/common/base_classes/BLDCDriver.h new file mode 100644 index 00000000..708323fb --- /dev/null +++ b/minimal_project_examples/atmega328_driver_standalone/src/common/base_classes/BLDCDriver.h @@ -0,0 +1,28 @@ +#ifndef BLDCDRIVER_H +#define BLDCDRIVER_H + +class BLDCDriver{ + public: + + /** Initialise hardware */ + virtual int init(); + /** Enable hardware */ + virtual void enable(); + /** Disable hardware */ + virtual void disable(); + + long pwm_frequency; //!< pwm frequency value in hertz + float voltage_power_supply; //!< power supply voltage + float voltage_limit; //!< limiting voltage set to the motor + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + virtual void setPwm(float Ua, float Ub, float Uc); +}; + +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_driver_standalone/src/common/base_classes/FOCMotor.cpp b/minimal_project_examples/atmega328_driver_standalone/src/common/base_classes/FOCMotor.cpp new file mode 100644 index 00000000..1d1a0f43 --- /dev/null +++ b/minimal_project_examples/atmega328_driver_standalone/src/common/base_classes/FOCMotor.cpp @@ -0,0 +1,241 @@ +#include "FOCMotor.h" + +/** + * Default constructor - setting all variabels to default values + */ +FOCMotor::FOCMotor() +{ + // maximum angular velocity to be used for positioning + velocity_limit = DEF_VEL_LIM; + // maximum voltage to be set to the motor + voltage_limit = DEF_POWER_SUPPLY; + + // index search velocity + velocity_index_search = DEF_INDEX_SEARCH_TARGET_VELOCITY; + // sensor and motor align voltage + voltage_sensor_align = DEF_VOLTAGE_SENSOR_ALIGN; + + // electric angle of comthe zero angle + zero_electric_angle = 0; + + // default modulation is SinePWM + foc_modulation = FOCModulationType::SinePWM; + + // default target value + target = 0; + voltage_d = 0; + voltage_q = 0; + + //monitor_port + monitor_port = nullptr; + //sensor + sensor = nullptr; +} + + +/** + Sensor communication methods +*/ +void FOCMotor::linkSensor(Sensor* _sensor) { + sensor = _sensor; +} +// shaft angle calculation +float FOCMotor::shaftAngle() { + // if no sensor linked return 0 + if(!sensor) return shaft_angle; + return sensor->getAngle(); +} +// shaft velocity calculation +float FOCMotor::shaftVelocity() { + // if no sensor linked return 0 + if(!sensor) return 0; + return LPF_velocity(sensor->getVelocity()); +} + +/** + * Monitoring functions + */ +// function implementing the monitor_port setter +void FOCMotor::useMonitoring(Print &print){ + monitor_port = &print; //operate on the address of print + if(monitor_port ) monitor_port->println("MOT: Monitor enabled!"); +} +// utility function intended to be used with serial plotter to monitor motor variables +// significantly slowing the execution down!!!! +void FOCMotor::monitor() { + if(!monitor_port) return; + switch (controller) { + case ControlType::velocity_openloop: + case ControlType::velocity: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_velocity_sp); + monitor_port->print("\t"); + monitor_port->println(shaft_velocity); + break; + case ControlType::angle_openloop: + case ControlType::angle: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_angle_sp); + monitor_port->print("\t"); + monitor_port->println(shaft_angle); + break; + case ControlType::voltage: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_angle); + monitor_port->print("\t"); + monitor_port->println(shaft_velocity); + break; + } +} + +int FOCMotor::command(String user_command) { + // error flag + int errorFlag = 1; + // if empty string + if(user_command.length() < 1) return errorFlag; + + // parse command letter + char cmd = user_command.charAt(0); + // check if get command + char GET = user_command.charAt(1) == '\n'; + // parse command values + float value = user_command.substring(1).toFloat(); + + // a bit of optimisation of variable memory for Arduino UNO (atmega328) + switch(cmd){ + case 'P': // velocity P gain change + case 'I': // velocity I gain change + case 'D': // velocity D gain change + case 'R': // velocity voltage ramp change + if(monitor_port) monitor_port->print(" PID velocity| "); + break; + case 'F': // velocity Tf low pass filter change + if(monitor_port) monitor_port->print(" LPF velocity| "); + break; + case 'K': // angle loop gain P change + if(monitor_port) monitor_port->print(" P angle| "); + break; + case 'L': // velocity voltage limit change + case 'N': // angle loop gain velocity_limit change + if(monitor_port) monitor_port->print(" Limits| "); + break; + } + + // apply the the command + switch(cmd){ + case 'P': // velocity P gain change + if(monitor_port) monitor_port->print("P: "); + if(!GET) PID_velocity.P = value; + if(monitor_port) monitor_port->println(PID_velocity.P); + break; + case 'I': // velocity I gain change + if(monitor_port) monitor_port->print("I: "); + if(!GET) PID_velocity.I = value; + if(monitor_port) monitor_port->println(PID_velocity.I); + break; + case 'D': // velocity D gain change + if(monitor_port) monitor_port->print("D: "); + if(!GET) PID_velocity.D = value; + if(monitor_port) monitor_port->println(PID_velocity.D); + break; + case 'R': // velocity voltage ramp change + if(monitor_port) monitor_port->print("volt_ramp: "); + if(!GET) PID_velocity.output_ramp = value; + if(monitor_port) monitor_port->println(PID_velocity.output_ramp); + break; + case 'L': // velocity voltage limit change + if(monitor_port) monitor_port->print("volt_limit: "); + if(!GET) { + voltage_limit = value; + PID_velocity.limit = value; + } + if(monitor_port) monitor_port->println(voltage_limit); + break; + case 'F': // velocity Tf low pass filter change + if(monitor_port) monitor_port->print("Tf: "); + if(!GET) LPF_velocity.Tf = value; + if(monitor_port) monitor_port->println(LPF_velocity.Tf); + break; + case 'K': // angle loop gain P change + if(monitor_port) monitor_port->print(" P: "); + if(!GET) P_angle.P = value; + if(monitor_port) monitor_port->println(P_angle.P); + break; + case 'N': // angle loop gain velocity_limit change + if(monitor_port) monitor_port->print("vel_limit: "); + if(!GET){ + velocity_limit = value; + P_angle.limit = value; + } + if(monitor_port) monitor_port->println(velocity_limit); + break; + case 'C': + // change control type + if(monitor_port) monitor_port->print("Control: "); + + if(GET){ // if get command + switch(controller){ + case ControlType::voltage: + if(monitor_port) monitor_port->println("voltage"); + break; + case ControlType::velocity: + if(monitor_port) monitor_port->println("velocity"); + break; + case ControlType::angle: + if(monitor_port) monitor_port->println("angle"); + break; + default: + if(monitor_port) monitor_port->println("open loop"); + } + }else{ // if set command + switch((int)value){ + case 0: + if(monitor_port) monitor_port->println("voltage"); + controller = ControlType::voltage; + break; + case 1: + if(monitor_port) monitor_port->println("velocity"); + controller = ControlType::velocity; + break; + case 2: + if(monitor_port) monitor_port->println("angle"); + controller = ControlType::angle; + break; + default: // not valid command + errorFlag = 0; + } + } + break; + case 'V': // get current values of the state variables + switch((int)value){ + case 0: // get voltage + if(monitor_port) monitor_port->print("Uq: "); + if(monitor_port) monitor_port->println(voltage_q); + break; + case 1: // get velocity + if(monitor_port) monitor_port->print("Velocity: "); + if(monitor_port) monitor_port->println(shaft_velocity); + break; + case 2: // get angle + if(monitor_port) monitor_port->print("Angle: "); + if(monitor_port) monitor_port->println(shaft_angle); + break; + case 3: // get angle + if(monitor_port) monitor_port->print("Target: "); + if(monitor_port) monitor_port->println(target); + break; + default: // not valid command + errorFlag = 0; + } + break; + default: // target change + if(monitor_port) monitor_port->print("Target : "); + target = user_command.toFloat(); + if(monitor_port) monitor_port->println(target); + } + // return 0 if error and 1 if ok + return errorFlag; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_driver_standalone/src/common/base_classes/FOCMotor.h b/minimal_project_examples/atmega328_driver_standalone/src/common/base_classes/FOCMotor.h new file mode 100644 index 00000000..986e1ab2 --- /dev/null +++ b/minimal_project_examples/atmega328_driver_standalone/src/common/base_classes/FOCMotor.h @@ -0,0 +1,197 @@ +#ifndef FOCMOTOR_H +#define FOCMOTOR_H + +#include "Arduino.h" +#include "Sensor.h" +#include "BLDCDriver.h" + +#include "../time_utils.h" +#include "../foc_utils.h" +#include "../defaults.h" +#include "../pid.h" +#include "../lowpass_filter.h" + + +/** + * Motiron control type + */ +enum ControlType{ + voltage,//!< Torque control using voltage + velocity,//!< Velocity motion control + angle,//!< Position/angle motion control + velocity_openloop, + angle_openloop +}; + +/** + * FOC modulation type + */ +enum FOCModulationType{ + SinePWM, //!< Sinusoidal PWM modulation + SpaceVectorPWM, //!< Space vector modulation method + Trapezoid_120, + Trapezoid_150 +}; + +/** + Generic motor class +*/ +class FOCMotor +{ + public: + /** + * Default constructor - setting all variabels to default values + */ + FOCMotor(); + + /** Motor hardware init function */ + virtual void init()=0; + /** Motor disable function */ + virtual void disable()=0; + /** Motor enable function */ + virtual void enable()=0; + + /** + * Function linking a motor and a sensor + * + * @param sensor Sensor class wrapper for the FOC algorihtm to read the motor angle and velocity + */ + void linkSensor(Sensor* sensor); + + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + * + * - If zero_electric_offset parameter is set the alignment procedure is skipped + * + * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. + * @param sensor_direction sensor natural direction - default is CW + * + */ + virtual int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW)=0; + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + virtual void loopFOC()=0; + /** + * Function executing the control loops set by the controller parameter of the BLDCMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + virtual void move(float target = NOT_SET)=0; + + // State calculation methods + /** Shaft angle calculation in radians [rad] */ + float shaftAngle(); + /** + * Shaft angle calculation function in radian per second [rad/s] + * It implements low pass filtering + */ + float shaftVelocity(); + + // state variables + float target; //!< current target value - depends of the controller + float shaft_angle;//!< current motor angle + float shaft_velocity;//!< current motor velocity + float shaft_velocity_sp;//!< current target velocity + float shaft_angle_sp;//!< current target angle + float voltage_q;//!< current voltage u_q set + float voltage_d;//!< current voltage u_d set + + // motor configuration parameters + float voltage_sensor_align;//!< sensor and motor align voltage parameter + float velocity_index_search;//!< target velocity for index search + int pole_pairs;//!< Motor pole pairs number + + // limiting variables + float voltage_limit; //!< Voltage limitting variable - global limit + float velocity_limit; //!< Velocity limitting variable - global limit + + float zero_electric_angle;//! _2PI ? a_sin - _2PI : a_sin; + return _sin(a_sin); +} + + +// normalizing radian angle to [0,2PI] +float _normalizeAngle(float angle){ + float a = fmod(angle, _2PI); + return a >= 0 ? a : (a + _2PI); +} + +// Electrical angle calculation +float _electricalAngle(float shaft_angle, int pole_pairs) { + return (shaft_angle * pole_pairs); +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_driver_standalone/src/common/foc_utils.h b/minimal_project_examples/atmega328_driver_standalone/src/common/foc_utils.h new file mode 100644 index 00000000..7da3f7bc --- /dev/null +++ b/minimal_project_examples/atmega328_driver_standalone/src/common/foc_utils.h @@ -0,0 +1,55 @@ +#ifndef FOCUTILS_LIB_H +#define FOCUTILS_LIB_H + +#include "Arduino.h" + +// sign function +#define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) ) +#define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) +#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) + +// utility defines +#define _2_SQRT3 1.15470053838 +#define _SQRT3 1.73205080757 +#define _1_SQRT3 0.57735026919 +#define _SQRT3_2 0.86602540378 +#define _SQRT2 1.41421356237 +#define _120_D2R 2.09439510239 +#define _PI 3.14159265359 +#define _PI_2 1.57079632679 +#define _PI_3 1.0471975512 +#define _2PI 6.28318530718 +#define _3PI_2 4.71238898038 + +#define NOT_SET -12345.0 + +/** + * Function approximating the sine calculation by using fixed size array + * - execution time ~40us (Arduino UNO) + * + * @param a angle in between 0 and 2PI + */ +float _sin(float a); +/** + * Function approximating cosine calculation by using fixed size array + * - execution time ~50us (Arduino UNO) + * + * @param a angle in between 0 and 2PI + */ +float _cos(float a); + +/** + * normalizing radian angle to [0,2PI] + * @param angle - angle to be normalized + */ +float _normalizeAngle(float angle); + + +/** + * Electrical angle calculation + * + * @param shaft_angle - shaft angle of the motor + * @param pole_pairs - number of pole pairs + */ +float _electricalAngle(float shaft_angle, int pole_pairs); +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_driver_standalone/src/common/lowpass_filter.cpp b/minimal_project_examples/atmega328_driver_standalone/src/common/lowpass_filter.cpp new file mode 100644 index 00000000..9bf4ab7a --- /dev/null +++ b/minimal_project_examples/atmega328_driver_standalone/src/common/lowpass_filter.cpp @@ -0,0 +1,25 @@ +#include "lowpass_filter.h" + +LowPassFilter::LowPassFilter(float time_constant) + : Tf(time_constant) + , y_prev(0.0f) +{ + timestamp_prev = _micros(); +} + + +float LowPassFilter::operator() (float x) +{ + unsigned long timestamp = _micros(); + float dt = (timestamp - timestamp_prev)*1e-6f; + + if (dt < 0.0f || dt > 0.5f) + dt = 1e-3f; + + float alpha = Tf/(Tf + dt); + float y = alpha*y_prev + (1.0f - alpha)*x; + + y_prev = y; + timestamp_prev = timestamp; + return y; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_driver_standalone/src/common/lowpass_filter.h b/minimal_project_examples/atmega328_driver_standalone/src/common/lowpass_filter.h new file mode 100644 index 00000000..7c51be54 --- /dev/null +++ b/minimal_project_examples/atmega328_driver_standalone/src/common/lowpass_filter.h @@ -0,0 +1,29 @@ +#ifndef LOWPASS_FILTER_H +#define LOWPASS_FILTER_H + + +#include "time_utils.h" +#include "foc_utils.h" + + +/** + * Low pass filter class + */ +class LowPassFilter +{ +public: + /** + * @param Tf - Low pass filter time constant + */ + LowPassFilter(float Tf); + ~LowPassFilter() = default; + + float operator() (float x); + float Tf; //!< Low pass filter time constant + +protected: + unsigned long timestamp_prev; //!< Last execution timestamp + float y_prev; //!< filtered value in previous execution step +}; + +#endif // LOWPASS_FILTER_H \ No newline at end of file diff --git a/minimal_project_examples/atmega328_driver_standalone/src/common/pid.cpp b/minimal_project_examples/atmega328_driver_standalone/src/common/pid.cpp new file mode 100644 index 00000000..1a0a9828 --- /dev/null +++ b/minimal_project_examples/atmega328_driver_standalone/src/common/pid.cpp @@ -0,0 +1,56 @@ +#include "pid.h" + +PIDController::PIDController(float P, float I, float D, float ramp, float limit) + : P(P) + , I(I) + , D(D) + , output_ramp(ramp) // output derivative limit [volts/second] + , limit(limit) // output supply limit [volts] + , integral_prev(0.0) + , error_prev(0.0) + , output_prev(0.0) +{ + timestamp_prev = _micros()*1e-6; +} + +// PID controller function +float PIDController::operator() (float error){ + // calculate the time from the last call + unsigned long timestamp_now = _micros(); + float Ts = (timestamp_now - timestamp_prev) * 1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + + // u(s) = (P + I/s + Ds)e(s) + // Discrete implementations + // proportional part + // u_p = P *e(k) + float proportional = P * error; + // Tustin transform of the integral part + // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) + float integral = integral_prev + I*Ts*0.5*(error + error_prev); + // antiwindup - limit the output voltage_q + integral = _constrain(integral, -limit, limit); + // Discrete derivation + // u_dk = D(ek - ek_1)/Ts + float derivative = D*(error - error_prev)/Ts; + + // sum all the components + float output = proportional + integral + derivative; + // antiwindup - limit the output variable + output = _constrain(output, -limit, limit); + + // limit the acceleration by ramping the output + float output_rate = (output - output_prev)/Ts; + if (output_rate > output_ramp) + output = output_prev + output_ramp*Ts; + else if (output_rate < -output_ramp) + output = output_prev - output_ramp*Ts; + + // saving for the next pass + integral_prev = integral; + output_prev = output; + error_prev = error; + timestamp_prev = timestamp_now; + return output; +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_driver_standalone/src/common/pid.h b/minimal_project_examples/atmega328_driver_standalone/src/common/pid.h new file mode 100644 index 00000000..0e9ddf54 --- /dev/null +++ b/minimal_project_examples/atmega328_driver_standalone/src/common/pid.h @@ -0,0 +1,40 @@ +#ifndef PID_H +#define PID_H + + +#include "time_utils.h" +#include "foc_utils.h" + +/** + * PID controller class + */ +class PIDController +{ +public: + /** + * + * @param P - Proportional gain + * @param I - Integral gain + * @param D - Derivative gain + * @param ramp - Maximum speed of change of the output value + * @param limit - Maximum output value + */ + PIDController(float P, float I, float D, float ramp, float limit); + ~PIDController() = default; + + float operator() (float error); + + float P; //!< Proportional gain + float I; //!< Integral gain + float D; //!< Derivative gain + float output_ramp; //!< Maximum speed of change of the output value + float limit; //!< Maximum output value + +protected: + float integral_prev; //!< last integral component value + float error_prev; //!< last tracking error value + float timestamp_prev; //!< Last execution timestamp + float output_prev; //!< last pid output value +}; + +#endif // PID_H \ No newline at end of file diff --git a/minimal_project_examples/atmega328_driver_standalone/src/common/time_utils.cpp b/minimal_project_examples/atmega328_driver_standalone/src/common/time_utils.cpp new file mode 100644 index 00000000..181d03cf --- /dev/null +++ b/minimal_project_examples/atmega328_driver_standalone/src/common/time_utils.cpp @@ -0,0 +1,31 @@ +#include "time_utils.h" + +// function buffering delay() +// arduino uno function doesn't work well with interrupts +void _delay(unsigned long ms){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) + // if arduino uno and other atmega328p chips + // use while instad of delay, + // due to wrong measurement based on changed timer0 + unsigned long t = _micros() + ms*1000; + while( _micros() < t ){}; +#else + // regular micros + delay(ms); +#endif +} + + +// function buffering _micros() +// arduino function doesn't work well with interrupts +unsigned long _micros(){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) +// if arduino uno and other atmega328p chips + //return the value based on the prescaler + if((TCCR0B & 0b00000111) == 0x01) return (micros()/32); + else return (micros()); +#else + // regular micros + return micros(); +#endif +} diff --git a/minimal_project_examples/atmega328_driver_standalone/src/common/time_utils.h b/minimal_project_examples/atmega328_driver_standalone/src/common/time_utils.h new file mode 100644 index 00000000..143d4853 --- /dev/null +++ b/minimal_project_examples/atmega328_driver_standalone/src/common/time_utils.h @@ -0,0 +1,22 @@ +#ifndef TIME_UTILS_H +#define TIME_UTILS_H + +#include "foc_utils.h" + +/** + * Function implementing delay() function in milliseconds + * - blocking function + * - hardware specific + + * @param ms number of milliseconds to wait + */ +void _delay(unsigned long ms); + +/** + * Function implementing timestamp getting function in microseconds + * hardware specific + */ +unsigned long _micros(); + + +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_driver_standalone/src/drivers/BLDCDriver3PWM.cpp b/minimal_project_examples/atmega328_driver_standalone/src/drivers/BLDCDriver3PWM.cpp new file mode 100644 index 00000000..a3fc052d --- /dev/null +++ b/minimal_project_examples/atmega328_driver_standalone/src/drivers/BLDCDriver3PWM.cpp @@ -0,0 +1,72 @@ +#include "BLDCDriver3PWM.h" + +BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en){ + // Pin initialization + pwmA = phA; + pwmB = phB; + pwmC = phC; + + // enable_pin pin + enable_pin = en; + + // default power-supply value + voltage_power_supply = DEF_POWER_SUPPLY; + voltage_limit = NOT_SET; + +} + +// enable motor driver +void BLDCDriver3PWM::enable(){ + // enable_pin the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, HIGH); + // set zero to PWM + setPwm(0,0,0); +} + +// disable motor driver +void BLDCDriver3PWM::disable() +{ + // set zero to PWM + setPwm(0, 0, 0); + // disable the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, LOW); + +} + +// init hardware pins +int BLDCDriver3PWM::init() { + // a bit of separation + _delay(1000); + + // PWM pins + pinMode(pwmA, OUTPUT); + pinMode(pwmB, OUTPUT); + pinMode(pwmC, OUTPUT); + if(enable_pin != NOT_SET) pinMode(enable_pin, OUTPUT); + + + // sanity check for the voltage limit configuration + if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + + // Set the pwm frequency to the pins + // hardware specific function - depending on driver and mcu + _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC); + return 0; +} + + +// Set voltage to the pwm pin +void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { + // limit the voltage in driver + Ua = _constrain(Ua, 0.0, voltage_limit); + Ub = _constrain(Ub, 0.0, voltage_limit); + Uc = _constrain(Uc, 0.0, voltage_limit); + // calculate duty cycle + // limited in [0,1] + float dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 ); + float dc_b = _constrain(Ub / voltage_power_supply, 0.0 , 1.0 ); + float dc_c = _constrain(Uc / voltage_power_supply, 0.0 , 1.0 ); + // hardware specific writing + // hardware specific function - depending on driver and mcu + _writeDutyCycle3PWM(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC); +} \ No newline at end of file diff --git a/minimal_project_examples/atmega328_driver_standalone/src/drivers/BLDCDriver3PWM.h b/minimal_project_examples/atmega328_driver_standalone/src/drivers/BLDCDriver3PWM.h new file mode 100644 index 00000000..b6f7d7f8 --- /dev/null +++ b/minimal_project_examples/atmega328_driver_standalone/src/drivers/BLDCDriver3PWM.h @@ -0,0 +1,52 @@ +#ifndef BLDCDriver3PWM_h +#define BLDCDriver3PWM_h + +#include "../common/base_classes/BLDCDriver.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "hardware_api.h" + +/** + 3 pwm bldc driver class +*/ +class BLDCDriver3PWM: public BLDCDriver +{ + public: + /** + BLDCDriver class constructor + @param phA A phase pwm pin + @param phB B phase pwm pin + @param phC C phase pwm pin + @param en enable pin (optional input) + */ + BLDCDriver3PWM(int phA,int phB,int phC, int en = NOT_SET); + + /** Motor hardware init function */ + int init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + // hardware variables + int pwmA; //!< phase A pwm pin number + int pwmB; //!< phase B pwm pin number + int pwmC; //!< phase C pwm pin number + int enable_pin; //!< enable pin number + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + void setPwm(float Ua, float Ub, float Uc) override; + + private: + +}; + + +#endif diff --git a/minimal_project_examples/atmega328_driver_standalone/src/drivers/hardware_api.h b/minimal_project_examples/atmega328_driver_standalone/src/drivers/hardware_api.h new file mode 100644 index 00000000..01c03867 --- /dev/null +++ b/minimal_project_examples/atmega328_driver_standalone/src/drivers/hardware_api.h @@ -0,0 +1,123 @@ +#ifndef HARDWARE_UTILS_H +#define HARDWARE_UTILS_H + +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA bldc driver + * @param pinB pinB bldc driver + */ +void _configure2PWM(long pwm_frequency, const int pinA, const int pinB); + +/** + * Configuring PWM frequency, resolution and alignment + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA bldc driver + * @param pinB pinB bldc driver + * @param pinC pinC bldc driver + */ +void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC); + +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 4PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pin1A pin1A stepper driver + * @param pin1B pin1B stepper driver + * @param pin2A pin2A stepper driver + * @param pin2B pin2B stepper driver + */ +void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B); + +/** + * Configuring PWM frequency, resolution and alignment + * - BLDC driver - 6PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low - if applicable + * @param pinA_h pinA high-side bldc driver + * @param pinA_l pinA low-side bldc driver + * @param pinB_h pinA high-side bldc driver + * @param pinB_l pinA low-side bldc driver + * @param pinC_h pinA high-side bldc driver + * @param pinC_l pinA low-side bldc driver + * + * @return 0 if config good, -1 if failed + */ +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param pinA phase A hardware pin number + * @param pinB phase B hardware pin number + */ +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param pinA phase A hardware pin number + * @param pinB phase B hardware pin number + * @param pinC phase C hardware pin number + */ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 4PWM setting + * - hardware specific + * + * @param dc_1a duty cycle phase 1A [0, 1] + * @param dc_1b duty cycle phase 1B [0, 1] + * @param dc_2a duty cycle phase 2A [0, 1] + * @param dc_2b duty cycle phase 2B [0, 1] + * @param pin1A phase 1A hardware pin number + * @param pin1B phase 1B hardware pin number + * @param pin2A phase 2A hardware pin number + * @param pin2B phase 2B hardware pin number + */ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B); + + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 6PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low + * @param pinA_h phase A high-side hardware pin number + * @param pinA_l phase A low-side hardware pin number + * @param pinB_h phase B high-side hardware pin number + * @param pinB_l phase B low-side hardware pin number + * @param pinC_h phase C high-side hardware pin number + * @param pinC_l phase C low-side hardware pin number + * + */ +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l); + +#endif \ No newline at end of file diff --git a/minimal_project_examples/atmega328_driver_standalone/src/drivers/hardware_specific/atmega328_mcu.cpp b/minimal_project_examples/atmega328_driver_standalone/src/drivers/hardware_specific/atmega328_mcu.cpp new file mode 100644 index 00000000..24fe563e --- /dev/null +++ b/minimal_project_examples/atmega328_driver_standalone/src/drivers/hardware_specific/atmega328_mcu.cpp @@ -0,0 +1,153 @@ +#include "../hardware_api.h" + +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) + +// set pwm frequency to 32KHz +void _pinHighFrequency(const int pin){ + // High PWM frequency + // https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328 + if (pin == 5 || pin == 6 ) { + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 + } + if (pin == 9 || pin == 10 ) + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 + if (pin == 3 || pin == 11) + TCCR2B = ((TCCR2B & 0b11111000) | 0x01);// set prescaler to 1 + +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); +} + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + _pinHighFrequency(pinB); + _pinHighFrequency(pinC); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pinA, 255.0*dc_a); + analogWrite(pinB, 255.0*dc_b); + analogWrite(pinC, 255.0*dc_c); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pin1A); + _pinHighFrequency(pin1B); + _pinHighFrequency(pin2A); + _pinHighFrequency(pin2B); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(pin1A, 255.0*dc_1a); + analogWrite(pin1B, 255.0*dc_1b); + analogWrite(pin2A, 255.0*dc_2a); + analogWrite(pin2B, 255.0*dc_2b); +} + + +// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm +int _configureComplementaryPair(int pinH, int pinL) { + if( (pinH == 5 && pinL == 6 ) || (pinH == 6 && pinL == 5 ) ){ + // configure the pwm phase-corrected mode + TCCR0A = ((TCCR0A & 0b11111100) | 0x01); + // configure complementary pwm on low side + if(pinH == 6 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ; + else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ; + // set prescaler to 1 - 32kHz freq + TCCR0B = ((TCCR0B & 0b11110000) | 0x01); + }else if( (pinH == 9 && pinL == 10 ) || (pinH == 10 && pinL == 9 ) ){ + // set prescaler to 1 - 32kHz freq + TCCR1B = ((TCCR1B & 0b11111000) | 0x01); + // configure complementary pwm on low side + if(pinH == 9 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ; + else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ; + }else if((pinH == 3 && pinL == 11 ) || (pinH == 11 && pinL == 3 ) ){ + // set prescaler to 1 - 32kHz freq + TCCR2B = ((TCCR2B & 0b11111000) | 0x01); + // configure complementary pwm on low side + if(pinH == 11 ) TCCR2A = 0b10110000 | (TCCR2A & 0b00001111) ; + else TCCR2A = 0b11100000 | (TCCR2A & 0b00001111) ; + }else{ + return -1; + } + return 0; +} + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arudino/ATmega328 +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + // High PWM frequency + // - always max 32kHz + int ret_flag = 0; + ret_flag += _configureComplementaryPair(pinA_h, pinA_l); + ret_flag += _configureComplementaryPair(pinB_h, pinB_l); + ret_flag += _configureComplementaryPair(pinC_h, pinC_l); + return ret_flag; // returns -1 if not well configured +} + +// function setting the +void _setPwmPair(int pinH, int pinL, float val, int dead_time) +{ + int pwm_h = _constrain(val-dead_time/2,0,255); + int pwm_l = _constrain(val+dead_time/2,0,255); + + analogWrite(pinH, pwm_h); + if(pwm_l == 255 || pwm_l == 0) + digitalWrite(pinL, pwm_l ? LOW : HIGH); + else + analogWrite(pinL, pwm_l); +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +// supports Arudino/ATmega328 +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); + _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); + _setPwmPair(pinC_h, pinC_l, dc_c*255.0, dead_zone*255.0); +} + +#endif \ No newline at end of file diff --git a/minimal_project_examples/esp32_bldc_magnetic_spi/esp32_bldc_magnetic_spi.ino b/minimal_project_examples/esp32_bldc_magnetic_spi/esp32_bldc_magnetic_spi.ino new file mode 100644 index 00000000..5e7d562c --- /dev/null +++ b/minimal_project_examples/esp32_bldc_magnetic_spi/esp32_bldc_magnetic_spi.ino @@ -0,0 +1,145 @@ +/** + * Comprehensive BLDC motor control example using magnetic sensor + * + * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: + * - configure PID controller constants + * - change motion control loops + * - monitor motor variabels + * - set target values + * - check all the configuration values + * + * To check the config value just enter the command letter. + * For example: - to read velocity PI controller P gain run: P + * - to set velocity PI controller P gain to 1.2 run: P1.2 + * + * To change the target value just enter a number in the terminal: + * For example: - to change the target value to -0.1453 enter: -0.1453 + * - to get the current target value enter: V3 + * + * List of commands: + * - P: velocity PID controller P gain + * - I: velocity PID controller I gain + * - D: velocity PID controller D gain + * - R: velocity PID controller voltage ramp + * - F: velocity Low pass filter time constant + * - K: angle P controller P gain + * - N: angle P controller velocity limit + * - L: system voltage limit + * - C: control loop + * - 0: voltage + * - 1: velocity + * - 2: angle + * - V: get motor variables + * - 0: currently set voltage + * - 1: current velocity + * - 2: current angle + * - 3: current target value + * + */ +#include "src/BLDCMotor.h" +#include "src/drivers/BLDCDriver3PWM.h" +#include "src/sensors/MagneticSensorSPI.h" + +// SPI magnetic sensor instance +MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF); + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(12, 13, 14, 27); + +void setup() { + + // initialise magnetic sensor hardware + sensor.init(); + // link the motor to the sensor + motor.linkSensor(&sensor); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link driver + motor.linkDriver(&driver); + + // choose FOC modulation + motor.foc_modulation = FOCModulationType::SpaceVectorPWM; + + // set control loop type to be used + motor.controller = ControlType::voltage; + + // contoller configuration based on the control type + motor.PID_velocity.P = 0.2; + motor.PID_velocity.I = 20; + motor.PID_velocity.D = 0; + // default voltage_power_supply + motor.voltage_limit = 12; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01; + + // angle loop controller + motor.P_angle.P = 20; + // angle loop velocity limit + motor.velocity_limit = 50; + + // use monitoring with serial for motor init + // monitoring port + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialise motor + motor.init(); + // align encoder and start FOC + motor.initFOC(); + + // set the inital target value + motor.target = 2; + + + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + Serial.println("Motor commands sketch | Initial motion control > torque/voltage : target 2V."); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor.loopFOC(); + + // iterative function setting the outter loop target + // velocity, position or voltage + // if tatget not set in parameter uses motor.target variable + motor.move(); + + // user communication + motor.command(serialReceiveUserCommand()); +} + +// utility function enabling serial communication the user +String serialReceiveUserCommand() { + + // a string to hold incoming data + static String received_chars; + + String command = ""; + + while (Serial.available()) { + // get the new byte: + char inChar = (char)Serial.read(); + // add it to the string buffer: + received_chars += inChar; + + // end of user input + if (inChar == '\n') { + + // execute the user command + command = received_chars; + + // reset the command buffer + received_chars = ""; + } + } + return command; +} \ No newline at end of file diff --git a/minimal_project_examples/esp32_bldc_magnetic_spi/src/BLDCMotor.cpp b/minimal_project_examples/esp32_bldc_magnetic_spi/src/BLDCMotor.cpp new file mode 100644 index 00000000..5d36ce35 --- /dev/null +++ b/minimal_project_examples/esp32_bldc_magnetic_spi/src/BLDCMotor.cpp @@ -0,0 +1,421 @@ +#include "BLDCMotor.h" + +// // BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en) +// // - phA, phB, phC - motor A,B,C phase pwm pins +// // - pp - pole pair number +// // - cpr - counts per rotation number (cpm=ppm*4) +// // - enable pin - (optional input) +// BLDCMotor::BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en){ +// noop; +// } + +// BLDCMotor( int pp) +// - phA, phB, phC - motor A,B,C phase pwm pins +// - pp - pole pair number +// - cpr - counts per rotation number (cpm=ppm*4) +// - enable pin - (optional input) +BLDCMotor::BLDCMotor(int pp) +: FOCMotor() +{ + // save pole pairs number + pole_pairs = pp; +} + + +/** + Link the driver which controls the motor +*/ +void BLDCMotor::linkDriver(BLDCDriver* _driver) { + driver = _driver; +} + +// init hardware pins +void BLDCMotor::init() { + if(monitor_port) monitor_port->println("MOT: Initialise variables."); + // sanity check for the voltage limit configuration + if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; + // constrain voltage for sensor alignment + if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; + // update the controller limits + PID_velocity.limit = voltage_limit; + P_angle.limit = velocity_limit; + + _delay(500); + // enable motor + if(monitor_port) monitor_port->println("MOT: Enable driver."); + enable(); + _delay(500); +} + + +// disable motor driver +void BLDCMotor::disable() +{ + // set zero to PWM + driver->setPwm(0, 0, 0); + // disable the driver + driver->disable(); +} +// enable motor driver +void BLDCMotor::enable() +{ + // enable the driver + driver->enable(); + // set zero to PWM + driver->setPwm(0, 0, 0); + +} + +/** + FOC functions +*/ +// FOC initialization function +int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction ) { + int exit_flag = 1; + // align motor if necessary + // alignment necessary for encoders! + if(zero_electric_offset != NOT_SET){ + // abosolute zero offset provided - no need to align + zero_electric_angle = zero_electric_offset; + // set the sensor direction - default CW + sensor->natural_direction = sensor_direction; + }else{ + // sensor and motor alignment + _delay(500); + exit_flag = alignSensor(); + _delay(500); + } + if(monitor_port) monitor_port->println("MOT: Motor ready."); + + return exit_flag; +} +// Encoder alignment to electrical 0 angle +int BLDCMotor::alignSensor() { + if(monitor_port) monitor_port->println("MOT: Align sensor."); + // align the electrical phases of the motor and sensor + // set angle -90 degrees + + float start_angle = shaftAngle(); + for (int i = 0; i <=5; i++ ) { + float angle = _3PI_2 + _2PI * i / 6.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(200); + } + float mid_angle = shaftAngle(); + for (int i = 5; i >=0; i-- ) { + float angle = _3PI_2 + _2PI * i / 6.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(200); + } + if (mid_angle < start_angle) { + if(monitor_port) monitor_port->println("MOT: natural_direction==CCW"); + sensor->natural_direction = Direction::CCW; + } else if (mid_angle == start_angle) { + if(monitor_port) monitor_port->println("MOT: Sensor failed to notice movement"); + } else{ + if(monitor_port) monitor_port->println("MOT: natural_direction==CW"); + } + + // let the motor stabilize for 2 sec + _delay(2000); + // set sensor to zero + sensor->initRelativeZero(); + _delay(500); + setPhaseVoltage(0, 0, 0); + _delay(200); + + // find the index if available + int exit_flag = absoluteZeroAlign(); + _delay(500); + if(monitor_port){ + if(exit_flag< 0 ) monitor_port->println("MOT: Error: Not found!"); + if(exit_flag> 0 ) monitor_port->println("MOT: Success!"); + else monitor_port->println("MOT: Not available!"); + } + return exit_flag; +} + + +// Encoder alignment the absolute zero angle +// - to the index +int BLDCMotor::absoluteZeroAlign() { + + if(monitor_port) monitor_port->println("MOT: Absolute zero align."); + // if no absolute zero return + if(!sensor->hasAbsoluteZero()) return 0; + + + if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println("MOT: Searching..."); + // search the absolute zero with small velocity + while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){ + loopFOC(); + voltage_q = PID_velocity(velocity_index_search - shaftVelocity()); + } + voltage_q = 0; + // disable motor + setPhaseVoltage(0, 0, 0); + + // align absolute zero if it has been found + if(!sensor->needsAbsoluteZeroSearch()){ + // align the sensor with the absolute zero + float zero_offset = sensor->initAbsoluteZero(); + // remember zero electric angle + zero_electric_angle = _normalizeAngle(_electricalAngle(zero_offset, pole_pairs)); + } + // return bool if zero found + return !sensor->needsAbsoluteZeroSearch() ? 1 : -1; +} + +// Iterative function looping FOC algorithm, setting Uq on the Motor +// The faster it can be run the better +void BLDCMotor::loopFOC() { + // shaft angle + shaft_angle = shaftAngle(); + // set the phase voltage - FOC heart function :) + setPhaseVoltage(voltage_q, voltage_d, _electricalAngle(shaft_angle,pole_pairs)); +} + +// Iterative function running outer loop of the FOC algorithm +// Behavior of this function is determined by the motor.controller variable +// It runs either angle, velocity or voltage loop +// - needs to be called iteratively it is asynchronous function +// - if target is not set it uses motor.target value +void BLDCMotor::move(float new_target) { + // set internal target variable + if( new_target != NOT_SET ) target = new_target; + // get angular velocity + shaft_velocity = shaftVelocity(); + // choose control loop + switch (controller) { + case ControlType::voltage: + voltage_q = target; + break; + case ControlType::angle: + // angle set point + // include angle loop + shaft_angle_sp = target; + shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); + voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + break; + case ControlType::velocity: + // velocity set point + // include velocity loop + shaft_velocity_sp = target; + voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + break; + case ControlType::velocity_openloop: + // velocity control in open loop + // loopFOC should not be called + shaft_velocity_sp = target; + velocityOpenloop(shaft_velocity_sp); + break; + case ControlType::angle_openloop: + // angle control in open loop + // loopFOC should not be called + shaft_angle_sp = target; + angleOpenloop(shaft_angle_sp); + break; + } +} + + +// Method using FOC to set Uq and Ud to the motor at the optimal angle +// Function implementing Space Vector PWM and Sine PWM algorithms +// +// Function using sine approximation +// regular sin + cos ~300us (no memory usaage) +// approx _sin + _cos ~110us (400Byte ~ 20% of memory) +void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { + + const bool centered = true; + int sector; + float _ca,_sa; + + switch (foc_modulation) + { + case FOCModulationType::Trapezoid_120 : + // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5 + static int trap_120_map[6][3] = { + {0,1,-1},{-1,1,0},{-1,0,1},{0,-1,1},{1,-1,0},{1,0,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + }; + // static int trap_120_state = 0; + sector = 6 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + + Ua = Uq + trap_120_map[sector][0] * Uq; + Ub = Uq + trap_120_map[sector][1] * Uq; + Uc = Uq + trap_120_map[sector][2] * Uq; + + if (centered) { + Ua += (driver->voltage_limit)/2 -Uq; + Ub += (driver->voltage_limit)/2 -Uq; + Uc += (driver->voltage_limit)/2 -Uq; + } + break; + + case FOCModulationType::Trapezoid_150 : + // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8 + static int trap_150_map[12][3] = { + {0,1,-1},{-1,1,-1},{-1,1,0},{-1,1,1},{-1,0,1},{-1,-1,1},{0,-1,1},{1,-1,1},{1,-1,0},{1,-1,-1},{1,0,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + }; + // static int trap_150_state = 0; + sector = 12 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + + Ua = Uq + trap_150_map[sector][0] * Uq; + Ub = Uq + trap_150_map[sector][1] * Uq; + Uc = Uq + trap_150_map[sector][2] * Uq; + + //center + if (centered) { + Ua += (driver->voltage_limit)/2 -Uq; + Ub += (driver->voltage_limit)/2 -Uq; + Uc += (driver->voltage_limit)/2 -Uq; + } + + break; + + case FOCModulationType::SinePWM : + // Sinusoidal PWM modulation + // Inverse Park + Clarke transformation + + // angle normalization in between 0 and 2pi + // only necessary if using _sin and _cos - approximation functions + angle_el = _normalizeAngle(angle_el + zero_electric_angle); + _ca = _cos(angle_el); + _sa = _sin(angle_el); + // Inverse park transform + Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; + Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; + + // Clarke transform + Ua = Ualpha + driver->voltage_limit/2; + Ub = -0.5 * Ualpha + _SQRT3_2 * Ubeta + driver->voltage_limit/2; + Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + driver->voltage_limit/2; + + if (!centered) { + float Umin = min(Ua, min(Ub, Uc)); + Ua -= Umin; + Ub -= Umin; + Uc -= Umin; + } + + break; + + case FOCModulationType::SpaceVectorPWM : + // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm + // https://www.youtube.com/watch?v=QMSWUMEAejg + + // if negative voltages change inverse the phase + // angle + 180degrees + if(Uq < 0) angle_el += _PI; + Uq = abs(Uq); + + // angle normalisation in between 0 and 2pi + // only necessary if using _sin and _cos - approximation functions + angle_el = _normalizeAngle(angle_el + zero_electric_angle + _PI_2); + + // find the sector we are in currently + sector = floor(angle_el / _PI_3) + 1; + // calculate the duty cycles + float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_limit; + float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_limit; + // two versions possible + float T0 = 0; // pulled to 0 - better for low power supply voltage + if (centered) { + T0 = 1 - T1 - T2; //centered around driver->voltage_limit/2 + } + + // calculate the duty cycles(times) + float Ta,Tb,Tc; + switch(sector){ + case 1: + Ta = T1 + T2 + T0/2; + Tb = T2 + T0/2; + Tc = T0/2; + break; + case 2: + Ta = T1 + T0/2; + Tb = T1 + T2 + T0/2; + Tc = T0/2; + break; + case 3: + Ta = T0/2; + Tb = T1 + T2 + T0/2; + Tc = T2 + T0/2; + break; + case 4: + Ta = T0/2; + Tb = T1+ T0/2; + Tc = T1 + T2 + T0/2; + break; + case 5: + Ta = T2 + T0/2; + Tb = T0/2; + Tc = T1 + T2 + T0/2; + break; + case 6: + Ta = T1 + T2 + T0/2; + Tb = T0/2; + Tc = T1 + T0/2; + break; + default: + // possible error state + Ta = 0; + Tb = 0; + Tc = 0; + } + + // calculate the phase voltages and center + Ua = Ta*driver->voltage_limit; + Ub = Tb*driver->voltage_limit; + Uc = Tc*driver->voltage_limit; + break; + + } + + // set the voltages in driver + driver->setPwm(Ua, Ub, Uc); +} + + + +// Function (iterative) generating open loop movement for target velocity +// - target_velocity - rad/s +// it uses voltage_limit variable +void BLDCMotor::velocityOpenloop(float target_velocity){ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6; + + // calculate the necessary angle to achieve target velocity + shaft_angle += target_velocity*Ts; + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; +} + +// Function (iterative) generating open loop movement towards the target angle +// - target_angle - rad +// it uses voltage_limit and velocity_limit variables +void BLDCMotor::angleOpenloop(float target_angle){ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6; + + // calculate the necessary angle to move from current position towards target angle + // with maximal velocity (velocity_limit) + if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)) + shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; + else + shaft_angle = target_angle; + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; +} diff --git a/minimal_project_examples/esp32_bldc_magnetic_spi/src/BLDCMotor.h b/minimal_project_examples/esp32_bldc_magnetic_spi/src/BLDCMotor.h new file mode 100644 index 00000000..164cc108 --- /dev/null +++ b/minimal_project_examples/esp32_bldc_magnetic_spi/src/BLDCMotor.h @@ -0,0 +1,109 @@ +#ifndef BLDCMotor_h +#define BLDCMotor_h + +#include "Arduino.h" +#include "common/base_classes/FOCMotor.h" +#include "common/base_classes/Sensor.h" +#include "common/base_classes/BLDCDriver.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" +#include "common/defaults.h" + +/** + BLDC motor class +*/ +class BLDCMotor: public FOCMotor +{ + public: + /** + BLDCMotor class constructor + @param pp pole pairs number + */ + BLDCMotor(int pp); + + /** + * Function linking a motor and a foc driver + * + * @param driver BLDCDriver class implementing all the hardware specific functions necessary PWM setting + */ + void linkDriver(BLDCDriver* driver); + + /** + * BLDCDriver link: + * - 3PWM + * - 6PWM + */ + BLDCDriver* driver; + + /** Motor hardware init function */ + void init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + */ + int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override; + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + void loopFOC() override; + + /** + * Function executing the control loops set by the controller parameter of the BLDCMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + void move(float target = NOT_SET) override; + + float Ua,Ub,Uc;//!< Current phase voltages Ua,Ub and Uc set to motor + float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform + + + private: + // FOC methods + /** + * Method using FOC to set Uq to the motor at the optimal angle + * Heart of the FOC algorithm + * + * @param Uq Current voltage in q axis to set to the motor + * @param Ud Current voltage in d axis to set to the motor + * @param angle_el current electrical angle of the motor + */ + void setPhaseVoltage(float Uq, float Ud, float angle_el); + /** Sensor alignment to electrical 0 angle of the motor */ + int alignSensor(); + /** Motor and sensor alignment to the sensors absolute 0 angle */ + int absoluteZeroAlign(); + + + // Open loop motion control + /** + * Function (iterative) generating open loop movement for target velocity + * it uses voltage_limit variable + * + * @param target_velocity - rad/s + */ + void velocityOpenloop(float target_velocity); + /** + * Function (iterative) generating open loop movement towards the target angle + * it uses voltage_limit and velocity_limit variables + * + * @param target_angle - rad + */ + void angleOpenloop(float target_angle); + // open loop variables + long open_loop_timestamp; +}; + + +#endif diff --git a/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/base_classes/BLDCDriver.h b/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/base_classes/BLDCDriver.h new file mode 100644 index 00000000..708323fb --- /dev/null +++ b/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/base_classes/BLDCDriver.h @@ -0,0 +1,28 @@ +#ifndef BLDCDRIVER_H +#define BLDCDRIVER_H + +class BLDCDriver{ + public: + + /** Initialise hardware */ + virtual int init(); + /** Enable hardware */ + virtual void enable(); + /** Disable hardware */ + virtual void disable(); + + long pwm_frequency; //!< pwm frequency value in hertz + float voltage_power_supply; //!< power supply voltage + float voltage_limit; //!< limiting voltage set to the motor + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + virtual void setPwm(float Ua, float Ub, float Uc); +}; + +#endif \ No newline at end of file diff --git a/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/base_classes/FOCMotor.cpp b/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/base_classes/FOCMotor.cpp new file mode 100644 index 00000000..1d1a0f43 --- /dev/null +++ b/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/base_classes/FOCMotor.cpp @@ -0,0 +1,241 @@ +#include "FOCMotor.h" + +/** + * Default constructor - setting all variabels to default values + */ +FOCMotor::FOCMotor() +{ + // maximum angular velocity to be used for positioning + velocity_limit = DEF_VEL_LIM; + // maximum voltage to be set to the motor + voltage_limit = DEF_POWER_SUPPLY; + + // index search velocity + velocity_index_search = DEF_INDEX_SEARCH_TARGET_VELOCITY; + // sensor and motor align voltage + voltage_sensor_align = DEF_VOLTAGE_SENSOR_ALIGN; + + // electric angle of comthe zero angle + zero_electric_angle = 0; + + // default modulation is SinePWM + foc_modulation = FOCModulationType::SinePWM; + + // default target value + target = 0; + voltage_d = 0; + voltage_q = 0; + + //monitor_port + monitor_port = nullptr; + //sensor + sensor = nullptr; +} + + +/** + Sensor communication methods +*/ +void FOCMotor::linkSensor(Sensor* _sensor) { + sensor = _sensor; +} +// shaft angle calculation +float FOCMotor::shaftAngle() { + // if no sensor linked return 0 + if(!sensor) return shaft_angle; + return sensor->getAngle(); +} +// shaft velocity calculation +float FOCMotor::shaftVelocity() { + // if no sensor linked return 0 + if(!sensor) return 0; + return LPF_velocity(sensor->getVelocity()); +} + +/** + * Monitoring functions + */ +// function implementing the monitor_port setter +void FOCMotor::useMonitoring(Print &print){ + monitor_port = &print; //operate on the address of print + if(monitor_port ) monitor_port->println("MOT: Monitor enabled!"); +} +// utility function intended to be used with serial plotter to monitor motor variables +// significantly slowing the execution down!!!! +void FOCMotor::monitor() { + if(!monitor_port) return; + switch (controller) { + case ControlType::velocity_openloop: + case ControlType::velocity: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_velocity_sp); + monitor_port->print("\t"); + monitor_port->println(shaft_velocity); + break; + case ControlType::angle_openloop: + case ControlType::angle: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_angle_sp); + monitor_port->print("\t"); + monitor_port->println(shaft_angle); + break; + case ControlType::voltage: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_angle); + monitor_port->print("\t"); + monitor_port->println(shaft_velocity); + break; + } +} + +int FOCMotor::command(String user_command) { + // error flag + int errorFlag = 1; + // if empty string + if(user_command.length() < 1) return errorFlag; + + // parse command letter + char cmd = user_command.charAt(0); + // check if get command + char GET = user_command.charAt(1) == '\n'; + // parse command values + float value = user_command.substring(1).toFloat(); + + // a bit of optimisation of variable memory for Arduino UNO (atmega328) + switch(cmd){ + case 'P': // velocity P gain change + case 'I': // velocity I gain change + case 'D': // velocity D gain change + case 'R': // velocity voltage ramp change + if(monitor_port) monitor_port->print(" PID velocity| "); + break; + case 'F': // velocity Tf low pass filter change + if(monitor_port) monitor_port->print(" LPF velocity| "); + break; + case 'K': // angle loop gain P change + if(monitor_port) monitor_port->print(" P angle| "); + break; + case 'L': // velocity voltage limit change + case 'N': // angle loop gain velocity_limit change + if(monitor_port) monitor_port->print(" Limits| "); + break; + } + + // apply the the command + switch(cmd){ + case 'P': // velocity P gain change + if(monitor_port) monitor_port->print("P: "); + if(!GET) PID_velocity.P = value; + if(monitor_port) monitor_port->println(PID_velocity.P); + break; + case 'I': // velocity I gain change + if(monitor_port) monitor_port->print("I: "); + if(!GET) PID_velocity.I = value; + if(monitor_port) monitor_port->println(PID_velocity.I); + break; + case 'D': // velocity D gain change + if(monitor_port) monitor_port->print("D: "); + if(!GET) PID_velocity.D = value; + if(monitor_port) monitor_port->println(PID_velocity.D); + break; + case 'R': // velocity voltage ramp change + if(monitor_port) monitor_port->print("volt_ramp: "); + if(!GET) PID_velocity.output_ramp = value; + if(monitor_port) monitor_port->println(PID_velocity.output_ramp); + break; + case 'L': // velocity voltage limit change + if(monitor_port) monitor_port->print("volt_limit: "); + if(!GET) { + voltage_limit = value; + PID_velocity.limit = value; + } + if(monitor_port) monitor_port->println(voltage_limit); + break; + case 'F': // velocity Tf low pass filter change + if(monitor_port) monitor_port->print("Tf: "); + if(!GET) LPF_velocity.Tf = value; + if(monitor_port) monitor_port->println(LPF_velocity.Tf); + break; + case 'K': // angle loop gain P change + if(monitor_port) monitor_port->print(" P: "); + if(!GET) P_angle.P = value; + if(monitor_port) monitor_port->println(P_angle.P); + break; + case 'N': // angle loop gain velocity_limit change + if(monitor_port) monitor_port->print("vel_limit: "); + if(!GET){ + velocity_limit = value; + P_angle.limit = value; + } + if(monitor_port) monitor_port->println(velocity_limit); + break; + case 'C': + // change control type + if(monitor_port) monitor_port->print("Control: "); + + if(GET){ // if get command + switch(controller){ + case ControlType::voltage: + if(monitor_port) monitor_port->println("voltage"); + break; + case ControlType::velocity: + if(monitor_port) monitor_port->println("velocity"); + break; + case ControlType::angle: + if(monitor_port) monitor_port->println("angle"); + break; + default: + if(monitor_port) monitor_port->println("open loop"); + } + }else{ // if set command + switch((int)value){ + case 0: + if(monitor_port) monitor_port->println("voltage"); + controller = ControlType::voltage; + break; + case 1: + if(monitor_port) monitor_port->println("velocity"); + controller = ControlType::velocity; + break; + case 2: + if(monitor_port) monitor_port->println("angle"); + controller = ControlType::angle; + break; + default: // not valid command + errorFlag = 0; + } + } + break; + case 'V': // get current values of the state variables + switch((int)value){ + case 0: // get voltage + if(monitor_port) monitor_port->print("Uq: "); + if(monitor_port) monitor_port->println(voltage_q); + break; + case 1: // get velocity + if(monitor_port) monitor_port->print("Velocity: "); + if(monitor_port) monitor_port->println(shaft_velocity); + break; + case 2: // get angle + if(monitor_port) monitor_port->print("Angle: "); + if(monitor_port) monitor_port->println(shaft_angle); + break; + case 3: // get angle + if(monitor_port) monitor_port->print("Target: "); + if(monitor_port) monitor_port->println(target); + break; + default: // not valid command + errorFlag = 0; + } + break; + default: // target change + if(monitor_port) monitor_port->print("Target : "); + target = user_command.toFloat(); + if(monitor_port) monitor_port->println(target); + } + // return 0 if error and 1 if ok + return errorFlag; +} \ No newline at end of file diff --git a/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/base_classes/FOCMotor.h b/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/base_classes/FOCMotor.h new file mode 100644 index 00000000..986e1ab2 --- /dev/null +++ b/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/base_classes/FOCMotor.h @@ -0,0 +1,197 @@ +#ifndef FOCMOTOR_H +#define FOCMOTOR_H + +#include "Arduino.h" +#include "Sensor.h" +#include "BLDCDriver.h" + +#include "../time_utils.h" +#include "../foc_utils.h" +#include "../defaults.h" +#include "../pid.h" +#include "../lowpass_filter.h" + + +/** + * Motiron control type + */ +enum ControlType{ + voltage,//!< Torque control using voltage + velocity,//!< Velocity motion control + angle,//!< Position/angle motion control + velocity_openloop, + angle_openloop +}; + +/** + * FOC modulation type + */ +enum FOCModulationType{ + SinePWM, //!< Sinusoidal PWM modulation + SpaceVectorPWM, //!< Space vector modulation method + Trapezoid_120, + Trapezoid_150 +}; + +/** + Generic motor class +*/ +class FOCMotor +{ + public: + /** + * Default constructor - setting all variabels to default values + */ + FOCMotor(); + + /** Motor hardware init function */ + virtual void init()=0; + /** Motor disable function */ + virtual void disable()=0; + /** Motor enable function */ + virtual void enable()=0; + + /** + * Function linking a motor and a sensor + * + * @param sensor Sensor class wrapper for the FOC algorihtm to read the motor angle and velocity + */ + void linkSensor(Sensor* sensor); + + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + * + * - If zero_electric_offset parameter is set the alignment procedure is skipped + * + * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. + * @param sensor_direction sensor natural direction - default is CW + * + */ + virtual int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW)=0; + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + virtual void loopFOC()=0; + /** + * Function executing the control loops set by the controller parameter of the BLDCMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + virtual void move(float target = NOT_SET)=0; + + // State calculation methods + /** Shaft angle calculation in radians [rad] */ + float shaftAngle(); + /** + * Shaft angle calculation function in radian per second [rad/s] + * It implements low pass filtering + */ + float shaftVelocity(); + + // state variables + float target; //!< current target value - depends of the controller + float shaft_angle;//!< current motor angle + float shaft_velocity;//!< current motor velocity + float shaft_velocity_sp;//!< current target velocity + float shaft_angle_sp;//!< current target angle + float voltage_q;//!< current voltage u_q set + float voltage_d;//!< current voltage u_d set + + // motor configuration parameters + float voltage_sensor_align;//!< sensor and motor align voltage parameter + float velocity_index_search;//!< target velocity for index search + int pole_pairs;//!< Motor pole pairs number + + // limiting variables + float voltage_limit; //!< Voltage limitting variable - global limit + float velocity_limit; //!< Velocity limitting variable - global limit + + float zero_electric_angle;//! _2PI ? a_sin - _2PI : a_sin; + return _sin(a_sin); +} + + +// normalizing radian angle to [0,2PI] +float _normalizeAngle(float angle){ + float a = fmod(angle, _2PI); + return a >= 0 ? a : (a + _2PI); +} + +// Electrical angle calculation +float _electricalAngle(float shaft_angle, int pole_pairs) { + return (shaft_angle * pole_pairs); +} \ No newline at end of file diff --git a/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/foc_utils.h b/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/foc_utils.h new file mode 100644 index 00000000..7da3f7bc --- /dev/null +++ b/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/foc_utils.h @@ -0,0 +1,55 @@ +#ifndef FOCUTILS_LIB_H +#define FOCUTILS_LIB_H + +#include "Arduino.h" + +// sign function +#define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) ) +#define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) +#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) + +// utility defines +#define _2_SQRT3 1.15470053838 +#define _SQRT3 1.73205080757 +#define _1_SQRT3 0.57735026919 +#define _SQRT3_2 0.86602540378 +#define _SQRT2 1.41421356237 +#define _120_D2R 2.09439510239 +#define _PI 3.14159265359 +#define _PI_2 1.57079632679 +#define _PI_3 1.0471975512 +#define _2PI 6.28318530718 +#define _3PI_2 4.71238898038 + +#define NOT_SET -12345.0 + +/** + * Function approximating the sine calculation by using fixed size array + * - execution time ~40us (Arduino UNO) + * + * @param a angle in between 0 and 2PI + */ +float _sin(float a); +/** + * Function approximating cosine calculation by using fixed size array + * - execution time ~50us (Arduino UNO) + * + * @param a angle in between 0 and 2PI + */ +float _cos(float a); + +/** + * normalizing radian angle to [0,2PI] + * @param angle - angle to be normalized + */ +float _normalizeAngle(float angle); + + +/** + * Electrical angle calculation + * + * @param shaft_angle - shaft angle of the motor + * @param pole_pairs - number of pole pairs + */ +float _electricalAngle(float shaft_angle, int pole_pairs); +#endif \ No newline at end of file diff --git a/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/lowpass_filter.cpp b/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/lowpass_filter.cpp new file mode 100644 index 00000000..9bf4ab7a --- /dev/null +++ b/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/lowpass_filter.cpp @@ -0,0 +1,25 @@ +#include "lowpass_filter.h" + +LowPassFilter::LowPassFilter(float time_constant) + : Tf(time_constant) + , y_prev(0.0f) +{ + timestamp_prev = _micros(); +} + + +float LowPassFilter::operator() (float x) +{ + unsigned long timestamp = _micros(); + float dt = (timestamp - timestamp_prev)*1e-6f; + + if (dt < 0.0f || dt > 0.5f) + dt = 1e-3f; + + float alpha = Tf/(Tf + dt); + float y = alpha*y_prev + (1.0f - alpha)*x; + + y_prev = y; + timestamp_prev = timestamp; + return y; +} \ No newline at end of file diff --git a/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/lowpass_filter.h b/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/lowpass_filter.h new file mode 100644 index 00000000..7c51be54 --- /dev/null +++ b/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/lowpass_filter.h @@ -0,0 +1,29 @@ +#ifndef LOWPASS_FILTER_H +#define LOWPASS_FILTER_H + + +#include "time_utils.h" +#include "foc_utils.h" + + +/** + * Low pass filter class + */ +class LowPassFilter +{ +public: + /** + * @param Tf - Low pass filter time constant + */ + LowPassFilter(float Tf); + ~LowPassFilter() = default; + + float operator() (float x); + float Tf; //!< Low pass filter time constant + +protected: + unsigned long timestamp_prev; //!< Last execution timestamp + float y_prev; //!< filtered value in previous execution step +}; + +#endif // LOWPASS_FILTER_H \ No newline at end of file diff --git a/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/pid.cpp b/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/pid.cpp new file mode 100644 index 00000000..1a0a9828 --- /dev/null +++ b/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/pid.cpp @@ -0,0 +1,56 @@ +#include "pid.h" + +PIDController::PIDController(float P, float I, float D, float ramp, float limit) + : P(P) + , I(I) + , D(D) + , output_ramp(ramp) // output derivative limit [volts/second] + , limit(limit) // output supply limit [volts] + , integral_prev(0.0) + , error_prev(0.0) + , output_prev(0.0) +{ + timestamp_prev = _micros()*1e-6; +} + +// PID controller function +float PIDController::operator() (float error){ + // calculate the time from the last call + unsigned long timestamp_now = _micros(); + float Ts = (timestamp_now - timestamp_prev) * 1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + + // u(s) = (P + I/s + Ds)e(s) + // Discrete implementations + // proportional part + // u_p = P *e(k) + float proportional = P * error; + // Tustin transform of the integral part + // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) + float integral = integral_prev + I*Ts*0.5*(error + error_prev); + // antiwindup - limit the output voltage_q + integral = _constrain(integral, -limit, limit); + // Discrete derivation + // u_dk = D(ek - ek_1)/Ts + float derivative = D*(error - error_prev)/Ts; + + // sum all the components + float output = proportional + integral + derivative; + // antiwindup - limit the output variable + output = _constrain(output, -limit, limit); + + // limit the acceleration by ramping the output + float output_rate = (output - output_prev)/Ts; + if (output_rate > output_ramp) + output = output_prev + output_ramp*Ts; + else if (output_rate < -output_ramp) + output = output_prev - output_ramp*Ts; + + // saving for the next pass + integral_prev = integral; + output_prev = output; + error_prev = error; + timestamp_prev = timestamp_now; + return output; +} \ No newline at end of file diff --git a/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/pid.h b/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/pid.h new file mode 100644 index 00000000..0e9ddf54 --- /dev/null +++ b/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/pid.h @@ -0,0 +1,40 @@ +#ifndef PID_H +#define PID_H + + +#include "time_utils.h" +#include "foc_utils.h" + +/** + * PID controller class + */ +class PIDController +{ +public: + /** + * + * @param P - Proportional gain + * @param I - Integral gain + * @param D - Derivative gain + * @param ramp - Maximum speed of change of the output value + * @param limit - Maximum output value + */ + PIDController(float P, float I, float D, float ramp, float limit); + ~PIDController() = default; + + float operator() (float error); + + float P; //!< Proportional gain + float I; //!< Integral gain + float D; //!< Derivative gain + float output_ramp; //!< Maximum speed of change of the output value + float limit; //!< Maximum output value + +protected: + float integral_prev; //!< last integral component value + float error_prev; //!< last tracking error value + float timestamp_prev; //!< Last execution timestamp + float output_prev; //!< last pid output value +}; + +#endif // PID_H \ No newline at end of file diff --git a/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/time_utils.cpp b/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/time_utils.cpp new file mode 100644 index 00000000..b00a8c78 --- /dev/null +++ b/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/time_utils.cpp @@ -0,0 +1,31 @@ +#include "time_utils.h" + +// function buffering delay() +// arduino uno function doesn't work well with interrupts +void _delay(unsigned long ms){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega2560__) + // if arduino uno and other atmega328p chips + // use while instad of delay, + // due to wrong measurement based on changed timer0 + unsigned long t = _micros() + ms*1000; + while( _micros() < t ){}; +#else + // regular micros + delay(ms); +#endif +} + + +// function buffering _micros() +// arduino function doesn't work well with interrupts +unsigned long _micros(){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega2560__) +// if arduino uno and other atmega328p chips + //return the value based on the prescaler + if((TCCR0B & 0b00000111) == 0x01) return (micros()/32); + else return (micros()); +#else + // regular micros + return micros(); +#endif +} diff --git a/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/time_utils.h b/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/time_utils.h new file mode 100644 index 00000000..143d4853 --- /dev/null +++ b/minimal_project_examples/esp32_bldc_magnetic_spi/src/common/time_utils.h @@ -0,0 +1,22 @@ +#ifndef TIME_UTILS_H +#define TIME_UTILS_H + +#include "foc_utils.h" + +/** + * Function implementing delay() function in milliseconds + * - blocking function + * - hardware specific + + * @param ms number of milliseconds to wait + */ +void _delay(unsigned long ms); + +/** + * Function implementing timestamp getting function in microseconds + * hardware specific + */ +unsigned long _micros(); + + +#endif \ No newline at end of file diff --git a/minimal_project_examples/esp32_bldc_magnetic_spi/src/drivers/BLDCDriver3PWM.cpp b/minimal_project_examples/esp32_bldc_magnetic_spi/src/drivers/BLDCDriver3PWM.cpp new file mode 100644 index 00000000..a3fc052d --- /dev/null +++ b/minimal_project_examples/esp32_bldc_magnetic_spi/src/drivers/BLDCDriver3PWM.cpp @@ -0,0 +1,72 @@ +#include "BLDCDriver3PWM.h" + +BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en){ + // Pin initialization + pwmA = phA; + pwmB = phB; + pwmC = phC; + + // enable_pin pin + enable_pin = en; + + // default power-supply value + voltage_power_supply = DEF_POWER_SUPPLY; + voltage_limit = NOT_SET; + +} + +// enable motor driver +void BLDCDriver3PWM::enable(){ + // enable_pin the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, HIGH); + // set zero to PWM + setPwm(0,0,0); +} + +// disable motor driver +void BLDCDriver3PWM::disable() +{ + // set zero to PWM + setPwm(0, 0, 0); + // disable the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, LOW); + +} + +// init hardware pins +int BLDCDriver3PWM::init() { + // a bit of separation + _delay(1000); + + // PWM pins + pinMode(pwmA, OUTPUT); + pinMode(pwmB, OUTPUT); + pinMode(pwmC, OUTPUT); + if(enable_pin != NOT_SET) pinMode(enable_pin, OUTPUT); + + + // sanity check for the voltage limit configuration + if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + + // Set the pwm frequency to the pins + // hardware specific function - depending on driver and mcu + _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC); + return 0; +} + + +// Set voltage to the pwm pin +void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { + // limit the voltage in driver + Ua = _constrain(Ua, 0.0, voltage_limit); + Ub = _constrain(Ub, 0.0, voltage_limit); + Uc = _constrain(Uc, 0.0, voltage_limit); + // calculate duty cycle + // limited in [0,1] + float dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 ); + float dc_b = _constrain(Ub / voltage_power_supply, 0.0 , 1.0 ); + float dc_c = _constrain(Uc / voltage_power_supply, 0.0 , 1.0 ); + // hardware specific writing + // hardware specific function - depending on driver and mcu + _writeDutyCycle3PWM(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC); +} \ No newline at end of file diff --git a/minimal_project_examples/esp32_bldc_magnetic_spi/src/drivers/BLDCDriver3PWM.h b/minimal_project_examples/esp32_bldc_magnetic_spi/src/drivers/BLDCDriver3PWM.h new file mode 100644 index 00000000..b6f7d7f8 --- /dev/null +++ b/minimal_project_examples/esp32_bldc_magnetic_spi/src/drivers/BLDCDriver3PWM.h @@ -0,0 +1,52 @@ +#ifndef BLDCDriver3PWM_h +#define BLDCDriver3PWM_h + +#include "../common/base_classes/BLDCDriver.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "hardware_api.h" + +/** + 3 pwm bldc driver class +*/ +class BLDCDriver3PWM: public BLDCDriver +{ + public: + /** + BLDCDriver class constructor + @param phA A phase pwm pin + @param phB B phase pwm pin + @param phC C phase pwm pin + @param en enable pin (optional input) + */ + BLDCDriver3PWM(int phA,int phB,int phC, int en = NOT_SET); + + /** Motor hardware init function */ + int init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + // hardware variables + int pwmA; //!< phase A pwm pin number + int pwmB; //!< phase B pwm pin number + int pwmC; //!< phase C pwm pin number + int enable_pin; //!< enable pin number + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + void setPwm(float Ua, float Ub, float Uc) override; + + private: + +}; + + +#endif diff --git a/minimal_project_examples/esp32_bldc_magnetic_spi/src/drivers/hardware_api.h b/minimal_project_examples/esp32_bldc_magnetic_spi/src/drivers/hardware_api.h new file mode 100644 index 00000000..01c03867 --- /dev/null +++ b/minimal_project_examples/esp32_bldc_magnetic_spi/src/drivers/hardware_api.h @@ -0,0 +1,123 @@ +#ifndef HARDWARE_UTILS_H +#define HARDWARE_UTILS_H + +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA bldc driver + * @param pinB pinB bldc driver + */ +void _configure2PWM(long pwm_frequency, const int pinA, const int pinB); + +/** + * Configuring PWM frequency, resolution and alignment + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA bldc driver + * @param pinB pinB bldc driver + * @param pinC pinC bldc driver + */ +void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC); + +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 4PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pin1A pin1A stepper driver + * @param pin1B pin1B stepper driver + * @param pin2A pin2A stepper driver + * @param pin2B pin2B stepper driver + */ +void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B); + +/** + * Configuring PWM frequency, resolution and alignment + * - BLDC driver - 6PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low - if applicable + * @param pinA_h pinA high-side bldc driver + * @param pinA_l pinA low-side bldc driver + * @param pinB_h pinA high-side bldc driver + * @param pinB_l pinA low-side bldc driver + * @param pinC_h pinA high-side bldc driver + * @param pinC_l pinA low-side bldc driver + * + * @return 0 if config good, -1 if failed + */ +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param pinA phase A hardware pin number + * @param pinB phase B hardware pin number + */ +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param pinA phase A hardware pin number + * @param pinB phase B hardware pin number + * @param pinC phase C hardware pin number + */ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 4PWM setting + * - hardware specific + * + * @param dc_1a duty cycle phase 1A [0, 1] + * @param dc_1b duty cycle phase 1B [0, 1] + * @param dc_2a duty cycle phase 2A [0, 1] + * @param dc_2b duty cycle phase 2B [0, 1] + * @param pin1A phase 1A hardware pin number + * @param pin1B phase 1B hardware pin number + * @param pin2A phase 2A hardware pin number + * @param pin2B phase 2B hardware pin number + */ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B); + + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 6PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low + * @param pinA_h phase A high-side hardware pin number + * @param pinA_l phase A low-side hardware pin number + * @param pinB_h phase B high-side hardware pin number + * @param pinB_l phase B low-side hardware pin number + * @param pinC_h phase C high-side hardware pin number + * @param pinC_l phase C low-side hardware pin number + * + */ +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l); + +#endif \ No newline at end of file diff --git a/minimal_project_examples/esp32_bldc_magnetic_spi/src/drivers/hardware_specific/esp32_mcu.cpp b/minimal_project_examples/esp32_bldc_magnetic_spi/src/drivers/hardware_specific/esp32_mcu.cpp new file mode 100644 index 00000000..983d4d54 --- /dev/null +++ b/minimal_project_examples/esp32_bldc_magnetic_spi/src/drivers/hardware_specific/esp32_mcu.cpp @@ -0,0 +1,431 @@ +#include "../hardware_api.h" + +#if defined(ESP_H) + +#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 160e6 + +// preferred pwm resolution default +#define _PWM_RES_DEF 2048 +// min resolution +#define _PWM_RES_MIN 1500 +// max resolution +#define _PWM_RES_MAX 3000 + +// structure containing motor slot configuration +// this library supports up to 4 motors +typedef struct { + int pinA; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator; + mcpwm_io_signals_t mcpwm_a; + mcpwm_io_signals_t mcpwm_b; + mcpwm_io_signals_t mcpwm_c; +} bldc_3pwm_motor_slots_t; +typedef struct { + int pin1A; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator1; + mcpwm_operator_t mcpwm_operator2; + mcpwm_io_signals_t mcpwm_1a; + mcpwm_io_signals_t mcpwm_1b; + mcpwm_io_signals_t mcpwm_2a; + mcpwm_io_signals_t mcpwm_2b; +} stepper_4pwm_motor_slots_t; +typedef struct { + int pin1pwm; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator; + mcpwm_io_signals_t mcpwm_a; + mcpwm_io_signals_t mcpwm_b; +} stepper_2pwm_motor_slots_t; + +typedef struct { + int pinAH; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator1; + mcpwm_operator_t mcpwm_operator2; + mcpwm_io_signals_t mcpwm_ah; + mcpwm_io_signals_t mcpwm_bh; + mcpwm_io_signals_t mcpwm_ch; + mcpwm_io_signals_t mcpwm_al; + mcpwm_io_signals_t mcpwm_bl; + mcpwm_io_signals_t mcpwm_cl; +} bldc_6pwm_motor_slots_t; + +// define bldc motor slots array +bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}, // 2nd motor will be MCPWM0 channel B + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 3rd motor will be MCPWM1 channel A + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B} // 4th motor will be MCPWM1 channel B + }; + +// define stepper motor slots array +bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 + }; + +// define 4pwm stepper motor slots array +stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[2] = { + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0 + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1 + }; + +// define 2pwm stepper motor slots array +stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = { + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 1st motor will be MCPWM0 channel A + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B}, // 2nd motor will be MCPWM0 channel B + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 3rd motor will be MCPWM1 channel A + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B} // 4th motor will be MCPWM1 channel B + }; + +// configuring high frequency pwm timer +// a lot of help from this post from Paul Gauld +// https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller +void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit, float dead_zone = NOT_SET){ + + mcpwm_config_t pwm_config; + pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave) + pwm_config.duty_mode = MCPWM_DUTY_MODE_0; // Active HIGH + mcpwm_init(mcpwm_unit, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings + mcpwm_init(mcpwm_unit, MCPWM_TIMER_1, &pwm_config); //Configure PWM0A & PWM0B with above settings + mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &pwm_config); //Configure PWM0A & PWM0B with above settings + + if (dead_zone != NOT_SET){ + // dead zone is configured + float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + } + _delay(100); + + mcpwm_stop(mcpwm_unit, MCPWM_TIMER_0); + mcpwm_stop(mcpwm_unit, MCPWM_TIMER_1); + mcpwm_stop(mcpwm_unit, MCPWM_TIMER_2); + + // manual configuration due to the lack of config flexibility in mcpwm_init() + mcpwm_num->clk_cfg.prescale = 0; + // calculate prescaler and period + // step 1: calculate the prescaler using the default pwm resolution + // prescaler = bus_freq / (pwm_freq * default_pwm_res) - 1 + int prescaler = ceil((double)_MCPWM_FREQ / (double)_PWM_RES_DEF / 2.0 / (double)pwm_frequency) - 1; + // constrain prescaler + prescaler = _constrain(prescaler, 0, 128); + // now calculate the real resolution timer period necessary (pwm resolution) + // pwm_res = bus_freq / (pwm_freq * (prescaler + 1)) + int resolution_corrected = (double)_MCPWM_FREQ / 2.0 / (double)pwm_frequency / (double)(prescaler + 1); + // if pwm resolution too low lower the prescaler + if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 ) + resolution_corrected = (double)_MCPWM_FREQ / 2.0 / (double)pwm_frequency / (double)(--prescaler + 1); + resolution_corrected = _constrain(resolution_corrected, _PWM_RES_MIN, _PWM_RES_MAX); + + // set prescaler + mcpwm_num->timer[0].period.prescale = prescaler; + mcpwm_num->timer[1].period.prescale = prescaler; + mcpwm_num->timer[2].period.prescale = prescaler; + _delay(1); + //set period + mcpwm_num->timer[0].period.period = resolution_corrected; + mcpwm_num->timer[1].period.period = resolution_corrected; + mcpwm_num->timer[2].period.period = resolution_corrected; + _delay(1); + mcpwm_num->timer[0].period.upmethod = 0; + mcpwm_num->timer[1].period.upmethod = 0; + mcpwm_num->timer[2].period.upmethod = 0; + _delay(1); + //restart the timers + mcpwm_start(mcpwm_unit, MCPWM_TIMER_0); + mcpwm_start(mcpwm_unit, MCPWM_TIMER_1); + mcpwm_start(mcpwm_unit, MCPWM_TIMER_2); + _delay(1); + + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SELECT_SYNC_INT0, 0); + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_SELECT_SYNC_INT0, 0); + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_SELECT_SYNC_INT0, 0); + _delay(1); + mcpwm_num->timer[0].sync.out_sel = 1; + _delay(1); + mcpwm_num->timer[0].sync.out_sel = 0; +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +// supports Arudino/ATmega328, STM32 and ESP32 +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency + else pwm_frequency = _constrain(pwm_frequency, 0, 40000); // constrain to 40kHz max - centered pwm has twice lower frequency + + stepper_2pwm_motor_slots_t m_slot = {}; + + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 4; slot_num++){ + if(esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = pinA; + m_slot = esp32_stepper_2pwm_motor_slots[slot_num]; + break; + } + } + + // disable all the slots with the same MCPWM + // disable 3pwm bldc motor which would go in the same slot + esp32_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT; + if( slot_num < 2 ){ + // slot 0 of the 4pwm stepper + esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + }else{ + // slot 1 of the stepper + esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + } + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + +} + + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// supports Arudino/ATmega328, STM32 and ESP32 +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency + else pwm_frequency = _constrain(pwm_frequency, 0, 40000); // constrain to 40kHz max - centered pwm has twice lower frequency + + bldc_3pwm_motor_slots_t m_slot = {}; + + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 4; slot_num++){ + if(esp32_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_bldc_3pwm_motor_slots[slot_num].pinA = pinA; + m_slot = esp32_bldc_3pwm_motor_slots[slot_num]; + break; + } + } + // disable all the slots with the same MCPWM + // disable 2pwm steppr motor which would go in the same slot + esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = _TAKEN_SLOT; + if( slot_num < 2 ){ + // slot 0 of the 4pwm stepper + esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + }else{ + // slot 1 of the stepper + esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + } + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_c, pinC); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency + else pwm_frequency = _constrain(pwm_frequency, 0, 40000); // constrain to 50kHz max - centered pwm has twice lower frequency + stepper_4pwm_motor_slots_t m_slot = {}; + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 2; slot_num++){ + if(esp32_stepper_4pwm_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_stepper_4pwm_motor_slots[slot_num].pin1A = pinA; + m_slot = esp32_stepper_4pwm_motor_slots[slot_num]; + break; + } + } + // disable all the slots with the same MCPWM + if( slot_num == 0 ){ + // slots 0 and 1 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; + // slots 0 and 1 of the 2pwm stepper taken + esp32_stepper_2pwm_motor_slots[0].pin1pwm = _TAKEN_SLOT; + esp32_stepper_2pwm_motor_slots[1].pin1pwm = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + }else{ + // slots 2 and 3 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; + // slots 2 and 3 of the 2pwm stepper taken + esp32_stepper_2pwm_motor_slots[2].pin1pwm = _TAKEN_SLOT; + esp32_stepper_2pwm_motor_slots[3].pin1pwm = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + } + + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1a, pinA); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1b, pinB); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2a, pinC); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2b, pinD); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +// ESP32 uses MCPWM +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // determine which motor slot is the motor connected to + for(int i = 0; i < 4; i++){ + if(esp32_stepper_2pwm_motor_slots[i].pin1pwm == pinA){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100] + mcpwm_set_duty(esp32_stepper_2pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_2pwm_motor_slots[i].mcpwm_operator, dc_a*100.0); + mcpwm_set_duty(esp32_stepper_2pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_2pwm_motor_slots[i].mcpwm_operator, dc_b*100.0); + break; + } + } +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +// ESP32 uses MCPWM +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // determine which motor slot is the motor connected to + for(int i = 0; i < 4; i++){ + if(esp32_bldc_3pwm_motor_slots[i].pinA == pinA){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100] + mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_a*100.0); + mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_b*100.0); + mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_c*100.0); + break; + } + } +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +// ESP32 uses MCPWM +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // determine which motor slot is the motor connected to + for(int i = 0; i < 2; i++){ + if(esp32_stepper_4pwm_motor_slots[i].pin1A == pin1A){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100] + mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator1, dc_1a*100.0); + mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator1, dc_1b*100.0); + mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator2, dc_2a*100.0); + mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator2, dc_2b*100.0); + break; + } + } +} + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency + else pwm_frequency = _constrain(pwm_frequency, 0, 40000); // constrain to 40kHz max - centered pwm has twice lower frequency + bldc_6pwm_motor_slots_t m_slot = {}; + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 2; slot_num++){ + if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h; + m_slot = esp32_bldc_6pwm_motor_slots[slot_num]; + break; + } + } + // if no slots available + if(slot_num >= 2) return -1; + + // disable all the slots with the same MCPWM + if( slot_num == 0 ){ + // slots 0 and 1 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; + }else{ + // slots 2 and 3 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; + } + + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_al, pinA_l); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bh, pinB_h); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bl, pinB_l); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ch, pinC_h); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_cl, pinC_l); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit, dead_zone); + // return + return 0; +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + // determine which motor slot is the motor connected to + for(int i = 0; i < 2; i++){ + if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100.0] + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0); + break; + } + } +} +#endif \ No newline at end of file diff --git a/minimal_project_examples/esp32_bldc_magnetic_spi/src/sensors/MagneticSensorSPI.cpp b/minimal_project_examples/esp32_bldc_magnetic_spi/src/sensors/MagneticSensorSPI.cpp new file mode 100644 index 00000000..58660e0b --- /dev/null +++ b/minimal_project_examples/esp32_bldc_magnetic_spi/src/sensors/MagneticSensorSPI.cpp @@ -0,0 +1,250 @@ +#include "MagneticSensorSPI.h" + +/** Typical configuration for the 14bit AMS AS5147 magnetic sensor over SPI interface */ +MagneticSensorSPIConfig_s AS5147_SPI = { + .spi_mode = SPI_MODE1, + .clock_speed = 1000000, + .bit_resolution = 14, + .angle_register = 0x3FFF, + .data_start_bit = 13, + .command_rw_bit = 14, + .command_parity_bit = 15 +}; + +/** Typical configuration for the 14bit MonolithicPower MA730 magnetic sensor over SPI interface */ +MagneticSensorSPIConfig_s MA730_SPI = { + .spi_mode = SPI_MODE0, + .clock_speed = 1000000, + .bit_resolution = 14, + .angle_register = 0x0000, + .data_start_bit = 15, + .command_rw_bit = 0, // not required + .command_parity_bit = 0 // parity not implemented +}; + + +// MagneticSensorSPI(int cs, float _bit_resolution, int _angle_register) +// cs - SPI chip select pin +// _bit_resolution sensor resolution bit number +// _angle_register - (optional) angle read register - default 0x3FFF +MagneticSensorSPI::MagneticSensorSPI(int cs, float _bit_resolution, int _angle_register){ + + chip_select_pin = cs; + // angle read register of the magnetic sensor + angle_register = _angle_register ? _angle_register : DEF_ANGLE_REGISTER; + // register maximum value (counts per revolution) + cpr = pow(2,_bit_resolution); + spi_mode = SPI_MODE1; + clock_speed = 1000000; + bit_resolution = _bit_resolution; + + command_parity_bit = 15; // for backwards compatibilty + command_rw_bit = 14; // for backwards compatibilty + data_start_bit = 13; // for backwards compatibilty + +} + +MagneticSensorSPI::MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs){ + chip_select_pin = cs; + // angle read register of the magnetic sensor + angle_register = config.angle_register ? config.angle_register : DEF_ANGLE_REGISTER; + // register maximum value (counts per revolution) + cpr = pow(2, config.bit_resolution); + spi_mode = config.spi_mode; + clock_speed = config.clock_speed; + bit_resolution = config.bit_resolution; + + command_parity_bit = config.command_parity_bit; // for backwards compatibilty + command_rw_bit = config.command_rw_bit; // for backwards compatibilty + data_start_bit = config.data_start_bit; // for backwards compatibilty +} + +void MagneticSensorSPI::init(SPIClass* _spi){ + + spi = _spi; + + // 1MHz clock (AMS should be able to accept up to 10MHz) + settings = SPISettings(clock_speed, MSBFIRST, spi_mode); + + //setup pins + pinMode(chip_select_pin, OUTPUT); + + //SPI has an internal SPI-device counter, it is possible to call "begin()" from different devices + spi->begin(); +#ifndef ESP_H // if not ESP32 board + spi->setBitOrder(MSBFIRST); // Set the SPI_1 bit order + spi->setDataMode(spi_mode) ; + spi->setClockDivider(SPI_CLOCK_DIV8); +#endif + + digitalWrite(chip_select_pin, HIGH); + // velocity calculation init + angle_prev = 0; + velocity_calc_timestamp = _micros(); + + // full rotations tracking number + full_rotation_offset = 0; + angle_data_prev = getRawCount(); + zero_offset = 0; +} + +// Shaft angle calculation +// angle is in radians [rad] +float MagneticSensorSPI::getAngle(){ + // raw data from the sensor + float angle_data = getRawCount(); + + // tracking the number of rotations + // in order to expand angle range form [0,2PI] + // to basically infinity + float d_angle = angle_data - angle_data_prev; + // if overflow happened track it as full rotation + if(abs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; + // save the current angle value for the next steps + // in order to know if overflow happened + angle_data_prev = angle_data; + + // zero offset adding + angle_data -= (int)zero_offset; + // return the full angle + // (number of full rotations)*2PI + current sensor angle + return natural_direction * (full_rotation_offset + ( angle_data / (float)cpr) * _2PI); +} + +// Shaft velocity calculation +float MagneticSensorSPI::getVelocity(){ + // calculate sample time + unsigned long now_us = _micros(); + float Ts = (now_us - velocity_calc_timestamp)*1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + + // current angle + float angle_c = getAngle(); + // velocity calculation + float vel = (angle_c - angle_prev)/Ts; + + // save variables for future pass + angle_prev = angle_c; + velocity_calc_timestamp = now_us; + return vel; +} + +// set current angle as zero angle +// return the angle [rad] difference +float MagneticSensorSPI::initRelativeZero(){ + float angle_offset = -getAngle(); + zero_offset = natural_direction * getRawCount(); + + // angle tracking variables + full_rotation_offset = 0; + return angle_offset; +} +// set absolute zero angle as zero angle +// return the angle [rad] difference +float MagneticSensorSPI::initAbsoluteZero(){ + float rotation = -(int)zero_offset; + // init absolute zero + zero_offset = 0; + + // angle tracking variables + full_rotation_offset = 0; + // return offset in radians + return rotation / (float)cpr * _2PI; +} +// returns 0 if it has no absolute 0 measurement +// 0 - incremental encoder without index +// 1 - encoder with index & magnetic sensors +int MagneticSensorSPI::hasAbsoluteZero(){ + return 1; +} +// returns 0 if it does need search for absolute zero +// 0 - magnetic sensor +// 1 - ecoder with index +int MagneticSensorSPI::needsAbsoluteZeroSearch(){ + return 0; +} + + +// function reading the raw counter of the magnetic sensor +int MagneticSensorSPI::getRawCount(){ + return (int)MagneticSensorSPI::read(angle_register); +} + +// SPI functions +/** + * Utility function used to calculate even parity of word + */ +byte MagneticSensorSPI::spiCalcEvenParity(word value){ + byte cnt = 0; + byte i; + + for (i = 0; i < 16; i++) + { + if (value & 0x1) cnt++; + value >>= 1; + } + return cnt & 0x1; +} + + /* + * Read a register from the sensor + * Takes the address of the register as a 16 bit word + * Returns the value of the register + */ +word MagneticSensorSPI::read(word angle_register){ + + word command = angle_register; + + if (command_rw_bit > 0) { + command = angle_register | (1 << command_rw_bit); + } + if (command_parity_bit > 0) { + //Add a parity bit on the the MSB + command |= ((word)spiCalcEvenParity(command) << command_parity_bit); + } + +#if !defined(_STM32_DEF_) // if not stm chips + //SPI - begin transaction + spi->beginTransaction(settings); +#endif + + //Send the command + digitalWrite(chip_select_pin, LOW); + digitalWrite(chip_select_pin, LOW); + spi->transfer16(command); + digitalWrite(chip_select_pin,HIGH); + digitalWrite(chip_select_pin,HIGH); + +#if defined( ESP_H ) // if ESP32 board + delayMicroseconds(50); +#else + delayMicroseconds(10); +#endif + + //Now read the response + digitalWrite(chip_select_pin, LOW); + digitalWrite(chip_select_pin, LOW); + word register_value = spi->transfer16(0x00); + digitalWrite(chip_select_pin, HIGH); + digitalWrite(chip_select_pin,HIGH); + +#if !defined(_STM32_DEF_) // if not stm chips + //SPI - end transaction + spi->endTransaction(); +#endif + + register_value = register_value >> (1 + data_start_bit - bit_resolution); //this should shift data to the rightmost bits of the word + + const static word data_mask = 0xFFFF >> (16 - bit_resolution); + + return register_value & data_mask; // Return the data, stripping the non data (e.g parity) bits +} + +/** + * Closes the SPI connection + * SPI has an internal SPI-device counter, for each init()-call the close() function must be called exactly 1 time + */ +void MagneticSensorSPI::close(){ + spi->end(); +} diff --git a/minimal_project_examples/esp32_bldc_magnetic_spi/src/sensors/MagneticSensorSPI.h b/minimal_project_examples/esp32_bldc_magnetic_spi/src/sensors/MagneticSensorSPI.h new file mode 100644 index 00000000..05fb9091 --- /dev/null +++ b/minimal_project_examples/esp32_bldc_magnetic_spi/src/sensors/MagneticSensorSPI.h @@ -0,0 +1,110 @@ +#ifndef MAGNETICSENSORSPI_LIB_H +#define MAGNETICSENSORSPI_LIB_H + +#include "Arduino.h" +#include +#include "../common/base_classes/Sensor.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + +#define DEF_ANGLE_REGISTER 0x3FFF + +struct MagneticSensorSPIConfig_s { + int spi_mode; + long clock_speed; + int bit_resolution; + int angle_register; + int data_start_bit; + int command_rw_bit; + int command_parity_bit; +}; +// typical configuration structures +extern MagneticSensorSPIConfig_s AS5147_SPI, MA730_SPI; + +class MagneticSensorSPI: public Sensor{ + public: + /** + * MagneticSensorSPI class constructor + * @param cs SPI chip select pin + * @param bit_resolution sensor resolution bit number + * @param angle_register (optional) angle read register - default 0x3FFF + */ + MagneticSensorSPI(int cs, float bit_resolution, int angle_register = 0); + /** + * MagneticSensorSPI class constructor + * @param config SPI config + * @param cs SPI chip select pin + */ + MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs); + + /** sensor initialise pins */ + void init(SPIClass* _spi = &SPI); + + // implementation of abstract functions of the Sensor class + /** get current angle (rad) */ + float getAngle() override; + /** get current angular velocity (rad/s) **/ + float getVelocity() override; + /** + * set current angle as zero angle + * return the angle [rad] difference + */ + float initRelativeZero() override; + /** + * set absolute zero angle as zero angle + * return the angle [rad] difference + */ + float initAbsoluteZero() override; + /** returns 1 because it is the absolute sensor */ + int hasAbsoluteZero() override; + /** returns 0 maning it doesn't need search for absolute zero */ + + int needsAbsoluteZeroSearch() override; + + // returns the spi mode (phase/polarity of read/writes) i.e one of SPI_MODE0 | SPI_MODE1 | SPI_MODE2 | SPI_MODE3 + int spi_mode; + + /* returns the speed of the SPI clock signal */ + long clock_speed; + + + private: + float cpr; //!< Maximum range of the magnetic sensor + // spi variables + int angle_register; //!< SPI angle register to read + int chip_select_pin; //!< SPI chip select pin + SPISettings settings; //!< SPI settings variable + // spi functions + /** Stop SPI communication */ + void close(); + /** Read one SPI register value */ + word read(word angle_register); + /** Calculate parity value */ + byte spiCalcEvenParity(word value); + + + word zero_offset; //!< user defined zero offset + /** + * Function getting current angle register value + * it uses angle_register variable + */ + int getRawCount(); + + // total angle tracking variables + float full_rotation_offset; //!println("MOT: Init variables."); + + // sanity check for the voltage limit configuration + if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; + // constrain voltage for sensor alignment + if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; + + // update the controller limits + PID_velocity.limit = voltage_limit; + P_angle.limit = velocity_limit; + + _delay(500); + // enable motor + if(monitor_port) monitor_port->println("MOT: Enable."); + enable(); + _delay(500); + +} + + +// disable motor driver +void StepperMotor::disable() +{ + // set zero to PWM + driver->setPwm(0, 0); + // disable driver + driver->disable(); +} +// enable motor driver +void StepperMotor::enable() +{ + // disable enable + driver->enable(); + // set zero to PWM + driver->setPwm(0, 0); +} + + +/** + FOC functions +*/ +// FOC initialization function +int StepperMotor::initFOC( float zero_electric_offset, Direction sensor_direction ) { + int exit_flag = 1; + // align motor if necessary + // alignment necessary for encoders! + if(zero_electric_offset != NOT_SET){ + // abosolute zero offset provided - no need to align + zero_electric_angle = zero_electric_offset; + // set the sensor direction - default CW + sensor->natural_direction = sensor_direction; + }else{ + // sensor and motor alignment + _delay(500); + exit_flag = alignSensor(); + _delay(500); + } + if(monitor_port) monitor_port->println("MOT: Motor ready."); + + return exit_flag; +} +// Encoder alignment to electrical 0 angle +int StepperMotor::alignSensor() { + if(monitor_port) monitor_port->println("MOT: Align sensor."); + // align the electrical phases of the motor and sensor + // set angle -90 degrees + + float start_angle = shaftAngle(); + for (int i = 0; i <=5; i++ ) { + float angle = _3PI_2 + _2PI * i / 6.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(200); + } + float mid_angle = shaftAngle(); + for (int i = 5; i >=0; i-- ) { + float angle = _3PI_2 + _2PI * i / 6.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(200); + } + if (mid_angle < start_angle) { + if(monitor_port) monitor_port->println("MOT: natural_direction==CCW"); + sensor->natural_direction = Direction::CCW; + } else if (mid_angle == start_angle) { + if(monitor_port) monitor_port->println("MOT: Sensor failed to notice movement"); + } else{ + if(monitor_port) monitor_port->println("MOT: natural_direction==CW"); + } + + // let the motor stabilize for 2 sec + _delay(2000); + // set sensor to zero + sensor->initRelativeZero(); + _delay(500); + setPhaseVoltage(0, 0, 0); + _delay(200); + + // find the index if available + int exit_flag = absoluteZeroAlign(); + _delay(500); + if(monitor_port){ + if(exit_flag< 0 ) monitor_port->println("MOT: Error: Not found!"); + if(exit_flag> 0 ) monitor_port->println("MOT: Success!"); + else monitor_port->println("MOT: Not available!"); + } + return exit_flag; +} + + +// Encoder alignment the absolute zero angle +// - to the index +int StepperMotor::absoluteZeroAlign() { + + if(monitor_port) monitor_port->println("MOT: Absolute zero align."); + // if no absolute zero return + if(!sensor->hasAbsoluteZero()) return 0; + + + if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println("MOT: Searching..."); + // search the absolute zero with small velocity + while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){ + loopFOC(); + voltage_q = PID_velocity(velocity_index_search - shaftVelocity()); + } + voltage_q = 0; + voltage_d = 0; + // disable motor + setPhaseVoltage(0, 0, 0); + + // align absolute zero if it has been found + if(!sensor->needsAbsoluteZeroSearch()){ + // align the sensor with the absolute zero + float zero_offset = sensor->initAbsoluteZero(); + // remember zero electric angle + zero_electric_angle = _normalizeAngle(_electricalAngle(zero_offset, pole_pairs)); + } + // return bool if zero found + return !sensor->needsAbsoluteZeroSearch() ? 1 : -1; +} + +// Iterative function looping FOC algorithm, setting Uq on the Motor +// The faster it can be run the better +void StepperMotor::loopFOC() { + // shaft angle + shaft_angle = shaftAngle(); + // set the phase voltage - FOC heart function :) + setPhaseVoltage(voltage_q, voltage_d, _electricalAngle(shaft_angle, pole_pairs)); +} + +// Iterative function running outer loop of the FOC algorithm +// Behavior of this function is determined by the motor.controller variable +// It runs either angle, velocity or voltage loop +// - needs to be called iteratively it is asynchronous function +// - if target is not set it uses motor.target value +void StepperMotor::move(float new_target) { + // set internal target variable + if( new_target != NOT_SET ) target = new_target; + // get angular velocity + shaft_velocity = shaftVelocity(); + // choose control loop + switch (controller) { + case ControlType::voltage: + voltage_q = target; + break; + case ControlType::angle: + // angle set point + // include angle loop + shaft_angle_sp = target; + shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); + voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + break; + case ControlType::velocity: + // velocity set point + // include velocity loop + shaft_velocity_sp = target; + voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + break; + case ControlType::velocity_openloop: + // velocity control in open loop + // loopFOC should not be called + shaft_velocity_sp = target; + velocityOpenloop(shaft_velocity_sp); + break; + case ControlType::angle_openloop: + // angle control in open loop + // loopFOC should not be called + shaft_angle_sp = target; + angleOpenloop(shaft_angle_sp); + break; + } +} + + +// Method using FOC to set Uq and Ud to the motor at the optimal angle +// Function implementing Sine PWM algorithms +// - space vector not implemented yet +// +// Function using sine approximation +// regular sin + cos ~300us (no memory usaage) +// approx _sin + _cos ~110us (400Byte ~ 20% of memory) +void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { + // Sinusoidal PWM modulation + // Inverse Park transformation + + // angle normalization in between 0 and 2pi + // only necessary if using _sin and _cos - approximation functions + angle_el = _normalizeAngle(angle_el + zero_electric_angle); + float _ca = _cos(angle_el); + float _sa = _sin(angle_el); + // Inverse park transform + Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; + Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; + + // set the voltages in hardware + driver->setPwm(Ualpha, Ubeta); +} + +// Function (iterative) generating open loop movement for target velocity +// - target_velocity - rad/s +// it uses voltage_limit variable +void StepperMotor::velocityOpenloop(float target_velocity){ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6; + + // calculate the necessary angle to achieve target velocity + shaft_angle += target_velocity*Ts; + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle,pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; +} + +// Function (iterative) generating open loop movement towards the target angle +// - target_angle - rad +// it uses voltage_limit and velocity_limit variables +void StepperMotor::angleOpenloop(float target_angle){ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6; + + // calculate the necessary angle to move from current position towards target angle + // with maximal velocity (velocity_limit) + if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)) + shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; + else + shaft_angle = target_angle; + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle,pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; +} \ No newline at end of file diff --git a/minimal_project_examples/esp32_stepper_openloop/src/StepperMotor.h b/minimal_project_examples/esp32_stepper_openloop/src/StepperMotor.h new file mode 100644 index 00000000..af4783bf --- /dev/null +++ b/minimal_project_examples/esp32_stepper_openloop/src/StepperMotor.h @@ -0,0 +1,117 @@ +/** + * @file StepperMotor.h + * + */ + +#ifndef StepperMotor_h +#define StepperMotor_h + +#include "Arduino.h" +#include "common/base_classes/FOCMotor.h" +#include "common/base_classes/StepperDriver.h" +#include "common/base_classes/Sensor.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" +#include "common/defaults.h" + +/** + Stepper Motor class +*/ +class StepperMotor: public FOCMotor +{ + public: + /** + StepperMotor class constructor + @param pp pole pair number - cpr counts per rotation number (cpm=ppm*4) + */ + StepperMotor(int pp); + + /** + * Function linking a motor and a foc driver + * + * @param driver StepperDriver class implementing all the hardware specific functions necessary PWM setting + */ + void linkDriver(StepperDriver* driver); + + /** + * StepperDriver link: + * - 4PWM - L298N for example + */ + StepperDriver* driver; + + /** Motor hardware init function */ + void init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + * + * - If zero_electric_offset parameter is set the alignment procedure is skipped + * + * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. + * @param sensor_direction sensor natural direction - default is CW + * + */ + int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override; + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + void loopFOC() override; + /** + * Function executing the control loops set by the controller parameter of the BLDCMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + void move(float target = NOT_SET) override; + + float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform + + private: + + // FOC methods + /** + * Method using FOC to set Uq to the motor at the optimal angle + * Heart of the FOC algorithm + * + * @param Uq Current voltage in q axis to set to the motor + * @param Ud Current voltage in d axis to set to the motor + * @param angle_el current electrical angle of the motor + */ + void setPhaseVoltage(float Uq, float Ud , float angle_el); + + /** Sensor alignment to electrical 0 angle of the motor */ + int alignSensor(); + /** Motor and sensor alignment to the sensors absolute 0 angle */ + int absoluteZeroAlign(); + + // Open loop motion control + /** + * Function (iterative) generating open loop movement for target velocity + * it uses voltage_limit variable + * + * @param target_velocity - rad/s + */ + void velocityOpenloop(float target_velocity); + /** + * Function (iterative) generating open loop movement towards the target angle + * it uses voltage_limit and velocity_limit variables + * + * @param target_angle - rad + */ + void angleOpenloop(float target_angle); + // open loop variables + long open_loop_timestamp; +}; + + +#endif diff --git a/minimal_project_examples/esp32_stepper_openloop/src/common/base_classes/BLDCDriver.h b/minimal_project_examples/esp32_stepper_openloop/src/common/base_classes/BLDCDriver.h new file mode 100644 index 00000000..708323fb --- /dev/null +++ b/minimal_project_examples/esp32_stepper_openloop/src/common/base_classes/BLDCDriver.h @@ -0,0 +1,28 @@ +#ifndef BLDCDRIVER_H +#define BLDCDRIVER_H + +class BLDCDriver{ + public: + + /** Initialise hardware */ + virtual int init(); + /** Enable hardware */ + virtual void enable(); + /** Disable hardware */ + virtual void disable(); + + long pwm_frequency; //!< pwm frequency value in hertz + float voltage_power_supply; //!< power supply voltage + float voltage_limit; //!< limiting voltage set to the motor + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + virtual void setPwm(float Ua, float Ub, float Uc); +}; + +#endif \ No newline at end of file diff --git a/minimal_project_examples/esp32_stepper_openloop/src/common/base_classes/FOCMotor.cpp b/minimal_project_examples/esp32_stepper_openloop/src/common/base_classes/FOCMotor.cpp new file mode 100644 index 00000000..1d1a0f43 --- /dev/null +++ b/minimal_project_examples/esp32_stepper_openloop/src/common/base_classes/FOCMotor.cpp @@ -0,0 +1,241 @@ +#include "FOCMotor.h" + +/** + * Default constructor - setting all variabels to default values + */ +FOCMotor::FOCMotor() +{ + // maximum angular velocity to be used for positioning + velocity_limit = DEF_VEL_LIM; + // maximum voltage to be set to the motor + voltage_limit = DEF_POWER_SUPPLY; + + // index search velocity + velocity_index_search = DEF_INDEX_SEARCH_TARGET_VELOCITY; + // sensor and motor align voltage + voltage_sensor_align = DEF_VOLTAGE_SENSOR_ALIGN; + + // electric angle of comthe zero angle + zero_electric_angle = 0; + + // default modulation is SinePWM + foc_modulation = FOCModulationType::SinePWM; + + // default target value + target = 0; + voltage_d = 0; + voltage_q = 0; + + //monitor_port + monitor_port = nullptr; + //sensor + sensor = nullptr; +} + + +/** + Sensor communication methods +*/ +void FOCMotor::linkSensor(Sensor* _sensor) { + sensor = _sensor; +} +// shaft angle calculation +float FOCMotor::shaftAngle() { + // if no sensor linked return 0 + if(!sensor) return shaft_angle; + return sensor->getAngle(); +} +// shaft velocity calculation +float FOCMotor::shaftVelocity() { + // if no sensor linked return 0 + if(!sensor) return 0; + return LPF_velocity(sensor->getVelocity()); +} + +/** + * Monitoring functions + */ +// function implementing the monitor_port setter +void FOCMotor::useMonitoring(Print &print){ + monitor_port = &print; //operate on the address of print + if(monitor_port ) monitor_port->println("MOT: Monitor enabled!"); +} +// utility function intended to be used with serial plotter to monitor motor variables +// significantly slowing the execution down!!!! +void FOCMotor::monitor() { + if(!monitor_port) return; + switch (controller) { + case ControlType::velocity_openloop: + case ControlType::velocity: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_velocity_sp); + monitor_port->print("\t"); + monitor_port->println(shaft_velocity); + break; + case ControlType::angle_openloop: + case ControlType::angle: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_angle_sp); + monitor_port->print("\t"); + monitor_port->println(shaft_angle); + break; + case ControlType::voltage: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_angle); + monitor_port->print("\t"); + monitor_port->println(shaft_velocity); + break; + } +} + +int FOCMotor::command(String user_command) { + // error flag + int errorFlag = 1; + // if empty string + if(user_command.length() < 1) return errorFlag; + + // parse command letter + char cmd = user_command.charAt(0); + // check if get command + char GET = user_command.charAt(1) == '\n'; + // parse command values + float value = user_command.substring(1).toFloat(); + + // a bit of optimisation of variable memory for Arduino UNO (atmega328) + switch(cmd){ + case 'P': // velocity P gain change + case 'I': // velocity I gain change + case 'D': // velocity D gain change + case 'R': // velocity voltage ramp change + if(monitor_port) monitor_port->print(" PID velocity| "); + break; + case 'F': // velocity Tf low pass filter change + if(monitor_port) monitor_port->print(" LPF velocity| "); + break; + case 'K': // angle loop gain P change + if(monitor_port) monitor_port->print(" P angle| "); + break; + case 'L': // velocity voltage limit change + case 'N': // angle loop gain velocity_limit change + if(monitor_port) monitor_port->print(" Limits| "); + break; + } + + // apply the the command + switch(cmd){ + case 'P': // velocity P gain change + if(monitor_port) monitor_port->print("P: "); + if(!GET) PID_velocity.P = value; + if(monitor_port) monitor_port->println(PID_velocity.P); + break; + case 'I': // velocity I gain change + if(monitor_port) monitor_port->print("I: "); + if(!GET) PID_velocity.I = value; + if(monitor_port) monitor_port->println(PID_velocity.I); + break; + case 'D': // velocity D gain change + if(monitor_port) monitor_port->print("D: "); + if(!GET) PID_velocity.D = value; + if(monitor_port) monitor_port->println(PID_velocity.D); + break; + case 'R': // velocity voltage ramp change + if(monitor_port) monitor_port->print("volt_ramp: "); + if(!GET) PID_velocity.output_ramp = value; + if(monitor_port) monitor_port->println(PID_velocity.output_ramp); + break; + case 'L': // velocity voltage limit change + if(monitor_port) monitor_port->print("volt_limit: "); + if(!GET) { + voltage_limit = value; + PID_velocity.limit = value; + } + if(monitor_port) monitor_port->println(voltage_limit); + break; + case 'F': // velocity Tf low pass filter change + if(monitor_port) monitor_port->print("Tf: "); + if(!GET) LPF_velocity.Tf = value; + if(monitor_port) monitor_port->println(LPF_velocity.Tf); + break; + case 'K': // angle loop gain P change + if(monitor_port) monitor_port->print(" P: "); + if(!GET) P_angle.P = value; + if(monitor_port) monitor_port->println(P_angle.P); + break; + case 'N': // angle loop gain velocity_limit change + if(monitor_port) monitor_port->print("vel_limit: "); + if(!GET){ + velocity_limit = value; + P_angle.limit = value; + } + if(monitor_port) monitor_port->println(velocity_limit); + break; + case 'C': + // change control type + if(monitor_port) monitor_port->print("Control: "); + + if(GET){ // if get command + switch(controller){ + case ControlType::voltage: + if(monitor_port) monitor_port->println("voltage"); + break; + case ControlType::velocity: + if(monitor_port) monitor_port->println("velocity"); + break; + case ControlType::angle: + if(monitor_port) monitor_port->println("angle"); + break; + default: + if(monitor_port) monitor_port->println("open loop"); + } + }else{ // if set command + switch((int)value){ + case 0: + if(monitor_port) monitor_port->println("voltage"); + controller = ControlType::voltage; + break; + case 1: + if(monitor_port) monitor_port->println("velocity"); + controller = ControlType::velocity; + break; + case 2: + if(monitor_port) monitor_port->println("angle"); + controller = ControlType::angle; + break; + default: // not valid command + errorFlag = 0; + } + } + break; + case 'V': // get current values of the state variables + switch((int)value){ + case 0: // get voltage + if(monitor_port) monitor_port->print("Uq: "); + if(monitor_port) monitor_port->println(voltage_q); + break; + case 1: // get velocity + if(monitor_port) monitor_port->print("Velocity: "); + if(monitor_port) monitor_port->println(shaft_velocity); + break; + case 2: // get angle + if(monitor_port) monitor_port->print("Angle: "); + if(monitor_port) monitor_port->println(shaft_angle); + break; + case 3: // get angle + if(monitor_port) monitor_port->print("Target: "); + if(monitor_port) monitor_port->println(target); + break; + default: // not valid command + errorFlag = 0; + } + break; + default: // target change + if(monitor_port) monitor_port->print("Target : "); + target = user_command.toFloat(); + if(monitor_port) monitor_port->println(target); + } + // return 0 if error and 1 if ok + return errorFlag; +} \ No newline at end of file diff --git a/minimal_project_examples/esp32_stepper_openloop/src/common/base_classes/FOCMotor.h b/minimal_project_examples/esp32_stepper_openloop/src/common/base_classes/FOCMotor.h new file mode 100644 index 00000000..986e1ab2 --- /dev/null +++ b/minimal_project_examples/esp32_stepper_openloop/src/common/base_classes/FOCMotor.h @@ -0,0 +1,197 @@ +#ifndef FOCMOTOR_H +#define FOCMOTOR_H + +#include "Arduino.h" +#include "Sensor.h" +#include "BLDCDriver.h" + +#include "../time_utils.h" +#include "../foc_utils.h" +#include "../defaults.h" +#include "../pid.h" +#include "../lowpass_filter.h" + + +/** + * Motiron control type + */ +enum ControlType{ + voltage,//!< Torque control using voltage + velocity,//!< Velocity motion control + angle,//!< Position/angle motion control + velocity_openloop, + angle_openloop +}; + +/** + * FOC modulation type + */ +enum FOCModulationType{ + SinePWM, //!< Sinusoidal PWM modulation + SpaceVectorPWM, //!< Space vector modulation method + Trapezoid_120, + Trapezoid_150 +}; + +/** + Generic motor class +*/ +class FOCMotor +{ + public: + /** + * Default constructor - setting all variabels to default values + */ + FOCMotor(); + + /** Motor hardware init function */ + virtual void init()=0; + /** Motor disable function */ + virtual void disable()=0; + /** Motor enable function */ + virtual void enable()=0; + + /** + * Function linking a motor and a sensor + * + * @param sensor Sensor class wrapper for the FOC algorihtm to read the motor angle and velocity + */ + void linkSensor(Sensor* sensor); + + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + * + * - If zero_electric_offset parameter is set the alignment procedure is skipped + * + * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. + * @param sensor_direction sensor natural direction - default is CW + * + */ + virtual int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW)=0; + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + virtual void loopFOC()=0; + /** + * Function executing the control loops set by the controller parameter of the BLDCMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + virtual void move(float target = NOT_SET)=0; + + // State calculation methods + /** Shaft angle calculation in radians [rad] */ + float shaftAngle(); + /** + * Shaft angle calculation function in radian per second [rad/s] + * It implements low pass filtering + */ + float shaftVelocity(); + + // state variables + float target; //!< current target value - depends of the controller + float shaft_angle;//!< current motor angle + float shaft_velocity;//!< current motor velocity + float shaft_velocity_sp;//!< current target velocity + float shaft_angle_sp;//!< current target angle + float voltage_q;//!< current voltage u_q set + float voltage_d;//!< current voltage u_d set + + // motor configuration parameters + float voltage_sensor_align;//!< sensor and motor align voltage parameter + float velocity_index_search;//!< target velocity for index search + int pole_pairs;//!< Motor pole pairs number + + // limiting variables + float voltage_limit; //!< Voltage limitting variable - global limit + float velocity_limit; //!< Velocity limitting variable - global limit + + float zero_electric_angle;//! _2PI ? a_sin - _2PI : a_sin; + return _sin(a_sin); +} + + +// normalizing radian angle to [0,2PI] +float _normalizeAngle(float angle){ + float a = fmod(angle, _2PI); + return a >= 0 ? a : (a + _2PI); +} + +// Electrical angle calculation +float _electricalAngle(float shaft_angle, int pole_pairs) { + return (shaft_angle * pole_pairs); +} \ No newline at end of file diff --git a/minimal_project_examples/esp32_stepper_openloop/src/common/foc_utils.h b/minimal_project_examples/esp32_stepper_openloop/src/common/foc_utils.h new file mode 100644 index 00000000..7da3f7bc --- /dev/null +++ b/minimal_project_examples/esp32_stepper_openloop/src/common/foc_utils.h @@ -0,0 +1,55 @@ +#ifndef FOCUTILS_LIB_H +#define FOCUTILS_LIB_H + +#include "Arduino.h" + +// sign function +#define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) ) +#define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) +#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) + +// utility defines +#define _2_SQRT3 1.15470053838 +#define _SQRT3 1.73205080757 +#define _1_SQRT3 0.57735026919 +#define _SQRT3_2 0.86602540378 +#define _SQRT2 1.41421356237 +#define _120_D2R 2.09439510239 +#define _PI 3.14159265359 +#define _PI_2 1.57079632679 +#define _PI_3 1.0471975512 +#define _2PI 6.28318530718 +#define _3PI_2 4.71238898038 + +#define NOT_SET -12345.0 + +/** + * Function approximating the sine calculation by using fixed size array + * - execution time ~40us (Arduino UNO) + * + * @param a angle in between 0 and 2PI + */ +float _sin(float a); +/** + * Function approximating cosine calculation by using fixed size array + * - execution time ~50us (Arduino UNO) + * + * @param a angle in between 0 and 2PI + */ +float _cos(float a); + +/** + * normalizing radian angle to [0,2PI] + * @param angle - angle to be normalized + */ +float _normalizeAngle(float angle); + + +/** + * Electrical angle calculation + * + * @param shaft_angle - shaft angle of the motor + * @param pole_pairs - number of pole pairs + */ +float _electricalAngle(float shaft_angle, int pole_pairs); +#endif \ No newline at end of file diff --git a/minimal_project_examples/esp32_stepper_openloop/src/common/lowpass_filter.cpp b/minimal_project_examples/esp32_stepper_openloop/src/common/lowpass_filter.cpp new file mode 100644 index 00000000..9bf4ab7a --- /dev/null +++ b/minimal_project_examples/esp32_stepper_openloop/src/common/lowpass_filter.cpp @@ -0,0 +1,25 @@ +#include "lowpass_filter.h" + +LowPassFilter::LowPassFilter(float time_constant) + : Tf(time_constant) + , y_prev(0.0f) +{ + timestamp_prev = _micros(); +} + + +float LowPassFilter::operator() (float x) +{ + unsigned long timestamp = _micros(); + float dt = (timestamp - timestamp_prev)*1e-6f; + + if (dt < 0.0f || dt > 0.5f) + dt = 1e-3f; + + float alpha = Tf/(Tf + dt); + float y = alpha*y_prev + (1.0f - alpha)*x; + + y_prev = y; + timestamp_prev = timestamp; + return y; +} \ No newline at end of file diff --git a/minimal_project_examples/esp32_stepper_openloop/src/common/lowpass_filter.h b/minimal_project_examples/esp32_stepper_openloop/src/common/lowpass_filter.h new file mode 100644 index 00000000..7c51be54 --- /dev/null +++ b/minimal_project_examples/esp32_stepper_openloop/src/common/lowpass_filter.h @@ -0,0 +1,29 @@ +#ifndef LOWPASS_FILTER_H +#define LOWPASS_FILTER_H + + +#include "time_utils.h" +#include "foc_utils.h" + + +/** + * Low pass filter class + */ +class LowPassFilter +{ +public: + /** + * @param Tf - Low pass filter time constant + */ + LowPassFilter(float Tf); + ~LowPassFilter() = default; + + float operator() (float x); + float Tf; //!< Low pass filter time constant + +protected: + unsigned long timestamp_prev; //!< Last execution timestamp + float y_prev; //!< filtered value in previous execution step +}; + +#endif // LOWPASS_FILTER_H \ No newline at end of file diff --git a/minimal_project_examples/esp32_stepper_openloop/src/common/pid.cpp b/minimal_project_examples/esp32_stepper_openloop/src/common/pid.cpp new file mode 100644 index 00000000..1a0a9828 --- /dev/null +++ b/minimal_project_examples/esp32_stepper_openloop/src/common/pid.cpp @@ -0,0 +1,56 @@ +#include "pid.h" + +PIDController::PIDController(float P, float I, float D, float ramp, float limit) + : P(P) + , I(I) + , D(D) + , output_ramp(ramp) // output derivative limit [volts/second] + , limit(limit) // output supply limit [volts] + , integral_prev(0.0) + , error_prev(0.0) + , output_prev(0.0) +{ + timestamp_prev = _micros()*1e-6; +} + +// PID controller function +float PIDController::operator() (float error){ + // calculate the time from the last call + unsigned long timestamp_now = _micros(); + float Ts = (timestamp_now - timestamp_prev) * 1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + + // u(s) = (P + I/s + Ds)e(s) + // Discrete implementations + // proportional part + // u_p = P *e(k) + float proportional = P * error; + // Tustin transform of the integral part + // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) + float integral = integral_prev + I*Ts*0.5*(error + error_prev); + // antiwindup - limit the output voltage_q + integral = _constrain(integral, -limit, limit); + // Discrete derivation + // u_dk = D(ek - ek_1)/Ts + float derivative = D*(error - error_prev)/Ts; + + // sum all the components + float output = proportional + integral + derivative; + // antiwindup - limit the output variable + output = _constrain(output, -limit, limit); + + // limit the acceleration by ramping the output + float output_rate = (output - output_prev)/Ts; + if (output_rate > output_ramp) + output = output_prev + output_ramp*Ts; + else if (output_rate < -output_ramp) + output = output_prev - output_ramp*Ts; + + // saving for the next pass + integral_prev = integral; + output_prev = output; + error_prev = error; + timestamp_prev = timestamp_now; + return output; +} \ No newline at end of file diff --git a/minimal_project_examples/esp32_stepper_openloop/src/common/pid.h b/minimal_project_examples/esp32_stepper_openloop/src/common/pid.h new file mode 100644 index 00000000..0e9ddf54 --- /dev/null +++ b/minimal_project_examples/esp32_stepper_openloop/src/common/pid.h @@ -0,0 +1,40 @@ +#ifndef PID_H +#define PID_H + + +#include "time_utils.h" +#include "foc_utils.h" + +/** + * PID controller class + */ +class PIDController +{ +public: + /** + * + * @param P - Proportional gain + * @param I - Integral gain + * @param D - Derivative gain + * @param ramp - Maximum speed of change of the output value + * @param limit - Maximum output value + */ + PIDController(float P, float I, float D, float ramp, float limit); + ~PIDController() = default; + + float operator() (float error); + + float P; //!< Proportional gain + float I; //!< Integral gain + float D; //!< Derivative gain + float output_ramp; //!< Maximum speed of change of the output value + float limit; //!< Maximum output value + +protected: + float integral_prev; //!< last integral component value + float error_prev; //!< last tracking error value + float timestamp_prev; //!< Last execution timestamp + float output_prev; //!< last pid output value +}; + +#endif // PID_H \ No newline at end of file diff --git a/minimal_project_examples/esp32_stepper_openloop/src/common/time_utils.cpp b/minimal_project_examples/esp32_stepper_openloop/src/common/time_utils.cpp new file mode 100644 index 00000000..b00a8c78 --- /dev/null +++ b/minimal_project_examples/esp32_stepper_openloop/src/common/time_utils.cpp @@ -0,0 +1,31 @@ +#include "time_utils.h" + +// function buffering delay() +// arduino uno function doesn't work well with interrupts +void _delay(unsigned long ms){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega2560__) + // if arduino uno and other atmega328p chips + // use while instad of delay, + // due to wrong measurement based on changed timer0 + unsigned long t = _micros() + ms*1000; + while( _micros() < t ){}; +#else + // regular micros + delay(ms); +#endif +} + + +// function buffering _micros() +// arduino function doesn't work well with interrupts +unsigned long _micros(){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega2560__) +// if arduino uno and other atmega328p chips + //return the value based on the prescaler + if((TCCR0B & 0b00000111) == 0x01) return (micros()/32); + else return (micros()); +#else + // regular micros + return micros(); +#endif +} diff --git a/minimal_project_examples/esp32_stepper_openloop/src/common/time_utils.h b/minimal_project_examples/esp32_stepper_openloop/src/common/time_utils.h new file mode 100644 index 00000000..143d4853 --- /dev/null +++ b/minimal_project_examples/esp32_stepper_openloop/src/common/time_utils.h @@ -0,0 +1,22 @@ +#ifndef TIME_UTILS_H +#define TIME_UTILS_H + +#include "foc_utils.h" + +/** + * Function implementing delay() function in milliseconds + * - blocking function + * - hardware specific + + * @param ms number of milliseconds to wait + */ +void _delay(unsigned long ms); + +/** + * Function implementing timestamp getting function in microseconds + * hardware specific + */ +unsigned long _micros(); + + +#endif \ No newline at end of file diff --git a/minimal_project_examples/esp32_stepper_openloop/src/drivers/StepperDriver4PWM.cpp b/minimal_project_examples/esp32_stepper_openloop/src/drivers/StepperDriver4PWM.cpp new file mode 100644 index 00000000..28e2dcae --- /dev/null +++ b/minimal_project_examples/esp32_stepper_openloop/src/drivers/StepperDriver4PWM.cpp @@ -0,0 +1,81 @@ +#include "StepperDriver4PWM.h" + +StepperDriver4PWM::StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B,int en1, int en2){ + // Pin initialization + pwm1A = ph1A; + pwm1B = ph1B; + pwm2A = ph2A; + pwm2B = ph2B; + + // enable_pin pin + enable_pin1 = en1; + enable_pin2 = en2; + + // default power-supply value + voltage_power_supply = DEF_POWER_SUPPLY; + voltage_limit = NOT_SET; + +} + +// enable motor driver +void StepperDriver4PWM::enable(){ + // enable_pin the driver - if enable_pin pin available + if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, HIGH); + if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, HIGH); + // set zero to PWM + setPwm(0,0); +} + +// disable motor driver +void StepperDriver4PWM::disable() +{ + // set zero to PWM + setPwm(0, 0); + // disable the driver - if enable_pin pin available + if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, LOW); + if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, LOW); + +} + +// init hardware pins +int StepperDriver4PWM::init() { + // a bit of separation + _delay(1000); + + // PWM pins + pinMode(pwm1A, OUTPUT); + pinMode(pwm1B, OUTPUT); + pinMode(pwm2A, OUTPUT); + pinMode(pwm2B, OUTPUT); + if(enable_pin1 != NOT_SET) pinMode(enable_pin1, OUTPUT); + if(enable_pin2 != NOT_SET) pinMode(enable_pin2, OUTPUT); + + // sanity check for the voltage limit configuration + if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + + // Set the pwm frequency to the pins + // hardware specific function - depending on driver and mcu + _configure4PWM(pwm_frequency, pwm1A, pwm1B, pwm2A, pwm2B); + return 0; +} + + +// Set voltage to the pwm pin +void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { + float duty_cycle1A(0.0),duty_cycle1B(0.0),duty_cycle2A(0.0),duty_cycle2B(0.0); + // limit the voltage in driver + Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit); + Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit); + // hardware specific writing + if( Ualpha > 0 ) + duty_cycle1B = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); + else + duty_cycle1A = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); + + if( Ubeta > 0 ) + duty_cycle2B = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0); + else + duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0); + // write to hardware + _writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, pwm1A, pwm1B, pwm2A, pwm2B); +} \ No newline at end of file diff --git a/minimal_project_examples/esp32_stepper_openloop/src/drivers/StepperDriver4PWM.h b/minimal_project_examples/esp32_stepper_openloop/src/drivers/StepperDriver4PWM.h new file mode 100644 index 00000000..e4b2ee42 --- /dev/null +++ b/minimal_project_examples/esp32_stepper_openloop/src/drivers/StepperDriver4PWM.h @@ -0,0 +1,55 @@ +#ifndef STEPPER_DRIVER_4PWM_h +#define STEPPER_DRIVER_4PWM_h + +#include "../common/base_classes/StepperDriver.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "hardware_api.h" + +/** + 4 pwm stepper driver class +*/ +class StepperDriver4PWM: public StepperDriver +{ + public: + /** + StepperMotor class constructor + @param ph1A 1A phase pwm pin + @param ph1B 1B phase pwm pin + @param ph2A 2A phase pwm pin + @param ph2B 2B phase pwm pin + @param en1 enable pin phase 1 (optional input) + @param en2 enable pin phase 2 (optional input) + */ + StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B, int en1 = NOT_SET, int en2 = NOT_SET); + + /** Motor hardware init function */ + int init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + // hardware variables + int pwm1A; //!< phase 1A pwm pin number + int pwm1B; //!< phase 1B pwm pin number + int pwm2A; //!< phase 2A pwm pin number + int pwm2B; //!< phase 2B pwm pin number + int enable_pin1; //!< enable pin number phase 1 + int enable_pin2; //!< enable pin number phase 2 + + /** + * Set phase voltages to the harware + * + * @param Ua phase A voltage + * @param Ub phase B voltage + */ + void setPwm(float Ua, float Ub) override; + + private: + +}; + + +#endif diff --git a/minimal_project_examples/esp32_stepper_openloop/src/drivers/hardware_api.h b/minimal_project_examples/esp32_stepper_openloop/src/drivers/hardware_api.h new file mode 100644 index 00000000..01c03867 --- /dev/null +++ b/minimal_project_examples/esp32_stepper_openloop/src/drivers/hardware_api.h @@ -0,0 +1,123 @@ +#ifndef HARDWARE_UTILS_H +#define HARDWARE_UTILS_H + +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA bldc driver + * @param pinB pinB bldc driver + */ +void _configure2PWM(long pwm_frequency, const int pinA, const int pinB); + +/** + * Configuring PWM frequency, resolution and alignment + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA bldc driver + * @param pinB pinB bldc driver + * @param pinC pinC bldc driver + */ +void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC); + +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 4PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pin1A pin1A stepper driver + * @param pin1B pin1B stepper driver + * @param pin2A pin2A stepper driver + * @param pin2B pin2B stepper driver + */ +void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B); + +/** + * Configuring PWM frequency, resolution and alignment + * - BLDC driver - 6PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low - if applicable + * @param pinA_h pinA high-side bldc driver + * @param pinA_l pinA low-side bldc driver + * @param pinB_h pinA high-side bldc driver + * @param pinB_l pinA low-side bldc driver + * @param pinC_h pinA high-side bldc driver + * @param pinC_l pinA low-side bldc driver + * + * @return 0 if config good, -1 if failed + */ +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param pinA phase A hardware pin number + * @param pinB phase B hardware pin number + */ +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param pinA phase A hardware pin number + * @param pinB phase B hardware pin number + * @param pinC phase C hardware pin number + */ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 4PWM setting + * - hardware specific + * + * @param dc_1a duty cycle phase 1A [0, 1] + * @param dc_1b duty cycle phase 1B [0, 1] + * @param dc_2a duty cycle phase 2A [0, 1] + * @param dc_2b duty cycle phase 2B [0, 1] + * @param pin1A phase 1A hardware pin number + * @param pin1B phase 1B hardware pin number + * @param pin2A phase 2A hardware pin number + * @param pin2B phase 2B hardware pin number + */ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B); + + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 6PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low + * @param pinA_h phase A high-side hardware pin number + * @param pinA_l phase A low-side hardware pin number + * @param pinB_h phase B high-side hardware pin number + * @param pinB_l phase B low-side hardware pin number + * @param pinC_h phase C high-side hardware pin number + * @param pinC_l phase C low-side hardware pin number + * + */ +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l); + +#endif \ No newline at end of file diff --git a/minimal_project_examples/esp32_stepper_openloop/src/drivers/hardware_specific/esp32_mcu.cpp b/minimal_project_examples/esp32_stepper_openloop/src/drivers/hardware_specific/esp32_mcu.cpp new file mode 100644 index 00000000..983d4d54 --- /dev/null +++ b/minimal_project_examples/esp32_stepper_openloop/src/drivers/hardware_specific/esp32_mcu.cpp @@ -0,0 +1,431 @@ +#include "../hardware_api.h" + +#if defined(ESP_H) + +#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 160e6 + +// preferred pwm resolution default +#define _PWM_RES_DEF 2048 +// min resolution +#define _PWM_RES_MIN 1500 +// max resolution +#define _PWM_RES_MAX 3000 + +// structure containing motor slot configuration +// this library supports up to 4 motors +typedef struct { + int pinA; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator; + mcpwm_io_signals_t mcpwm_a; + mcpwm_io_signals_t mcpwm_b; + mcpwm_io_signals_t mcpwm_c; +} bldc_3pwm_motor_slots_t; +typedef struct { + int pin1A; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator1; + mcpwm_operator_t mcpwm_operator2; + mcpwm_io_signals_t mcpwm_1a; + mcpwm_io_signals_t mcpwm_1b; + mcpwm_io_signals_t mcpwm_2a; + mcpwm_io_signals_t mcpwm_2b; +} stepper_4pwm_motor_slots_t; +typedef struct { + int pin1pwm; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator; + mcpwm_io_signals_t mcpwm_a; + mcpwm_io_signals_t mcpwm_b; +} stepper_2pwm_motor_slots_t; + +typedef struct { + int pinAH; + mcpwm_dev_t* mcpwm_num; + mcpwm_unit_t mcpwm_unit; + mcpwm_operator_t mcpwm_operator1; + mcpwm_operator_t mcpwm_operator2; + mcpwm_io_signals_t mcpwm_ah; + mcpwm_io_signals_t mcpwm_bh; + mcpwm_io_signals_t mcpwm_ch; + mcpwm_io_signals_t mcpwm_al; + mcpwm_io_signals_t mcpwm_bl; + mcpwm_io_signals_t mcpwm_cl; +} bldc_6pwm_motor_slots_t; + +// define bldc motor slots array +bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}, // 2nd motor will be MCPWM0 channel B + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 3rd motor will be MCPWM1 channel A + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B} // 4th motor will be MCPWM1 channel B + }; + +// define stepper motor slots array +bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 + }; + +// define 4pwm stepper motor slots array +stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[2] = { + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0 + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1 + }; + +// define 2pwm stepper motor slots array +stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = { + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 1st motor will be MCPWM0 channel A + {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B}, // 2nd motor will be MCPWM0 channel B + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 3rd motor will be MCPWM1 channel A + {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B} // 4th motor will be MCPWM1 channel B + }; + +// configuring high frequency pwm timer +// a lot of help from this post from Paul Gauld +// https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller +void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm_unit_t mcpwm_unit, float dead_zone = NOT_SET){ + + mcpwm_config_t pwm_config; + pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave) + pwm_config.duty_mode = MCPWM_DUTY_MODE_0; // Active HIGH + mcpwm_init(mcpwm_unit, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings + mcpwm_init(mcpwm_unit, MCPWM_TIMER_1, &pwm_config); //Configure PWM0A & PWM0B with above settings + mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &pwm_config); //Configure PWM0A & PWM0B with above settings + + if (dead_zone != NOT_SET){ + // dead zone is configured + float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + } + _delay(100); + + mcpwm_stop(mcpwm_unit, MCPWM_TIMER_0); + mcpwm_stop(mcpwm_unit, MCPWM_TIMER_1); + mcpwm_stop(mcpwm_unit, MCPWM_TIMER_2); + + // manual configuration due to the lack of config flexibility in mcpwm_init() + mcpwm_num->clk_cfg.prescale = 0; + // calculate prescaler and period + // step 1: calculate the prescaler using the default pwm resolution + // prescaler = bus_freq / (pwm_freq * default_pwm_res) - 1 + int prescaler = ceil((double)_MCPWM_FREQ / (double)_PWM_RES_DEF / 2.0 / (double)pwm_frequency) - 1; + // constrain prescaler + prescaler = _constrain(prescaler, 0, 128); + // now calculate the real resolution timer period necessary (pwm resolution) + // pwm_res = bus_freq / (pwm_freq * (prescaler + 1)) + int resolution_corrected = (double)_MCPWM_FREQ / 2.0 / (double)pwm_frequency / (double)(prescaler + 1); + // if pwm resolution too low lower the prescaler + if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 ) + resolution_corrected = (double)_MCPWM_FREQ / 2.0 / (double)pwm_frequency / (double)(--prescaler + 1); + resolution_corrected = _constrain(resolution_corrected, _PWM_RES_MIN, _PWM_RES_MAX); + + // set prescaler + mcpwm_num->timer[0].period.prescale = prescaler; + mcpwm_num->timer[1].period.prescale = prescaler; + mcpwm_num->timer[2].period.prescale = prescaler; + _delay(1); + //set period + mcpwm_num->timer[0].period.period = resolution_corrected; + mcpwm_num->timer[1].period.period = resolution_corrected; + mcpwm_num->timer[2].period.period = resolution_corrected; + _delay(1); + mcpwm_num->timer[0].period.upmethod = 0; + mcpwm_num->timer[1].period.upmethod = 0; + mcpwm_num->timer[2].period.upmethod = 0; + _delay(1); + //restart the timers + mcpwm_start(mcpwm_unit, MCPWM_TIMER_0); + mcpwm_start(mcpwm_unit, MCPWM_TIMER_1); + mcpwm_start(mcpwm_unit, MCPWM_TIMER_2); + _delay(1); + + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SELECT_SYNC_INT0, 0); + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_SELECT_SYNC_INT0, 0); + mcpwm_sync_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_SELECT_SYNC_INT0, 0); + _delay(1); + mcpwm_num->timer[0].sync.out_sel = 1; + _delay(1); + mcpwm_num->timer[0].sync.out_sel = 0; +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +// supports Arudino/ATmega328, STM32 and ESP32 +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency + else pwm_frequency = _constrain(pwm_frequency, 0, 40000); // constrain to 40kHz max - centered pwm has twice lower frequency + + stepper_2pwm_motor_slots_t m_slot = {}; + + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 4; slot_num++){ + if(esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = pinA; + m_slot = esp32_stepper_2pwm_motor_slots[slot_num]; + break; + } + } + + // disable all the slots with the same MCPWM + // disable 3pwm bldc motor which would go in the same slot + esp32_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT; + if( slot_num < 2 ){ + // slot 0 of the 4pwm stepper + esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + }else{ + // slot 1 of the stepper + esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + } + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + +} + + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// supports Arudino/ATmega328, STM32 and ESP32 +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency + else pwm_frequency = _constrain(pwm_frequency, 0, 40000); // constrain to 40kHz max - centered pwm has twice lower frequency + + bldc_3pwm_motor_slots_t m_slot = {}; + + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 4; slot_num++){ + if(esp32_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_bldc_3pwm_motor_slots[slot_num].pinA = pinA; + m_slot = esp32_bldc_3pwm_motor_slots[slot_num]; + break; + } + } + // disable all the slots with the same MCPWM + // disable 2pwm steppr motor which would go in the same slot + esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = _TAKEN_SLOT; + if( slot_num < 2 ){ + // slot 0 of the 4pwm stepper + esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + }else{ + // slot 1 of the stepper + esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + } + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_c, pinC); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); + +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency + else pwm_frequency = _constrain(pwm_frequency, 0, 40000); // constrain to 50kHz max - centered pwm has twice lower frequency + stepper_4pwm_motor_slots_t m_slot = {}; + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 2; slot_num++){ + if(esp32_stepper_4pwm_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_stepper_4pwm_motor_slots[slot_num].pin1A = pinA; + m_slot = esp32_stepper_4pwm_motor_slots[slot_num]; + break; + } + } + // disable all the slots with the same MCPWM + if( slot_num == 0 ){ + // slots 0 and 1 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; + // slots 0 and 1 of the 2pwm stepper taken + esp32_stepper_2pwm_motor_slots[0].pin1pwm = _TAKEN_SLOT; + esp32_stepper_2pwm_motor_slots[1].pin1pwm = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT; + }else{ + // slots 2 and 3 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; + // slots 2 and 3 of the 2pwm stepper taken + esp32_stepper_2pwm_motor_slots[2].pin1pwm = _TAKEN_SLOT; + esp32_stepper_2pwm_motor_slots[3].pin1pwm = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; + } + + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1a, pinA); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1b, pinB); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2a, pinC); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2b, pinD); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +// ESP32 uses MCPWM +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // determine which motor slot is the motor connected to + for(int i = 0; i < 4; i++){ + if(esp32_stepper_2pwm_motor_slots[i].pin1pwm == pinA){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100] + mcpwm_set_duty(esp32_stepper_2pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_2pwm_motor_slots[i].mcpwm_operator, dc_a*100.0); + mcpwm_set_duty(esp32_stepper_2pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_2pwm_motor_slots[i].mcpwm_operator, dc_b*100.0); + break; + } + } +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +// - hardware speciffic +// ESP32 uses MCPWM +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // determine which motor slot is the motor connected to + for(int i = 0; i < 4; i++){ + if(esp32_bldc_3pwm_motor_slots[i].pinA == pinA){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100] + mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_a*100.0); + mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_b*100.0); + mcpwm_set_duty(esp32_bldc_3pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, esp32_bldc_3pwm_motor_slots[i].mcpwm_operator, dc_c*100.0); + break; + } + } +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +// - hardware speciffic +// ESP32 uses MCPWM +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // determine which motor slot is the motor connected to + for(int i = 0; i < 2; i++){ + if(esp32_stepper_4pwm_motor_slots[i].pin1A == pin1A){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100] + mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator1, dc_1a*100.0); + mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator1, dc_1b*100.0); + mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator2, dc_2a*100.0); + mcpwm_set_duty(esp32_stepper_4pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, esp32_stepper_4pwm_motor_slots[i].mcpwm_operator2, dc_2b*100.0); + break; + } + } +} + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + + if(!pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency + else pwm_frequency = _constrain(pwm_frequency, 0, 40000); // constrain to 40kHz max - centered pwm has twice lower frequency + bldc_6pwm_motor_slots_t m_slot = {}; + // determine which motor are we connecting + // and set the appropriate configuration parameters + int slot_num; + for(slot_num = 0; slot_num < 2; slot_num++){ + if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot + esp32_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h; + m_slot = esp32_bldc_6pwm_motor_slots[slot_num]; + break; + } + } + // if no slots available + if(slot_num >= 2) return -1; + + // disable all the slots with the same MCPWM + if( slot_num == 0 ){ + // slots 0 and 1 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT; + // slot 0 of the 6pwm bldc + esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT; + }else{ + // slots 2 and 3 of the 3pwm bldc + esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT; + esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; + // slot 1 of the 6pwm bldc + esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; + } + + // configure pins + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_al, pinA_l); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bh, pinB_h); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bl, pinB_l); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ch, pinC_h); + mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_cl, pinC_l); + + // configure the timer + _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit, dead_zone); + // return + return 0; +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + // determine which motor slot is the motor connected to + for(int i = 0; i < 2; i++){ + if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found + // se the PWM on the slot timers + // transform duty cycle from [0,1] to [0,100.0] + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0); + mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0); + break; + } + } +} +#endif \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_encoder/src/BLDCMotor.cpp b/minimal_project_examples/stm32_bldc_encoder/src/BLDCMotor.cpp new file mode 100644 index 00000000..5d36ce35 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_encoder/src/BLDCMotor.cpp @@ -0,0 +1,421 @@ +#include "BLDCMotor.h" + +// // BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en) +// // - phA, phB, phC - motor A,B,C phase pwm pins +// // - pp - pole pair number +// // - cpr - counts per rotation number (cpm=ppm*4) +// // - enable pin - (optional input) +// BLDCMotor::BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en){ +// noop; +// } + +// BLDCMotor( int pp) +// - phA, phB, phC - motor A,B,C phase pwm pins +// - pp - pole pair number +// - cpr - counts per rotation number (cpm=ppm*4) +// - enable pin - (optional input) +BLDCMotor::BLDCMotor(int pp) +: FOCMotor() +{ + // save pole pairs number + pole_pairs = pp; +} + + +/** + Link the driver which controls the motor +*/ +void BLDCMotor::linkDriver(BLDCDriver* _driver) { + driver = _driver; +} + +// init hardware pins +void BLDCMotor::init() { + if(monitor_port) monitor_port->println("MOT: Initialise variables."); + // sanity check for the voltage limit configuration + if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; + // constrain voltage for sensor alignment + if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; + // update the controller limits + PID_velocity.limit = voltage_limit; + P_angle.limit = velocity_limit; + + _delay(500); + // enable motor + if(monitor_port) monitor_port->println("MOT: Enable driver."); + enable(); + _delay(500); +} + + +// disable motor driver +void BLDCMotor::disable() +{ + // set zero to PWM + driver->setPwm(0, 0, 0); + // disable the driver + driver->disable(); +} +// enable motor driver +void BLDCMotor::enable() +{ + // enable the driver + driver->enable(); + // set zero to PWM + driver->setPwm(0, 0, 0); + +} + +/** + FOC functions +*/ +// FOC initialization function +int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction ) { + int exit_flag = 1; + // align motor if necessary + // alignment necessary for encoders! + if(zero_electric_offset != NOT_SET){ + // abosolute zero offset provided - no need to align + zero_electric_angle = zero_electric_offset; + // set the sensor direction - default CW + sensor->natural_direction = sensor_direction; + }else{ + // sensor and motor alignment + _delay(500); + exit_flag = alignSensor(); + _delay(500); + } + if(monitor_port) monitor_port->println("MOT: Motor ready."); + + return exit_flag; +} +// Encoder alignment to electrical 0 angle +int BLDCMotor::alignSensor() { + if(monitor_port) monitor_port->println("MOT: Align sensor."); + // align the electrical phases of the motor and sensor + // set angle -90 degrees + + float start_angle = shaftAngle(); + for (int i = 0; i <=5; i++ ) { + float angle = _3PI_2 + _2PI * i / 6.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(200); + } + float mid_angle = shaftAngle(); + for (int i = 5; i >=0; i-- ) { + float angle = _3PI_2 + _2PI * i / 6.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(200); + } + if (mid_angle < start_angle) { + if(monitor_port) monitor_port->println("MOT: natural_direction==CCW"); + sensor->natural_direction = Direction::CCW; + } else if (mid_angle == start_angle) { + if(monitor_port) monitor_port->println("MOT: Sensor failed to notice movement"); + } else{ + if(monitor_port) monitor_port->println("MOT: natural_direction==CW"); + } + + // let the motor stabilize for 2 sec + _delay(2000); + // set sensor to zero + sensor->initRelativeZero(); + _delay(500); + setPhaseVoltage(0, 0, 0); + _delay(200); + + // find the index if available + int exit_flag = absoluteZeroAlign(); + _delay(500); + if(monitor_port){ + if(exit_flag< 0 ) monitor_port->println("MOT: Error: Not found!"); + if(exit_flag> 0 ) monitor_port->println("MOT: Success!"); + else monitor_port->println("MOT: Not available!"); + } + return exit_flag; +} + + +// Encoder alignment the absolute zero angle +// - to the index +int BLDCMotor::absoluteZeroAlign() { + + if(monitor_port) monitor_port->println("MOT: Absolute zero align."); + // if no absolute zero return + if(!sensor->hasAbsoluteZero()) return 0; + + + if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println("MOT: Searching..."); + // search the absolute zero with small velocity + while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){ + loopFOC(); + voltage_q = PID_velocity(velocity_index_search - shaftVelocity()); + } + voltage_q = 0; + // disable motor + setPhaseVoltage(0, 0, 0); + + // align absolute zero if it has been found + if(!sensor->needsAbsoluteZeroSearch()){ + // align the sensor with the absolute zero + float zero_offset = sensor->initAbsoluteZero(); + // remember zero electric angle + zero_electric_angle = _normalizeAngle(_electricalAngle(zero_offset, pole_pairs)); + } + // return bool if zero found + return !sensor->needsAbsoluteZeroSearch() ? 1 : -1; +} + +// Iterative function looping FOC algorithm, setting Uq on the Motor +// The faster it can be run the better +void BLDCMotor::loopFOC() { + // shaft angle + shaft_angle = shaftAngle(); + // set the phase voltage - FOC heart function :) + setPhaseVoltage(voltage_q, voltage_d, _electricalAngle(shaft_angle,pole_pairs)); +} + +// Iterative function running outer loop of the FOC algorithm +// Behavior of this function is determined by the motor.controller variable +// It runs either angle, velocity or voltage loop +// - needs to be called iteratively it is asynchronous function +// - if target is not set it uses motor.target value +void BLDCMotor::move(float new_target) { + // set internal target variable + if( new_target != NOT_SET ) target = new_target; + // get angular velocity + shaft_velocity = shaftVelocity(); + // choose control loop + switch (controller) { + case ControlType::voltage: + voltage_q = target; + break; + case ControlType::angle: + // angle set point + // include angle loop + shaft_angle_sp = target; + shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); + voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + break; + case ControlType::velocity: + // velocity set point + // include velocity loop + shaft_velocity_sp = target; + voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + break; + case ControlType::velocity_openloop: + // velocity control in open loop + // loopFOC should not be called + shaft_velocity_sp = target; + velocityOpenloop(shaft_velocity_sp); + break; + case ControlType::angle_openloop: + // angle control in open loop + // loopFOC should not be called + shaft_angle_sp = target; + angleOpenloop(shaft_angle_sp); + break; + } +} + + +// Method using FOC to set Uq and Ud to the motor at the optimal angle +// Function implementing Space Vector PWM and Sine PWM algorithms +// +// Function using sine approximation +// regular sin + cos ~300us (no memory usaage) +// approx _sin + _cos ~110us (400Byte ~ 20% of memory) +void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { + + const bool centered = true; + int sector; + float _ca,_sa; + + switch (foc_modulation) + { + case FOCModulationType::Trapezoid_120 : + // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5 + static int trap_120_map[6][3] = { + {0,1,-1},{-1,1,0},{-1,0,1},{0,-1,1},{1,-1,0},{1,0,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + }; + // static int trap_120_state = 0; + sector = 6 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + + Ua = Uq + trap_120_map[sector][0] * Uq; + Ub = Uq + trap_120_map[sector][1] * Uq; + Uc = Uq + trap_120_map[sector][2] * Uq; + + if (centered) { + Ua += (driver->voltage_limit)/2 -Uq; + Ub += (driver->voltage_limit)/2 -Uq; + Uc += (driver->voltage_limit)/2 -Uq; + } + break; + + case FOCModulationType::Trapezoid_150 : + // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8 + static int trap_150_map[12][3] = { + {0,1,-1},{-1,1,-1},{-1,1,0},{-1,1,1},{-1,0,1},{-1,-1,1},{0,-1,1},{1,-1,1},{1,-1,0},{1,-1,-1},{1,0,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + }; + // static int trap_150_state = 0; + sector = 12 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + + Ua = Uq + trap_150_map[sector][0] * Uq; + Ub = Uq + trap_150_map[sector][1] * Uq; + Uc = Uq + trap_150_map[sector][2] * Uq; + + //center + if (centered) { + Ua += (driver->voltage_limit)/2 -Uq; + Ub += (driver->voltage_limit)/2 -Uq; + Uc += (driver->voltage_limit)/2 -Uq; + } + + break; + + case FOCModulationType::SinePWM : + // Sinusoidal PWM modulation + // Inverse Park + Clarke transformation + + // angle normalization in between 0 and 2pi + // only necessary if using _sin and _cos - approximation functions + angle_el = _normalizeAngle(angle_el + zero_electric_angle); + _ca = _cos(angle_el); + _sa = _sin(angle_el); + // Inverse park transform + Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; + Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; + + // Clarke transform + Ua = Ualpha + driver->voltage_limit/2; + Ub = -0.5 * Ualpha + _SQRT3_2 * Ubeta + driver->voltage_limit/2; + Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + driver->voltage_limit/2; + + if (!centered) { + float Umin = min(Ua, min(Ub, Uc)); + Ua -= Umin; + Ub -= Umin; + Uc -= Umin; + } + + break; + + case FOCModulationType::SpaceVectorPWM : + // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm + // https://www.youtube.com/watch?v=QMSWUMEAejg + + // if negative voltages change inverse the phase + // angle + 180degrees + if(Uq < 0) angle_el += _PI; + Uq = abs(Uq); + + // angle normalisation in between 0 and 2pi + // only necessary if using _sin and _cos - approximation functions + angle_el = _normalizeAngle(angle_el + zero_electric_angle + _PI_2); + + // find the sector we are in currently + sector = floor(angle_el / _PI_3) + 1; + // calculate the duty cycles + float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_limit; + float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_limit; + // two versions possible + float T0 = 0; // pulled to 0 - better for low power supply voltage + if (centered) { + T0 = 1 - T1 - T2; //centered around driver->voltage_limit/2 + } + + // calculate the duty cycles(times) + float Ta,Tb,Tc; + switch(sector){ + case 1: + Ta = T1 + T2 + T0/2; + Tb = T2 + T0/2; + Tc = T0/2; + break; + case 2: + Ta = T1 + T0/2; + Tb = T1 + T2 + T0/2; + Tc = T0/2; + break; + case 3: + Ta = T0/2; + Tb = T1 + T2 + T0/2; + Tc = T2 + T0/2; + break; + case 4: + Ta = T0/2; + Tb = T1+ T0/2; + Tc = T1 + T2 + T0/2; + break; + case 5: + Ta = T2 + T0/2; + Tb = T0/2; + Tc = T1 + T2 + T0/2; + break; + case 6: + Ta = T1 + T2 + T0/2; + Tb = T0/2; + Tc = T1 + T0/2; + break; + default: + // possible error state + Ta = 0; + Tb = 0; + Tc = 0; + } + + // calculate the phase voltages and center + Ua = Ta*driver->voltage_limit; + Ub = Tb*driver->voltage_limit; + Uc = Tc*driver->voltage_limit; + break; + + } + + // set the voltages in driver + driver->setPwm(Ua, Ub, Uc); +} + + + +// Function (iterative) generating open loop movement for target velocity +// - target_velocity - rad/s +// it uses voltage_limit variable +void BLDCMotor::velocityOpenloop(float target_velocity){ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6; + + // calculate the necessary angle to achieve target velocity + shaft_angle += target_velocity*Ts; + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; +} + +// Function (iterative) generating open loop movement towards the target angle +// - target_angle - rad +// it uses voltage_limit and velocity_limit variables +void BLDCMotor::angleOpenloop(float target_angle){ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6; + + // calculate the necessary angle to move from current position towards target angle + // with maximal velocity (velocity_limit) + if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)) + shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; + else + shaft_angle = target_angle; + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; +} diff --git a/minimal_project_examples/stm32_bldc_encoder/src/BLDCMotor.h b/minimal_project_examples/stm32_bldc_encoder/src/BLDCMotor.h new file mode 100644 index 00000000..164cc108 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_encoder/src/BLDCMotor.h @@ -0,0 +1,109 @@ +#ifndef BLDCMotor_h +#define BLDCMotor_h + +#include "Arduino.h" +#include "common/base_classes/FOCMotor.h" +#include "common/base_classes/Sensor.h" +#include "common/base_classes/BLDCDriver.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" +#include "common/defaults.h" + +/** + BLDC motor class +*/ +class BLDCMotor: public FOCMotor +{ + public: + /** + BLDCMotor class constructor + @param pp pole pairs number + */ + BLDCMotor(int pp); + + /** + * Function linking a motor and a foc driver + * + * @param driver BLDCDriver class implementing all the hardware specific functions necessary PWM setting + */ + void linkDriver(BLDCDriver* driver); + + /** + * BLDCDriver link: + * - 3PWM + * - 6PWM + */ + BLDCDriver* driver; + + /** Motor hardware init function */ + void init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + */ + int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override; + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + void loopFOC() override; + + /** + * Function executing the control loops set by the controller parameter of the BLDCMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + void move(float target = NOT_SET) override; + + float Ua,Ub,Uc;//!< Current phase voltages Ua,Ub and Uc set to motor + float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform + + + private: + // FOC methods + /** + * Method using FOC to set Uq to the motor at the optimal angle + * Heart of the FOC algorithm + * + * @param Uq Current voltage in q axis to set to the motor + * @param Ud Current voltage in d axis to set to the motor + * @param angle_el current electrical angle of the motor + */ + void setPhaseVoltage(float Uq, float Ud, float angle_el); + /** Sensor alignment to electrical 0 angle of the motor */ + int alignSensor(); + /** Motor and sensor alignment to the sensors absolute 0 angle */ + int absoluteZeroAlign(); + + + // Open loop motion control + /** + * Function (iterative) generating open loop movement for target velocity + * it uses voltage_limit variable + * + * @param target_velocity - rad/s + */ + void velocityOpenloop(float target_velocity); + /** + * Function (iterative) generating open loop movement towards the target angle + * it uses voltage_limit and velocity_limit variables + * + * @param target_angle - rad + */ + void angleOpenloop(float target_angle); + // open loop variables + long open_loop_timestamp; +}; + + +#endif diff --git a/minimal_project_examples/stm32_bldc_encoder/src/common/base_classes/BLDCDriver.h b/minimal_project_examples/stm32_bldc_encoder/src/common/base_classes/BLDCDriver.h new file mode 100644 index 00000000..708323fb --- /dev/null +++ b/minimal_project_examples/stm32_bldc_encoder/src/common/base_classes/BLDCDriver.h @@ -0,0 +1,28 @@ +#ifndef BLDCDRIVER_H +#define BLDCDRIVER_H + +class BLDCDriver{ + public: + + /** Initialise hardware */ + virtual int init(); + /** Enable hardware */ + virtual void enable(); + /** Disable hardware */ + virtual void disable(); + + long pwm_frequency; //!< pwm frequency value in hertz + float voltage_power_supply; //!< power supply voltage + float voltage_limit; //!< limiting voltage set to the motor + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + virtual void setPwm(float Ua, float Ub, float Uc); +}; + +#endif \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_encoder/src/common/base_classes/FOCMotor.cpp b/minimal_project_examples/stm32_bldc_encoder/src/common/base_classes/FOCMotor.cpp new file mode 100644 index 00000000..1d1a0f43 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_encoder/src/common/base_classes/FOCMotor.cpp @@ -0,0 +1,241 @@ +#include "FOCMotor.h" + +/** + * Default constructor - setting all variabels to default values + */ +FOCMotor::FOCMotor() +{ + // maximum angular velocity to be used for positioning + velocity_limit = DEF_VEL_LIM; + // maximum voltage to be set to the motor + voltage_limit = DEF_POWER_SUPPLY; + + // index search velocity + velocity_index_search = DEF_INDEX_SEARCH_TARGET_VELOCITY; + // sensor and motor align voltage + voltage_sensor_align = DEF_VOLTAGE_SENSOR_ALIGN; + + // electric angle of comthe zero angle + zero_electric_angle = 0; + + // default modulation is SinePWM + foc_modulation = FOCModulationType::SinePWM; + + // default target value + target = 0; + voltage_d = 0; + voltage_q = 0; + + //monitor_port + monitor_port = nullptr; + //sensor + sensor = nullptr; +} + + +/** + Sensor communication methods +*/ +void FOCMotor::linkSensor(Sensor* _sensor) { + sensor = _sensor; +} +// shaft angle calculation +float FOCMotor::shaftAngle() { + // if no sensor linked return 0 + if(!sensor) return shaft_angle; + return sensor->getAngle(); +} +// shaft velocity calculation +float FOCMotor::shaftVelocity() { + // if no sensor linked return 0 + if(!sensor) return 0; + return LPF_velocity(sensor->getVelocity()); +} + +/** + * Monitoring functions + */ +// function implementing the monitor_port setter +void FOCMotor::useMonitoring(Print &print){ + monitor_port = &print; //operate on the address of print + if(monitor_port ) monitor_port->println("MOT: Monitor enabled!"); +} +// utility function intended to be used with serial plotter to monitor motor variables +// significantly slowing the execution down!!!! +void FOCMotor::monitor() { + if(!monitor_port) return; + switch (controller) { + case ControlType::velocity_openloop: + case ControlType::velocity: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_velocity_sp); + monitor_port->print("\t"); + monitor_port->println(shaft_velocity); + break; + case ControlType::angle_openloop: + case ControlType::angle: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_angle_sp); + monitor_port->print("\t"); + monitor_port->println(shaft_angle); + break; + case ControlType::voltage: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_angle); + monitor_port->print("\t"); + monitor_port->println(shaft_velocity); + break; + } +} + +int FOCMotor::command(String user_command) { + // error flag + int errorFlag = 1; + // if empty string + if(user_command.length() < 1) return errorFlag; + + // parse command letter + char cmd = user_command.charAt(0); + // check if get command + char GET = user_command.charAt(1) == '\n'; + // parse command values + float value = user_command.substring(1).toFloat(); + + // a bit of optimisation of variable memory for Arduino UNO (atmega328) + switch(cmd){ + case 'P': // velocity P gain change + case 'I': // velocity I gain change + case 'D': // velocity D gain change + case 'R': // velocity voltage ramp change + if(monitor_port) monitor_port->print(" PID velocity| "); + break; + case 'F': // velocity Tf low pass filter change + if(monitor_port) monitor_port->print(" LPF velocity| "); + break; + case 'K': // angle loop gain P change + if(monitor_port) monitor_port->print(" P angle| "); + break; + case 'L': // velocity voltage limit change + case 'N': // angle loop gain velocity_limit change + if(monitor_port) monitor_port->print(" Limits| "); + break; + } + + // apply the the command + switch(cmd){ + case 'P': // velocity P gain change + if(monitor_port) monitor_port->print("P: "); + if(!GET) PID_velocity.P = value; + if(monitor_port) monitor_port->println(PID_velocity.P); + break; + case 'I': // velocity I gain change + if(monitor_port) monitor_port->print("I: "); + if(!GET) PID_velocity.I = value; + if(monitor_port) monitor_port->println(PID_velocity.I); + break; + case 'D': // velocity D gain change + if(monitor_port) monitor_port->print("D: "); + if(!GET) PID_velocity.D = value; + if(monitor_port) monitor_port->println(PID_velocity.D); + break; + case 'R': // velocity voltage ramp change + if(monitor_port) monitor_port->print("volt_ramp: "); + if(!GET) PID_velocity.output_ramp = value; + if(monitor_port) monitor_port->println(PID_velocity.output_ramp); + break; + case 'L': // velocity voltage limit change + if(monitor_port) monitor_port->print("volt_limit: "); + if(!GET) { + voltage_limit = value; + PID_velocity.limit = value; + } + if(monitor_port) monitor_port->println(voltage_limit); + break; + case 'F': // velocity Tf low pass filter change + if(monitor_port) monitor_port->print("Tf: "); + if(!GET) LPF_velocity.Tf = value; + if(monitor_port) monitor_port->println(LPF_velocity.Tf); + break; + case 'K': // angle loop gain P change + if(monitor_port) monitor_port->print(" P: "); + if(!GET) P_angle.P = value; + if(monitor_port) monitor_port->println(P_angle.P); + break; + case 'N': // angle loop gain velocity_limit change + if(monitor_port) monitor_port->print("vel_limit: "); + if(!GET){ + velocity_limit = value; + P_angle.limit = value; + } + if(monitor_port) monitor_port->println(velocity_limit); + break; + case 'C': + // change control type + if(monitor_port) monitor_port->print("Control: "); + + if(GET){ // if get command + switch(controller){ + case ControlType::voltage: + if(monitor_port) monitor_port->println("voltage"); + break; + case ControlType::velocity: + if(monitor_port) monitor_port->println("velocity"); + break; + case ControlType::angle: + if(monitor_port) monitor_port->println("angle"); + break; + default: + if(monitor_port) monitor_port->println("open loop"); + } + }else{ // if set command + switch((int)value){ + case 0: + if(monitor_port) monitor_port->println("voltage"); + controller = ControlType::voltage; + break; + case 1: + if(monitor_port) monitor_port->println("velocity"); + controller = ControlType::velocity; + break; + case 2: + if(monitor_port) monitor_port->println("angle"); + controller = ControlType::angle; + break; + default: // not valid command + errorFlag = 0; + } + } + break; + case 'V': // get current values of the state variables + switch((int)value){ + case 0: // get voltage + if(monitor_port) monitor_port->print("Uq: "); + if(monitor_port) monitor_port->println(voltage_q); + break; + case 1: // get velocity + if(monitor_port) monitor_port->print("Velocity: "); + if(monitor_port) monitor_port->println(shaft_velocity); + break; + case 2: // get angle + if(monitor_port) monitor_port->print("Angle: "); + if(monitor_port) monitor_port->println(shaft_angle); + break; + case 3: // get angle + if(monitor_port) monitor_port->print("Target: "); + if(monitor_port) monitor_port->println(target); + break; + default: // not valid command + errorFlag = 0; + } + break; + default: // target change + if(monitor_port) monitor_port->print("Target : "); + target = user_command.toFloat(); + if(monitor_port) monitor_port->println(target); + } + // return 0 if error and 1 if ok + return errorFlag; +} \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_encoder/src/common/base_classes/FOCMotor.h b/minimal_project_examples/stm32_bldc_encoder/src/common/base_classes/FOCMotor.h new file mode 100644 index 00000000..986e1ab2 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_encoder/src/common/base_classes/FOCMotor.h @@ -0,0 +1,197 @@ +#ifndef FOCMOTOR_H +#define FOCMOTOR_H + +#include "Arduino.h" +#include "Sensor.h" +#include "BLDCDriver.h" + +#include "../time_utils.h" +#include "../foc_utils.h" +#include "../defaults.h" +#include "../pid.h" +#include "../lowpass_filter.h" + + +/** + * Motiron control type + */ +enum ControlType{ + voltage,//!< Torque control using voltage + velocity,//!< Velocity motion control + angle,//!< Position/angle motion control + velocity_openloop, + angle_openloop +}; + +/** + * FOC modulation type + */ +enum FOCModulationType{ + SinePWM, //!< Sinusoidal PWM modulation + SpaceVectorPWM, //!< Space vector modulation method + Trapezoid_120, + Trapezoid_150 +}; + +/** + Generic motor class +*/ +class FOCMotor +{ + public: + /** + * Default constructor - setting all variabels to default values + */ + FOCMotor(); + + /** Motor hardware init function */ + virtual void init()=0; + /** Motor disable function */ + virtual void disable()=0; + /** Motor enable function */ + virtual void enable()=0; + + /** + * Function linking a motor and a sensor + * + * @param sensor Sensor class wrapper for the FOC algorihtm to read the motor angle and velocity + */ + void linkSensor(Sensor* sensor); + + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + * + * - If zero_electric_offset parameter is set the alignment procedure is skipped + * + * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. + * @param sensor_direction sensor natural direction - default is CW + * + */ + virtual int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW)=0; + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + virtual void loopFOC()=0; + /** + * Function executing the control loops set by the controller parameter of the BLDCMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + virtual void move(float target = NOT_SET)=0; + + // State calculation methods + /** Shaft angle calculation in radians [rad] */ + float shaftAngle(); + /** + * Shaft angle calculation function in radian per second [rad/s] + * It implements low pass filtering + */ + float shaftVelocity(); + + // state variables + float target; //!< current target value - depends of the controller + float shaft_angle;//!< current motor angle + float shaft_velocity;//!< current motor velocity + float shaft_velocity_sp;//!< current target velocity + float shaft_angle_sp;//!< current target angle + float voltage_q;//!< current voltage u_q set + float voltage_d;//!< current voltage u_d set + + // motor configuration parameters + float voltage_sensor_align;//!< sensor and motor align voltage parameter + float velocity_index_search;//!< target velocity for index search + int pole_pairs;//!< Motor pole pairs number + + // limiting variables + float voltage_limit; //!< Voltage limitting variable - global limit + float velocity_limit; //!< Velocity limitting variable - global limit + + float zero_electric_angle;//! _2PI ? a_sin - _2PI : a_sin; + return _sin(a_sin); +} + + +// normalizing radian angle to [0,2PI] +float _normalizeAngle(float angle){ + float a = fmod(angle, _2PI); + return a >= 0 ? a : (a + _2PI); +} + +// Electrical angle calculation +float _electricalAngle(float shaft_angle, int pole_pairs) { + return (shaft_angle * pole_pairs); +} \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_encoder/src/common/foc_utils.h b/minimal_project_examples/stm32_bldc_encoder/src/common/foc_utils.h new file mode 100644 index 00000000..7da3f7bc --- /dev/null +++ b/minimal_project_examples/stm32_bldc_encoder/src/common/foc_utils.h @@ -0,0 +1,55 @@ +#ifndef FOCUTILS_LIB_H +#define FOCUTILS_LIB_H + +#include "Arduino.h" + +// sign function +#define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) ) +#define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) +#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) + +// utility defines +#define _2_SQRT3 1.15470053838 +#define _SQRT3 1.73205080757 +#define _1_SQRT3 0.57735026919 +#define _SQRT3_2 0.86602540378 +#define _SQRT2 1.41421356237 +#define _120_D2R 2.09439510239 +#define _PI 3.14159265359 +#define _PI_2 1.57079632679 +#define _PI_3 1.0471975512 +#define _2PI 6.28318530718 +#define _3PI_2 4.71238898038 + +#define NOT_SET -12345.0 + +/** + * Function approximating the sine calculation by using fixed size array + * - execution time ~40us (Arduino UNO) + * + * @param a angle in between 0 and 2PI + */ +float _sin(float a); +/** + * Function approximating cosine calculation by using fixed size array + * - execution time ~50us (Arduino UNO) + * + * @param a angle in between 0 and 2PI + */ +float _cos(float a); + +/** + * normalizing radian angle to [0,2PI] + * @param angle - angle to be normalized + */ +float _normalizeAngle(float angle); + + +/** + * Electrical angle calculation + * + * @param shaft_angle - shaft angle of the motor + * @param pole_pairs - number of pole pairs + */ +float _electricalAngle(float shaft_angle, int pole_pairs); +#endif \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_encoder/src/common/lowpass_filter.cpp b/minimal_project_examples/stm32_bldc_encoder/src/common/lowpass_filter.cpp new file mode 100644 index 00000000..9bf4ab7a --- /dev/null +++ b/minimal_project_examples/stm32_bldc_encoder/src/common/lowpass_filter.cpp @@ -0,0 +1,25 @@ +#include "lowpass_filter.h" + +LowPassFilter::LowPassFilter(float time_constant) + : Tf(time_constant) + , y_prev(0.0f) +{ + timestamp_prev = _micros(); +} + + +float LowPassFilter::operator() (float x) +{ + unsigned long timestamp = _micros(); + float dt = (timestamp - timestamp_prev)*1e-6f; + + if (dt < 0.0f || dt > 0.5f) + dt = 1e-3f; + + float alpha = Tf/(Tf + dt); + float y = alpha*y_prev + (1.0f - alpha)*x; + + y_prev = y; + timestamp_prev = timestamp; + return y; +} \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_encoder/src/common/lowpass_filter.h b/minimal_project_examples/stm32_bldc_encoder/src/common/lowpass_filter.h new file mode 100644 index 00000000..7c51be54 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_encoder/src/common/lowpass_filter.h @@ -0,0 +1,29 @@ +#ifndef LOWPASS_FILTER_H +#define LOWPASS_FILTER_H + + +#include "time_utils.h" +#include "foc_utils.h" + + +/** + * Low pass filter class + */ +class LowPassFilter +{ +public: + /** + * @param Tf - Low pass filter time constant + */ + LowPassFilter(float Tf); + ~LowPassFilter() = default; + + float operator() (float x); + float Tf; //!< Low pass filter time constant + +protected: + unsigned long timestamp_prev; //!< Last execution timestamp + float y_prev; //!< filtered value in previous execution step +}; + +#endif // LOWPASS_FILTER_H \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_encoder/src/common/pid.cpp b/minimal_project_examples/stm32_bldc_encoder/src/common/pid.cpp new file mode 100644 index 00000000..1a0a9828 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_encoder/src/common/pid.cpp @@ -0,0 +1,56 @@ +#include "pid.h" + +PIDController::PIDController(float P, float I, float D, float ramp, float limit) + : P(P) + , I(I) + , D(D) + , output_ramp(ramp) // output derivative limit [volts/second] + , limit(limit) // output supply limit [volts] + , integral_prev(0.0) + , error_prev(0.0) + , output_prev(0.0) +{ + timestamp_prev = _micros()*1e-6; +} + +// PID controller function +float PIDController::operator() (float error){ + // calculate the time from the last call + unsigned long timestamp_now = _micros(); + float Ts = (timestamp_now - timestamp_prev) * 1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + + // u(s) = (P + I/s + Ds)e(s) + // Discrete implementations + // proportional part + // u_p = P *e(k) + float proportional = P * error; + // Tustin transform of the integral part + // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) + float integral = integral_prev + I*Ts*0.5*(error + error_prev); + // antiwindup - limit the output voltage_q + integral = _constrain(integral, -limit, limit); + // Discrete derivation + // u_dk = D(ek - ek_1)/Ts + float derivative = D*(error - error_prev)/Ts; + + // sum all the components + float output = proportional + integral + derivative; + // antiwindup - limit the output variable + output = _constrain(output, -limit, limit); + + // limit the acceleration by ramping the output + float output_rate = (output - output_prev)/Ts; + if (output_rate > output_ramp) + output = output_prev + output_ramp*Ts; + else if (output_rate < -output_ramp) + output = output_prev - output_ramp*Ts; + + // saving for the next pass + integral_prev = integral; + output_prev = output; + error_prev = error; + timestamp_prev = timestamp_now; + return output; +} \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_encoder/src/common/pid.h b/minimal_project_examples/stm32_bldc_encoder/src/common/pid.h new file mode 100644 index 00000000..0e9ddf54 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_encoder/src/common/pid.h @@ -0,0 +1,40 @@ +#ifndef PID_H +#define PID_H + + +#include "time_utils.h" +#include "foc_utils.h" + +/** + * PID controller class + */ +class PIDController +{ +public: + /** + * + * @param P - Proportional gain + * @param I - Integral gain + * @param D - Derivative gain + * @param ramp - Maximum speed of change of the output value + * @param limit - Maximum output value + */ + PIDController(float P, float I, float D, float ramp, float limit); + ~PIDController() = default; + + float operator() (float error); + + float P; //!< Proportional gain + float I; //!< Integral gain + float D; //!< Derivative gain + float output_ramp; //!< Maximum speed of change of the output value + float limit; //!< Maximum output value + +protected: + float integral_prev; //!< last integral component value + float error_prev; //!< last tracking error value + float timestamp_prev; //!< Last execution timestamp + float output_prev; //!< last pid output value +}; + +#endif // PID_H \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_encoder/src/common/time_utils.cpp b/minimal_project_examples/stm32_bldc_encoder/src/common/time_utils.cpp new file mode 100644 index 00000000..181d03cf --- /dev/null +++ b/minimal_project_examples/stm32_bldc_encoder/src/common/time_utils.cpp @@ -0,0 +1,31 @@ +#include "time_utils.h" + +// function buffering delay() +// arduino uno function doesn't work well with interrupts +void _delay(unsigned long ms){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) + // if arduino uno and other atmega328p chips + // use while instad of delay, + // due to wrong measurement based on changed timer0 + unsigned long t = _micros() + ms*1000; + while( _micros() < t ){}; +#else + // regular micros + delay(ms); +#endif +} + + +// function buffering _micros() +// arduino function doesn't work well with interrupts +unsigned long _micros(){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) +// if arduino uno and other atmega328p chips + //return the value based on the prescaler + if((TCCR0B & 0b00000111) == 0x01) return (micros()/32); + else return (micros()); +#else + // regular micros + return micros(); +#endif +} diff --git a/minimal_project_examples/stm32_bldc_encoder/src/common/time_utils.h b/minimal_project_examples/stm32_bldc_encoder/src/common/time_utils.h new file mode 100644 index 00000000..143d4853 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_encoder/src/common/time_utils.h @@ -0,0 +1,22 @@ +#ifndef TIME_UTILS_H +#define TIME_UTILS_H + +#include "foc_utils.h" + +/** + * Function implementing delay() function in milliseconds + * - blocking function + * - hardware specific + + * @param ms number of milliseconds to wait + */ +void _delay(unsigned long ms); + +/** + * Function implementing timestamp getting function in microseconds + * hardware specific + */ +unsigned long _micros(); + + +#endif \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_encoder/src/drivers/BLDCDriver6PWM.cpp b/minimal_project_examples/stm32_bldc_encoder/src/drivers/BLDCDriver6PWM.cpp new file mode 100644 index 00000000..266a5412 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_encoder/src/drivers/BLDCDriver6PWM.cpp @@ -0,0 +1,80 @@ +#include "BLDCDriver6PWM.h" + +BLDCDriver6PWM::BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h,int phC_l, int en){ + // Pin initialization + pwmA_h = phA_h; + pwmB_h = phB_h; + pwmC_h = phC_h; + pwmA_l = phA_l; + pwmB_l = phB_l; + pwmC_l = phC_l; + + // enable_pin pin + enable_pin = en; + + // default power-supply value + voltage_power_supply = DEF_POWER_SUPPLY; + voltage_limit = NOT_SET; + + // dead zone initial - 2% + dead_zone = 0.02; + +} + +// enable motor driver +void BLDCDriver6PWM::enable(){ + // enable_pin the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, HIGH); + // set zero to PWM + setPwm(0, 0, 0); +} + +// disable motor driver +void BLDCDriver6PWM::disable() +{ + // set zero to PWM + setPwm(0, 0, 0); + // disable the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, LOW); + +} + +// init hardware pins +int BLDCDriver6PWM::init() { + // a bit of separation + _delay(1000); + + // PWM pins + pinMode(pwmA_l, OUTPUT); + pinMode(pwmB_h, OUTPUT); + pinMode(pwmC_h, OUTPUT); + pinMode(pwmA_l, OUTPUT); + pinMode(pwmB_l, OUTPUT); + pinMode(pwmC_l, OUTPUT); + if(enable_pin != NOT_SET) pinMode(enable_pin, OUTPUT); + + + // sanity check for the voltage limit configuration + if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + + // configure 6pwm + // hardware specific function - depending on driver and mcu + return _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); +} + + +// Set voltage to the pwm pin +void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { + // limit the voltage in driver + Ua = _constrain(Ua, 0, voltage_limit); + Ub = _constrain(Ub, 0, voltage_limit); + Uc = _constrain(Uc, 0, voltage_limit); + // calculate duty cycle + // limited in [0,1] + float dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 ); + float dc_b = _constrain(Ub / voltage_power_supply, 0.0 , 1.0 ); + float dc_c = _constrain(Uc / voltage_power_supply, 0.0 , 1.0 ); + // hardware specific writing + // hardware specific function - depending on driver and mcu + _writeDutyCycle6PWM(dc_a, dc_b, dc_c, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); +} \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_encoder/src/drivers/BLDCDriver6PWM.h b/minimal_project_examples/stm32_bldc_encoder/src/drivers/BLDCDriver6PWM.h new file mode 100644 index 00000000..d13b69fd --- /dev/null +++ b/minimal_project_examples/stm32_bldc_encoder/src/drivers/BLDCDriver6PWM.h @@ -0,0 +1,57 @@ +#ifndef BLDCDriver6PWM_h +#define BLDCDriver6PWM_h + +#include "../common/base_classes/BLDCDriver.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "hardware_api.h" + +/** + 6 pwm bldc driver class +*/ +class BLDCDriver6PWM: public BLDCDriver +{ + public: + /** + BLDCDriver class constructor + @param phA_h A phase pwm pin + @param phA_l A phase pwm pin + @param phB_h B phase pwm pin + @param phB_l A phase pwm pin + @param phC_h C phase pwm pin + @param phC_l A phase pwm pin + @param en enable pin (optional input) + */ + BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h,int phC_l, int en = NOT_SET); + + /** Motor hardware init function */ + int init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + // hardware variables + int pwmA_h,pwmA_l; //!< phase A pwm pin number + int pwmB_h,pwmB_l; //!< phase B pwm pin number + int pwmC_h,pwmC_l; //!< phase C pwm pin number + int enable_pin; //!< enable pin number + + float dead_zone; //!< a percentage of dead-time(zone) (both high and low side in low) for each pwm cycle [0,1] + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + void setPwm(float Ua, float Ub, float Uc) override; + + private: + +}; + + +#endif diff --git a/minimal_project_examples/stm32_bldc_encoder/src/drivers/hardware_api.h b/minimal_project_examples/stm32_bldc_encoder/src/drivers/hardware_api.h new file mode 100644 index 00000000..01c03867 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_encoder/src/drivers/hardware_api.h @@ -0,0 +1,123 @@ +#ifndef HARDWARE_UTILS_H +#define HARDWARE_UTILS_H + +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA bldc driver + * @param pinB pinB bldc driver + */ +void _configure2PWM(long pwm_frequency, const int pinA, const int pinB); + +/** + * Configuring PWM frequency, resolution and alignment + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA bldc driver + * @param pinB pinB bldc driver + * @param pinC pinC bldc driver + */ +void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC); + +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 4PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pin1A pin1A stepper driver + * @param pin1B pin1B stepper driver + * @param pin2A pin2A stepper driver + * @param pin2B pin2B stepper driver + */ +void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B); + +/** + * Configuring PWM frequency, resolution and alignment + * - BLDC driver - 6PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low - if applicable + * @param pinA_h pinA high-side bldc driver + * @param pinA_l pinA low-side bldc driver + * @param pinB_h pinA high-side bldc driver + * @param pinB_l pinA low-side bldc driver + * @param pinC_h pinA high-side bldc driver + * @param pinC_l pinA low-side bldc driver + * + * @return 0 if config good, -1 if failed + */ +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param pinA phase A hardware pin number + * @param pinB phase B hardware pin number + */ +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param pinA phase A hardware pin number + * @param pinB phase B hardware pin number + * @param pinC phase C hardware pin number + */ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 4PWM setting + * - hardware specific + * + * @param dc_1a duty cycle phase 1A [0, 1] + * @param dc_1b duty cycle phase 1B [0, 1] + * @param dc_2a duty cycle phase 2A [0, 1] + * @param dc_2b duty cycle phase 2B [0, 1] + * @param pin1A phase 1A hardware pin number + * @param pin1B phase 1B hardware pin number + * @param pin2A phase 2A hardware pin number + * @param pin2B phase 2B hardware pin number + */ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B); + + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 6PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low + * @param pinA_h phase A high-side hardware pin number + * @param pinA_l phase A low-side hardware pin number + * @param pinB_h phase B high-side hardware pin number + * @param pinB_l phase B low-side hardware pin number + * @param pinC_h phase C high-side hardware pin number + * @param pinC_l phase C low-side hardware pin number + * + */ +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l); + +#endif \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_encoder/src/drivers/hardware_specific/stm32_mcu.cpp b/minimal_project_examples/stm32_bldc_encoder/src/drivers/hardware_specific/stm32_mcu.cpp new file mode 100644 index 00000000..0dc83cf7 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_encoder/src/drivers/hardware_specific/stm32_mcu.cpp @@ -0,0 +1,326 @@ + +#include "../hardware_api.h" + +#if defined(_STM32_DEF_) + +#define _PWM_RESOLUTION 12 // 12bit +#define _PWM_RANGE 4095.0// 2^12 -1 = 4095 +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 50000 // 50khz + + +#define _HARDWARE_6PWM 1 +#define _SOFTWARE_6PWM 0 +#define _ERROR_6PWM -1 + + +// setting pwm to hardware pin - instead analogWrite() +void _setPwm(int ulPin, uint32_t value, int resolution) +{ + PinName pin = digitalPinToPinName(ulPin); + TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); + uint32_t index = get_timer_index(Instance); + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + + uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + HT->setCaptureCompare(channel, value, (TimerCompareFormat_t)resolution); +} + + +// init pin pwm +HardwareTimer* _initPinPWM(uint32_t PWM_freq, int ulPin) +{ + PinName pin = digitalPinToPinName(ulPin); + TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); + + uint32_t index = get_timer_index(Instance); + if (HardwareTimer_Handle[index] == NULL) { + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); + HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); + } + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, pin); + HT->setOverflow(PWM_freq, HERTZ_FORMAT); + HT->pause(); + HT->refresh(); + return HT; +} + + +// init high side pin +HardwareTimer* _initPinPWMHigh(uint32_t PWM_freq, int ulPin) +{ + return _initPinPWM(PWM_freq, ulPin); +} + +// init low side pin +HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) +{ + PinName pin = digitalPinToPinName(ulPin); + TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); + uint32_t index = get_timer_index(Instance); + + if (HardwareTimer_Handle[index] == NULL) { + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); + HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); + TIM_OC_InitTypeDef sConfigOC = TIM_OC_InitTypeDef(); + sConfigOC.OCMode = TIM_OCMODE_PWM2; + sConfigOC.Pulse = 100; + sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; + sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; + sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; + sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET; + sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; + uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel); + } + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, pin); + HT->setOverflow(PWM_freq, HERTZ_FORMAT); + HT->pause(); + HT->refresh(); + return HT; +} + + +// align the timers to end the init +void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3) +{ + HT1->pause(); + HT1->refresh(); + HT2->pause(); + HT2->refresh(); + HT3->pause(); + HT3->refresh(); + HT1->resume(); + HT2->resume(); + HT3->resume(); +} + +// align the timers to end the init +void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3,HardwareTimer *HT4) +{ + HT1->pause(); + HT1->refresh(); + HT2->pause(); + HT2->refresh(); + HT3->pause(); + HT3->refresh(); + HT4->pause(); + HT4->refresh(); + HT1->resume(); + HT2->resume(); + HT3->resume(); + HT4->resume(); +} + +HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) +{ + PinName uhPinName = digitalPinToPinName(pinA_h); + PinName ulPinName = digitalPinToPinName(pinA_l); + PinName vhPinName = digitalPinToPinName(pinB_h); + PinName vlPinName = digitalPinToPinName(pinB_l); + PinName whPinName = digitalPinToPinName(pinC_h); + PinName wlPinName = digitalPinToPinName(pinC_l); + + TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM); + + uint32_t index = get_timer_index(Instance); + + if (HardwareTimer_Handle[index] == NULL) { + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM)); + HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); + ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT); + } + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + + HT->setMode(STM_PIN_CHANNEL(pinmap_function(uhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, uhPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(ulPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, ulPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(vhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vhPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(vlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vlPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(whPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, whPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, wlPinName); + + // dead time is set in nanoseconds + uint32_t dead_time_ns = (float)(1e9/PWM_freq)*dead_zone; + uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns); + LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! + LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N); + + HT->pause(); + HT->refresh(); + HT->resume(); + return HT; +} + + +// returns 0 if each pair of pwm channels has same channel +// returns 1 all the channels belong to the same timer - hardware inverted channels +// returns -1 if neither - avoid configuring - error!!! +int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + PinName nameAH = digitalPinToPinName(pinA_h); + PinName nameAL = digitalPinToPinName(pinA_l); + PinName nameBH = digitalPinToPinName(pinB_h); + PinName nameBL = digitalPinToPinName(pinB_l); + PinName nameCH = digitalPinToPinName(pinC_h); + PinName nameCL = digitalPinToPinName(pinC_l); + int tim1 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAH, PinMap_PWM)); + int tim2 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAL, PinMap_PWM)); + int tim3 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBH, PinMap_PWM)); + int tim4 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBL, PinMap_PWM)); + int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM)); + int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM)); + if(tim1 == tim2 && tim2==tim3 && tim3==tim4 && tim4==tim5 && tim5==tim6) + return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer + else if(tim1 == tim2 && tim3==tim4 && tim5==tim6) + return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer + else + return _ERROR_6PWM; // might be error avoid configuration +} + + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); + HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); + // allign the timers + _alignPWMTimers(HT1, HT2, HT2); +} + + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); + HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); + HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); + // allign the timers + _alignPWMTimers(HT1, HT2, HT3); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); + HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); + HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); + HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinD); + // allign the timers + _alignPWMTimers(HT1, HT2, HT3, HT4); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +//- hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // transform duty cycle from [0,1] to [0,4095] + _setPwm(pinA, _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(pinB, _PWM_RANGE*dc_b, _PWM_RESOLUTION); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +//- hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,4095] + _setPwm(pinA, _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(pinB, _PWM_RANGE*dc_b, _PWM_RESOLUTION); + _setPwm(pinC, _PWM_RANGE*dc_c, _PWM_RESOLUTION); +} + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +//- hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,4095] + _setPwm(pin1A, _PWM_RANGE*dc_1a, _PWM_RESOLUTION); + _setPwm(pin1B, _PWM_RANGE*dc_1b, _PWM_RESOLUTION); + _setPwm(pin2A, _PWM_RANGE*dc_2a, _PWM_RESOLUTION); + _setPwm(pin2B, _PWM_RANGE*dc_2b, _PWM_RESOLUTION); +} + + + + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to |%0kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + + // find configuration + int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); + // configure accordingly + switch(config){ + case _ERROR_6PWM: + return -1; // fail + break; + case _HARDWARE_6PWM: + _initHardware6PWMInterface(pwm_frequency, dead_zone, pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); + break; + case _SOFTWARE_6PWM: + HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinA_h); + _initPinPWMLow(pwm_frequency, pinA_l); + HardwareTimer* HT2 = _initPinPWMHigh(pwm_frequency,pinB_h); + _initPinPWMLow(pwm_frequency, pinB_l); + HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency,pinC_h); + _initPinPWMLow(pwm_frequency, pinC_l); + _alignPWMTimers(HT1, HT2, HT3); + break; + } + return 0; // success +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + // find configuration + int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); + // set pwm accordingly + switch(config){ + case _HARDWARE_6PWM: + _setPwm(pinA_h, _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(pinB_h, _PWM_RANGE*dc_b, _PWM_RESOLUTION); + _setPwm(pinC_h, _PWM_RANGE*dc_c, _PWM_RESOLUTION); + break; + case _SOFTWARE_6PWM: + _setPwm(pinA_l, _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinA_h, _constrain(dc_a - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinB_l, _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinB_h, _constrain(dc_b - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinC_l, _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinC_h, _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + break; + } +} +#endif \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_encoder/src/sensors/Encoder.cpp b/minimal_project_examples/stm32_bldc_encoder/src/sensors/Encoder.cpp new file mode 100644 index 00000000..1965c015 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_encoder/src/sensors/Encoder.cpp @@ -0,0 +1,228 @@ +#include "Encoder.h" + + +/* + Encoder(int encA, int encB , int cpr, int index) + - encA, encB - encoder A and B pins + - cpr - counts per rotation number (cpm=ppm*4) + - index pin - (optional input) +*/ + +Encoder::Encoder(int _encA, int _encB , float _ppr, int _index){ + + // Encoder measurement structure init + // hardware pins + pinA = _encA; + pinB = _encB; + // counter setup + pulse_counter = 0; + pulse_timestamp = 0; + + cpr = _ppr; + A_active = 0; + B_active = 0; + I_active = 0; + // index pin + index_pin = _index; // its 0 if not used + index_pulse_counter = 0; + + // velocity calculation variables + prev_Th = 0; + pulse_per_second = 0; + prev_pulse_counter = 0; + prev_timestamp_us = _micros(); + + // extern pullup as default + pullup = Pullup::EXTERN; + // enable quadrature encoder by default + quadrature = Quadrature::ON; +} + +// Encoder interrupt callback functions +// A channel +void Encoder::handleA() { + int A = digitalRead(pinA); + switch (quadrature){ + case Quadrature::ON: + // CPR = 4xPPR + if ( A != A_active ) { + pulse_counter += (A_active == B_active) ? 1 : -1; + pulse_timestamp = _micros(); + A_active = A; + } + break; + case Quadrature::OFF: + // CPR = PPR + if(A && !digitalRead(pinB)){ + pulse_counter++; + pulse_timestamp = _micros(); + } + break; + } +} +// B channel +void Encoder::handleB() { + int B = digitalRead(pinB); + switch (quadrature){ + case Quadrature::ON: + // // CPR = 4xPPR + if ( B != B_active ) { + pulse_counter += (A_active != B_active) ? 1 : -1; + pulse_timestamp = _micros(); + B_active = B; + } + break; + case Quadrature::OFF: + // CPR = PPR + if(B && !digitalRead(pinA)){ + pulse_counter--; + pulse_timestamp = _micros(); + } + break; + } +} + +// Index channel +void Encoder::handleIndex() { + if(hasIndex()){ + int I = digitalRead(index_pin); + if(I && !I_active){ + // align encoder on each index + if(index_pulse_counter){ + long tmp = pulse_counter; + // corrent the counter value + pulse_counter = round((double)pulse_counter/(double)cpr)*cpr; + // preserve relative speed + prev_pulse_counter += pulse_counter - tmp; + } else { + // initial offset + index_pulse_counter = pulse_counter; + } + } + I_active = I; + } +} + +/* + Shaft angle calculation +*/ +float Encoder::getAngle(){ + return natural_direction * _2PI * (pulse_counter) / ((float)cpr); +} +/* + Shaft velocity calculation + function using mixed time and frequency measurement technique +*/ +float Encoder::getVelocity(){ + // timestamp + long timestamp_us = _micros(); + // sampling time calculation + float Ts = (timestamp_us - prev_timestamp_us) * 1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + + // time from last impulse + float Th = (timestamp_us - pulse_timestamp) * 1e-6; + long dN = pulse_counter - prev_pulse_counter; + + // Pulse per second calculation (Eq.3.) + // dN - impulses received + // Ts - sampling time - time in between function calls + // Th - time from last impulse + // Th_1 - time form last impulse of the previous call + // only increment if some impulses received + float dt = Ts + prev_Th - Th; + pulse_per_second = (dN != 0 && dt > Ts/2) ? dN / dt : pulse_per_second; + + // if more than 0.05 passed in between impulses + if ( Th > 0.1) pulse_per_second = 0; + + // velocity calculation + float velocity = pulse_per_second / ((float)cpr) * (_2PI); + + // save variables for next pass + prev_timestamp_us = timestamp_us; + // save velocity calculation variables + prev_Th = Th; + prev_pulse_counter = pulse_counter; + return natural_direction * (velocity); +} + +// getter for index pin +// return -1 if no index +int Encoder::needsAbsoluteZeroSearch(){ + return index_pulse_counter == 0; +} +// getter for index pin +int Encoder::hasAbsoluteZero(){ + return hasIndex(); +} +// initialize counter to zero +float Encoder::initRelativeZero(){ + long angle_offset = -pulse_counter; + pulse_counter = 0; + pulse_timestamp = _micros(); + return _2PI * (angle_offset) / ((float)cpr); +} +// initialize index to zero +float Encoder::initAbsoluteZero(){ + pulse_counter -= index_pulse_counter; + prev_pulse_counter = pulse_counter; + return (index_pulse_counter) / ((float)cpr) * (_2PI); +} +// private function used to determine if encoder has index +int Encoder::hasIndex(){ + return index_pin != 0; +} + + +// encoder initialisation of the hardware pins +// and calculation variables +void Encoder::init(){ + + // Encoder - check if pullup needed for your encoder + if(pullup == Pullup::INTERN){ + pinMode(pinA, INPUT_PULLUP); + pinMode(pinB, INPUT_PULLUP); + if(hasIndex()) pinMode(index_pin,INPUT_PULLUP); + }else{ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if(hasIndex()) pinMode(index_pin,INPUT); + } + + // counter setup + pulse_counter = 0; + pulse_timestamp = _micros(); + // velocity calculation variables + prev_Th = 0; + pulse_per_second = 0; + prev_pulse_counter = 0; + prev_timestamp_us = _micros(); + + // initial cpr = PPR + // change it if the mode is quadrature + if(quadrature == Quadrature::ON) cpr = 4*cpr; + +} + +// function enabling hardware interrupts of the for the callback provided +// if callback is not provided then the interrupt is not enabled +void Encoder::enableInterrupts(void (*doA)(), void(*doB)(), void(*doIndex)()){ + // attach interrupt if functions provided + switch(quadrature){ + case Quadrature::ON: + // A callback and B callback + if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE); + if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE); + break; + case Quadrature::OFF: + // A callback and B callback + if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, RISING); + if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, RISING); + break; + } + + // if index used initialize the index interrupt + if(hasIndex() && doIndex != nullptr) attachInterrupt(digitalPinToInterrupt(index_pin), doIndex, CHANGE); +} diff --git a/minimal_project_examples/stm32_bldc_encoder/src/sensors/Encoder.h b/minimal_project_examples/stm32_bldc_encoder/src/sensors/Encoder.h new file mode 100644 index 00000000..29122888 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_encoder/src/sensors/Encoder.h @@ -0,0 +1,105 @@ +#ifndef ENCODER_LIB_H +#define ENCODER_LIB_H + +#include "Arduino.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/base_classes/Sensor.h" + + +/** + * Quadrature mode configuration structure + */ +enum Quadrature{ + ON, //!< Enable quadrature mode CPR = 4xPPR + OFF //!< Disable quadrature mode / CPR = PPR +}; + +class Encoder: public Sensor{ + public: + /** + Encoder class constructor + @param encA encoder B pin + @param encB encoder B pin + @param ppr impulses per rotation (cpr=ppr*4) + @param index index pin number (optional input) + */ + Encoder(int encA, int encB , float ppr, int index = 0); + + /** encoder initialise pins */ + void init(); + /** + * function enabling hardware interrupts for the encoder channels with provided callback functions + * if callback is not provided then the interrupt is not enabled + * + * @param doA pointer to the A channel interrupt handler function + * @param doB pointer to the B channel interrupt handler function + * @param doIndex pointer to the Index channel interrupt handler function + * + */ + void enableInterrupts(void (*doA)() = nullptr, void(*doB)() = nullptr, void(*doIndex)() = nullptr); + + // Encoder interrupt callback functions + /** A channel callback function */ + void handleA(); + /** B channel callback function */ + void handleB(); + /** Index channel callback function */ + void handleIndex(); + + + // pins A and B + int pinA; //!< encoder hardware pin A + int pinB; //!< encoder hardware pin B + int index_pin; //!< index pin + + // Encoder configuration + Pullup pullup; //!< Configuration parameter internal or external pullups + Quadrature quadrature;//!< Configuration parameter enable or disable quadrature mode + float cpr;//!< encoder cpr number + + // Abstract functions of the Sensor class implementation + /** get current angle (rad) */ + float getAngle() override; + /** get current angular velocity (rad/s) */ + float getVelocity() override; + /** + * set current angle as zero angle + * return the angle [rad] difference + */ + float initRelativeZero() override; + /** + * set index angle as zero angle + * return the angle [rad] difference + */ + float initAbsoluteZero() override; + /** + * returns 0 if it has no index + * 0 - encoder without index + * 1 - encoder with index + */ + int hasAbsoluteZero() override; + /** + * returns 0 if it does need search for absolute zero + * 0 - encoder without index + * 1 - ecoder with index + */ + int needsAbsoluteZeroSearch() override; + + private: + int hasIndex(); //!< function returning 1 if encoder has index pin and 0 if not. + + volatile long pulse_counter;//!< current pulse counter + volatile long pulse_timestamp;//!< last impulse timestamp in us + volatile int A_active; //!< current active states of A channel + volatile int B_active; //!< current active states of B channel + volatile int I_active; //!< current active states of Index channel + volatile long index_pulse_counter; //!< impulse counter number upon first index interrupt + + // velocity calculation variables + float prev_Th, pulse_per_second; + volatile long prev_pulse_counter, prev_timestamp_us; +}; + + +#endif diff --git a/minimal_project_examples/stm32_bldc_encoder/stm32_bldc_encoder.ino b/minimal_project_examples/stm32_bldc_encoder/stm32_bldc_encoder.ino new file mode 100644 index 00000000..3ddabbab --- /dev/null +++ b/minimal_project_examples/stm32_bldc_encoder/stm32_bldc_encoder.ino @@ -0,0 +1,152 @@ +/** + * Comprehensive BLDC motor control example using encoder + * + * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: + * - configure PID controller constants + * - change motion control loops + * - monitor motor variabels + * - set target values + * - check all the configuration values + * + * To check the config value just enter the command letter. + * For example: - to read velocity PI controller P gain run: P + * - to set velocity PI controller P gain to 1.2 run: P1.2 + * + * To change the target value just enter a number in the terminal: + * For example: - to change the target value to -0.1453 enter: -0.1453 + * - to get the current target value enter: V3 + * + * List of commands: + * - P: velocity PID controller P gain + * - I: velocity PID controller I gain + * - D: velocity PID controller D gain + * - R: velocity PID controller voltage ramp + * - F: velocity Low pass filter time constant + * - K: angle P controller P gain + * - N: angle P controller velocity limit + * - L: system voltage limit + * - C: control loop + * - 0: voltage + * - 1: velocity + * - 2: angle + * - V: get motor variables + * - 0: currently set voltage + * - 1: current velocity + * - 2: current anglex + * - 3: current target value + * + */ +#include "src/BLDCMotor.h" +#include "src/sensors/Encoder.h" +#include "src/drivers/BLDCDriver6PWM.h" + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver6PWM driver = BLDCDriver6PWM(7, 2, 6, 3, 5, 4, 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();} + +void setup() { + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.dead_zone = 0.05; // 5% + driver.init(); + // link driver + motor.linkDriver(&driver); + + + // choose FOC modulation + motor.foc_modulation = FOCModulationType::SpaceVectorPWM; + // set control loop type to be used + motor.controller = ControlType::voltage; + + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.2; + motor.PID_velocity.I = 20; + motor.PID_velocity.D = 0; + // default voltage_power_supply + motor.voltage_limit = 12; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01; + + // angle loop controller + motor.P_angle.P = 20; + // angle loop velocity limit + motor.velocity_limit = 50; + + // use monitoring with serial for motor init + // monitoring port + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialise motor + motor.init(); + // align encoder and start FOC + motor.initFOC(); + + // set the inital target value + motor.target = 2; + + + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + Serial.println("Motor commands sketch | Initial motion control > torque/voltage : target 2V."); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor.loopFOC(); + + // iterative function setting the outter loop target + // velocity, position or voltage + // if tatget not set in parameter uses motor.target variable + motor.move(); + + // user communication + motor.command(serialReceiveUserCommand()); +} + +// utility function enabling serial communication the user +String serialReceiveUserCommand() { + + // a string to hold incoming data + static String received_chars; + + String command = ""; + + while (Serial.available()) { + // get the new byte: + char inChar = (char)Serial.read(); + // add it to the string buffer: + received_chars += inChar; + + // end of user input + if (inChar == '\n') { + + // execute the user command + command = received_chars; + + // reset the command buffer + received_chars = ""; + } + } + return command; +} \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_hall/src/BLDCMotor.cpp b/minimal_project_examples/stm32_bldc_hall/src/BLDCMotor.cpp new file mode 100644 index 00000000..5d36ce35 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_hall/src/BLDCMotor.cpp @@ -0,0 +1,421 @@ +#include "BLDCMotor.h" + +// // BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en) +// // - phA, phB, phC - motor A,B,C phase pwm pins +// // - pp - pole pair number +// // - cpr - counts per rotation number (cpm=ppm*4) +// // - enable pin - (optional input) +// BLDCMotor::BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en){ +// noop; +// } + +// BLDCMotor( int pp) +// - phA, phB, phC - motor A,B,C phase pwm pins +// - pp - pole pair number +// - cpr - counts per rotation number (cpm=ppm*4) +// - enable pin - (optional input) +BLDCMotor::BLDCMotor(int pp) +: FOCMotor() +{ + // save pole pairs number + pole_pairs = pp; +} + + +/** + Link the driver which controls the motor +*/ +void BLDCMotor::linkDriver(BLDCDriver* _driver) { + driver = _driver; +} + +// init hardware pins +void BLDCMotor::init() { + if(monitor_port) monitor_port->println("MOT: Initialise variables."); + // sanity check for the voltage limit configuration + if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; + // constrain voltage for sensor alignment + if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; + // update the controller limits + PID_velocity.limit = voltage_limit; + P_angle.limit = velocity_limit; + + _delay(500); + // enable motor + if(monitor_port) monitor_port->println("MOT: Enable driver."); + enable(); + _delay(500); +} + + +// disable motor driver +void BLDCMotor::disable() +{ + // set zero to PWM + driver->setPwm(0, 0, 0); + // disable the driver + driver->disable(); +} +// enable motor driver +void BLDCMotor::enable() +{ + // enable the driver + driver->enable(); + // set zero to PWM + driver->setPwm(0, 0, 0); + +} + +/** + FOC functions +*/ +// FOC initialization function +int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction ) { + int exit_flag = 1; + // align motor if necessary + // alignment necessary for encoders! + if(zero_electric_offset != NOT_SET){ + // abosolute zero offset provided - no need to align + zero_electric_angle = zero_electric_offset; + // set the sensor direction - default CW + sensor->natural_direction = sensor_direction; + }else{ + // sensor and motor alignment + _delay(500); + exit_flag = alignSensor(); + _delay(500); + } + if(monitor_port) monitor_port->println("MOT: Motor ready."); + + return exit_flag; +} +// Encoder alignment to electrical 0 angle +int BLDCMotor::alignSensor() { + if(monitor_port) monitor_port->println("MOT: Align sensor."); + // align the electrical phases of the motor and sensor + // set angle -90 degrees + + float start_angle = shaftAngle(); + for (int i = 0; i <=5; i++ ) { + float angle = _3PI_2 + _2PI * i / 6.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(200); + } + float mid_angle = shaftAngle(); + for (int i = 5; i >=0; i-- ) { + float angle = _3PI_2 + _2PI * i / 6.0; + setPhaseVoltage(voltage_sensor_align, 0, angle); + _delay(200); + } + if (mid_angle < start_angle) { + if(monitor_port) monitor_port->println("MOT: natural_direction==CCW"); + sensor->natural_direction = Direction::CCW; + } else if (mid_angle == start_angle) { + if(monitor_port) monitor_port->println("MOT: Sensor failed to notice movement"); + } else{ + if(monitor_port) monitor_port->println("MOT: natural_direction==CW"); + } + + // let the motor stabilize for 2 sec + _delay(2000); + // set sensor to zero + sensor->initRelativeZero(); + _delay(500); + setPhaseVoltage(0, 0, 0); + _delay(200); + + // find the index if available + int exit_flag = absoluteZeroAlign(); + _delay(500); + if(monitor_port){ + if(exit_flag< 0 ) monitor_port->println("MOT: Error: Not found!"); + if(exit_flag> 0 ) monitor_port->println("MOT: Success!"); + else monitor_port->println("MOT: Not available!"); + } + return exit_flag; +} + + +// Encoder alignment the absolute zero angle +// - to the index +int BLDCMotor::absoluteZeroAlign() { + + if(monitor_port) monitor_port->println("MOT: Absolute zero align."); + // if no absolute zero return + if(!sensor->hasAbsoluteZero()) return 0; + + + if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println("MOT: Searching..."); + // search the absolute zero with small velocity + while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){ + loopFOC(); + voltage_q = PID_velocity(velocity_index_search - shaftVelocity()); + } + voltage_q = 0; + // disable motor + setPhaseVoltage(0, 0, 0); + + // align absolute zero if it has been found + if(!sensor->needsAbsoluteZeroSearch()){ + // align the sensor with the absolute zero + float zero_offset = sensor->initAbsoluteZero(); + // remember zero electric angle + zero_electric_angle = _normalizeAngle(_electricalAngle(zero_offset, pole_pairs)); + } + // return bool if zero found + return !sensor->needsAbsoluteZeroSearch() ? 1 : -1; +} + +// Iterative function looping FOC algorithm, setting Uq on the Motor +// The faster it can be run the better +void BLDCMotor::loopFOC() { + // shaft angle + shaft_angle = shaftAngle(); + // set the phase voltage - FOC heart function :) + setPhaseVoltage(voltage_q, voltage_d, _electricalAngle(shaft_angle,pole_pairs)); +} + +// Iterative function running outer loop of the FOC algorithm +// Behavior of this function is determined by the motor.controller variable +// It runs either angle, velocity or voltage loop +// - needs to be called iteratively it is asynchronous function +// - if target is not set it uses motor.target value +void BLDCMotor::move(float new_target) { + // set internal target variable + if( new_target != NOT_SET ) target = new_target; + // get angular velocity + shaft_velocity = shaftVelocity(); + // choose control loop + switch (controller) { + case ControlType::voltage: + voltage_q = target; + break; + case ControlType::angle: + // angle set point + // include angle loop + shaft_angle_sp = target; + shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); + voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + break; + case ControlType::velocity: + // velocity set point + // include velocity loop + shaft_velocity_sp = target; + voltage_q = PID_velocity(shaft_velocity_sp - shaft_velocity); + break; + case ControlType::velocity_openloop: + // velocity control in open loop + // loopFOC should not be called + shaft_velocity_sp = target; + velocityOpenloop(shaft_velocity_sp); + break; + case ControlType::angle_openloop: + // angle control in open loop + // loopFOC should not be called + shaft_angle_sp = target; + angleOpenloop(shaft_angle_sp); + break; + } +} + + +// Method using FOC to set Uq and Ud to the motor at the optimal angle +// Function implementing Space Vector PWM and Sine PWM algorithms +// +// Function using sine approximation +// regular sin + cos ~300us (no memory usaage) +// approx _sin + _cos ~110us (400Byte ~ 20% of memory) +void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { + + const bool centered = true; + int sector; + float _ca,_sa; + + switch (foc_modulation) + { + case FOCModulationType::Trapezoid_120 : + // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5 + static int trap_120_map[6][3] = { + {0,1,-1},{-1,1,0},{-1,0,1},{0,-1,1},{1,-1,0},{1,0,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + }; + // static int trap_120_state = 0; + sector = 6 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + + Ua = Uq + trap_120_map[sector][0] * Uq; + Ub = Uq + trap_120_map[sector][1] * Uq; + Uc = Uq + trap_120_map[sector][2] * Uq; + + if (centered) { + Ua += (driver->voltage_limit)/2 -Uq; + Ub += (driver->voltage_limit)/2 -Uq; + Uc += (driver->voltage_limit)/2 -Uq; + } + break; + + case FOCModulationType::Trapezoid_150 : + // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8 + static int trap_150_map[12][3] = { + {0,1,-1},{-1,1,-1},{-1,1,0},{-1,1,1},{-1,0,1},{-1,-1,1},{0,-1,1},{1,-1,1},{1,-1,0},{1,-1,-1},{1,0,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z + }; + // static int trap_150_state = 0; + sector = 12 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes + + Ua = Uq + trap_150_map[sector][0] * Uq; + Ub = Uq + trap_150_map[sector][1] * Uq; + Uc = Uq + trap_150_map[sector][2] * Uq; + + //center + if (centered) { + Ua += (driver->voltage_limit)/2 -Uq; + Ub += (driver->voltage_limit)/2 -Uq; + Uc += (driver->voltage_limit)/2 -Uq; + } + + break; + + case FOCModulationType::SinePWM : + // Sinusoidal PWM modulation + // Inverse Park + Clarke transformation + + // angle normalization in between 0 and 2pi + // only necessary if using _sin and _cos - approximation functions + angle_el = _normalizeAngle(angle_el + zero_electric_angle); + _ca = _cos(angle_el); + _sa = _sin(angle_el); + // Inverse park transform + Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq; + Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq; + + // Clarke transform + Ua = Ualpha + driver->voltage_limit/2; + Ub = -0.5 * Ualpha + _SQRT3_2 * Ubeta + driver->voltage_limit/2; + Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + driver->voltage_limit/2; + + if (!centered) { + float Umin = min(Ua, min(Ub, Uc)); + Ua -= Umin; + Ub -= Umin; + Uc -= Umin; + } + + break; + + case FOCModulationType::SpaceVectorPWM : + // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm + // https://www.youtube.com/watch?v=QMSWUMEAejg + + // if negative voltages change inverse the phase + // angle + 180degrees + if(Uq < 0) angle_el += _PI; + Uq = abs(Uq); + + // angle normalisation in between 0 and 2pi + // only necessary if using _sin and _cos - approximation functions + angle_el = _normalizeAngle(angle_el + zero_electric_angle + _PI_2); + + // find the sector we are in currently + sector = floor(angle_el / _PI_3) + 1; + // calculate the duty cycles + float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_limit; + float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_limit; + // two versions possible + float T0 = 0; // pulled to 0 - better for low power supply voltage + if (centered) { + T0 = 1 - T1 - T2; //centered around driver->voltage_limit/2 + } + + // calculate the duty cycles(times) + float Ta,Tb,Tc; + switch(sector){ + case 1: + Ta = T1 + T2 + T0/2; + Tb = T2 + T0/2; + Tc = T0/2; + break; + case 2: + Ta = T1 + T0/2; + Tb = T1 + T2 + T0/2; + Tc = T0/2; + break; + case 3: + Ta = T0/2; + Tb = T1 + T2 + T0/2; + Tc = T2 + T0/2; + break; + case 4: + Ta = T0/2; + Tb = T1+ T0/2; + Tc = T1 + T2 + T0/2; + break; + case 5: + Ta = T2 + T0/2; + Tb = T0/2; + Tc = T1 + T2 + T0/2; + break; + case 6: + Ta = T1 + T2 + T0/2; + Tb = T0/2; + Tc = T1 + T0/2; + break; + default: + // possible error state + Ta = 0; + Tb = 0; + Tc = 0; + } + + // calculate the phase voltages and center + Ua = Ta*driver->voltage_limit; + Ub = Tb*driver->voltage_limit; + Uc = Tc*driver->voltage_limit; + break; + + } + + // set the voltages in driver + driver->setPwm(Ua, Ub, Uc); +} + + + +// Function (iterative) generating open loop movement for target velocity +// - target_velocity - rad/s +// it uses voltage_limit variable +void BLDCMotor::velocityOpenloop(float target_velocity){ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6; + + // calculate the necessary angle to achieve target velocity + shaft_angle += target_velocity*Ts; + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; +} + +// Function (iterative) generating open loop movement towards the target angle +// - target_angle - rad +// it uses voltage_limit and velocity_limit variables +void BLDCMotor::angleOpenloop(float target_angle){ + // get current timestamp + unsigned long now_us = _micros(); + // calculate the sample time from last call + float Ts = (now_us - open_loop_timestamp) * 1e-6; + + // calculate the necessary angle to move from current position towards target angle + // with maximal velocity (velocity_limit) + if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)) + shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; + else + shaft_angle = target_angle; + + // set the maximal allowed voltage (voltage_limit) with the necessary angle + setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs)); + + // save timestamp for next call + open_loop_timestamp = now_us; +} diff --git a/minimal_project_examples/stm32_bldc_hall/src/BLDCMotor.h b/minimal_project_examples/stm32_bldc_hall/src/BLDCMotor.h new file mode 100644 index 00000000..164cc108 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_hall/src/BLDCMotor.h @@ -0,0 +1,109 @@ +#ifndef BLDCMotor_h +#define BLDCMotor_h + +#include "Arduino.h" +#include "common/base_classes/FOCMotor.h" +#include "common/base_classes/Sensor.h" +#include "common/base_classes/BLDCDriver.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" +#include "common/defaults.h" + +/** + BLDC motor class +*/ +class BLDCMotor: public FOCMotor +{ + public: + /** + BLDCMotor class constructor + @param pp pole pairs number + */ + BLDCMotor(int pp); + + /** + * Function linking a motor and a foc driver + * + * @param driver BLDCDriver class implementing all the hardware specific functions necessary PWM setting + */ + void linkDriver(BLDCDriver* driver); + + /** + * BLDCDriver link: + * - 3PWM + * - 6PWM + */ + BLDCDriver* driver; + + /** Motor hardware init function */ + void init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + */ + int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override; + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + void loopFOC() override; + + /** + * Function executing the control loops set by the controller parameter of the BLDCMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + void move(float target = NOT_SET) override; + + float Ua,Ub,Uc;//!< Current phase voltages Ua,Ub and Uc set to motor + float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform + + + private: + // FOC methods + /** + * Method using FOC to set Uq to the motor at the optimal angle + * Heart of the FOC algorithm + * + * @param Uq Current voltage in q axis to set to the motor + * @param Ud Current voltage in d axis to set to the motor + * @param angle_el current electrical angle of the motor + */ + void setPhaseVoltage(float Uq, float Ud, float angle_el); + /** Sensor alignment to electrical 0 angle of the motor */ + int alignSensor(); + /** Motor and sensor alignment to the sensors absolute 0 angle */ + int absoluteZeroAlign(); + + + // Open loop motion control + /** + * Function (iterative) generating open loop movement for target velocity + * it uses voltage_limit variable + * + * @param target_velocity - rad/s + */ + void velocityOpenloop(float target_velocity); + /** + * Function (iterative) generating open loop movement towards the target angle + * it uses voltage_limit and velocity_limit variables + * + * @param target_angle - rad + */ + void angleOpenloop(float target_angle); + // open loop variables + long open_loop_timestamp; +}; + + +#endif diff --git a/minimal_project_examples/stm32_bldc_hall/src/common/base_classes/BLDCDriver.h b/minimal_project_examples/stm32_bldc_hall/src/common/base_classes/BLDCDriver.h new file mode 100644 index 00000000..708323fb --- /dev/null +++ b/minimal_project_examples/stm32_bldc_hall/src/common/base_classes/BLDCDriver.h @@ -0,0 +1,28 @@ +#ifndef BLDCDRIVER_H +#define BLDCDRIVER_H + +class BLDCDriver{ + public: + + /** Initialise hardware */ + virtual int init(); + /** Enable hardware */ + virtual void enable(); + /** Disable hardware */ + virtual void disable(); + + long pwm_frequency; //!< pwm frequency value in hertz + float voltage_power_supply; //!< power supply voltage + float voltage_limit; //!< limiting voltage set to the motor + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + virtual void setPwm(float Ua, float Ub, float Uc); +}; + +#endif \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_hall/src/common/base_classes/FOCMotor.cpp b/minimal_project_examples/stm32_bldc_hall/src/common/base_classes/FOCMotor.cpp new file mode 100644 index 00000000..1d1a0f43 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_hall/src/common/base_classes/FOCMotor.cpp @@ -0,0 +1,241 @@ +#include "FOCMotor.h" + +/** + * Default constructor - setting all variabels to default values + */ +FOCMotor::FOCMotor() +{ + // maximum angular velocity to be used for positioning + velocity_limit = DEF_VEL_LIM; + // maximum voltage to be set to the motor + voltage_limit = DEF_POWER_SUPPLY; + + // index search velocity + velocity_index_search = DEF_INDEX_SEARCH_TARGET_VELOCITY; + // sensor and motor align voltage + voltage_sensor_align = DEF_VOLTAGE_SENSOR_ALIGN; + + // electric angle of comthe zero angle + zero_electric_angle = 0; + + // default modulation is SinePWM + foc_modulation = FOCModulationType::SinePWM; + + // default target value + target = 0; + voltage_d = 0; + voltage_q = 0; + + //monitor_port + monitor_port = nullptr; + //sensor + sensor = nullptr; +} + + +/** + Sensor communication methods +*/ +void FOCMotor::linkSensor(Sensor* _sensor) { + sensor = _sensor; +} +// shaft angle calculation +float FOCMotor::shaftAngle() { + // if no sensor linked return 0 + if(!sensor) return shaft_angle; + return sensor->getAngle(); +} +// shaft velocity calculation +float FOCMotor::shaftVelocity() { + // if no sensor linked return 0 + if(!sensor) return 0; + return LPF_velocity(sensor->getVelocity()); +} + +/** + * Monitoring functions + */ +// function implementing the monitor_port setter +void FOCMotor::useMonitoring(Print &print){ + monitor_port = &print; //operate on the address of print + if(monitor_port ) monitor_port->println("MOT: Monitor enabled!"); +} +// utility function intended to be used with serial plotter to monitor motor variables +// significantly slowing the execution down!!!! +void FOCMotor::monitor() { + if(!monitor_port) return; + switch (controller) { + case ControlType::velocity_openloop: + case ControlType::velocity: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_velocity_sp); + monitor_port->print("\t"); + monitor_port->println(shaft_velocity); + break; + case ControlType::angle_openloop: + case ControlType::angle: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_angle_sp); + monitor_port->print("\t"); + monitor_port->println(shaft_angle); + break; + case ControlType::voltage: + monitor_port->print(voltage_q); + monitor_port->print("\t"); + monitor_port->print(shaft_angle); + monitor_port->print("\t"); + monitor_port->println(shaft_velocity); + break; + } +} + +int FOCMotor::command(String user_command) { + // error flag + int errorFlag = 1; + // if empty string + if(user_command.length() < 1) return errorFlag; + + // parse command letter + char cmd = user_command.charAt(0); + // check if get command + char GET = user_command.charAt(1) == '\n'; + // parse command values + float value = user_command.substring(1).toFloat(); + + // a bit of optimisation of variable memory for Arduino UNO (atmega328) + switch(cmd){ + case 'P': // velocity P gain change + case 'I': // velocity I gain change + case 'D': // velocity D gain change + case 'R': // velocity voltage ramp change + if(monitor_port) monitor_port->print(" PID velocity| "); + break; + case 'F': // velocity Tf low pass filter change + if(monitor_port) monitor_port->print(" LPF velocity| "); + break; + case 'K': // angle loop gain P change + if(monitor_port) monitor_port->print(" P angle| "); + break; + case 'L': // velocity voltage limit change + case 'N': // angle loop gain velocity_limit change + if(monitor_port) monitor_port->print(" Limits| "); + break; + } + + // apply the the command + switch(cmd){ + case 'P': // velocity P gain change + if(monitor_port) monitor_port->print("P: "); + if(!GET) PID_velocity.P = value; + if(monitor_port) monitor_port->println(PID_velocity.P); + break; + case 'I': // velocity I gain change + if(monitor_port) monitor_port->print("I: "); + if(!GET) PID_velocity.I = value; + if(monitor_port) monitor_port->println(PID_velocity.I); + break; + case 'D': // velocity D gain change + if(monitor_port) monitor_port->print("D: "); + if(!GET) PID_velocity.D = value; + if(monitor_port) monitor_port->println(PID_velocity.D); + break; + case 'R': // velocity voltage ramp change + if(monitor_port) monitor_port->print("volt_ramp: "); + if(!GET) PID_velocity.output_ramp = value; + if(monitor_port) monitor_port->println(PID_velocity.output_ramp); + break; + case 'L': // velocity voltage limit change + if(monitor_port) monitor_port->print("volt_limit: "); + if(!GET) { + voltage_limit = value; + PID_velocity.limit = value; + } + if(monitor_port) monitor_port->println(voltage_limit); + break; + case 'F': // velocity Tf low pass filter change + if(monitor_port) monitor_port->print("Tf: "); + if(!GET) LPF_velocity.Tf = value; + if(monitor_port) monitor_port->println(LPF_velocity.Tf); + break; + case 'K': // angle loop gain P change + if(monitor_port) monitor_port->print(" P: "); + if(!GET) P_angle.P = value; + if(monitor_port) monitor_port->println(P_angle.P); + break; + case 'N': // angle loop gain velocity_limit change + if(monitor_port) monitor_port->print("vel_limit: "); + if(!GET){ + velocity_limit = value; + P_angle.limit = value; + } + if(monitor_port) monitor_port->println(velocity_limit); + break; + case 'C': + // change control type + if(monitor_port) monitor_port->print("Control: "); + + if(GET){ // if get command + switch(controller){ + case ControlType::voltage: + if(monitor_port) monitor_port->println("voltage"); + break; + case ControlType::velocity: + if(monitor_port) monitor_port->println("velocity"); + break; + case ControlType::angle: + if(monitor_port) monitor_port->println("angle"); + break; + default: + if(monitor_port) monitor_port->println("open loop"); + } + }else{ // if set command + switch((int)value){ + case 0: + if(monitor_port) monitor_port->println("voltage"); + controller = ControlType::voltage; + break; + case 1: + if(monitor_port) monitor_port->println("velocity"); + controller = ControlType::velocity; + break; + case 2: + if(monitor_port) monitor_port->println("angle"); + controller = ControlType::angle; + break; + default: // not valid command + errorFlag = 0; + } + } + break; + case 'V': // get current values of the state variables + switch((int)value){ + case 0: // get voltage + if(monitor_port) monitor_port->print("Uq: "); + if(monitor_port) monitor_port->println(voltage_q); + break; + case 1: // get velocity + if(monitor_port) monitor_port->print("Velocity: "); + if(monitor_port) monitor_port->println(shaft_velocity); + break; + case 2: // get angle + if(monitor_port) monitor_port->print("Angle: "); + if(monitor_port) monitor_port->println(shaft_angle); + break; + case 3: // get angle + if(monitor_port) monitor_port->print("Target: "); + if(monitor_port) monitor_port->println(target); + break; + default: // not valid command + errorFlag = 0; + } + break; + default: // target change + if(monitor_port) monitor_port->print("Target : "); + target = user_command.toFloat(); + if(monitor_port) monitor_port->println(target); + } + // return 0 if error and 1 if ok + return errorFlag; +} \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_hall/src/common/base_classes/FOCMotor.h b/minimal_project_examples/stm32_bldc_hall/src/common/base_classes/FOCMotor.h new file mode 100644 index 00000000..986e1ab2 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_hall/src/common/base_classes/FOCMotor.h @@ -0,0 +1,197 @@ +#ifndef FOCMOTOR_H +#define FOCMOTOR_H + +#include "Arduino.h" +#include "Sensor.h" +#include "BLDCDriver.h" + +#include "../time_utils.h" +#include "../foc_utils.h" +#include "../defaults.h" +#include "../pid.h" +#include "../lowpass_filter.h" + + +/** + * Motiron control type + */ +enum ControlType{ + voltage,//!< Torque control using voltage + velocity,//!< Velocity motion control + angle,//!< Position/angle motion control + velocity_openloop, + angle_openloop +}; + +/** + * FOC modulation type + */ +enum FOCModulationType{ + SinePWM, //!< Sinusoidal PWM modulation + SpaceVectorPWM, //!< Space vector modulation method + Trapezoid_120, + Trapezoid_150 +}; + +/** + Generic motor class +*/ +class FOCMotor +{ + public: + /** + * Default constructor - setting all variabels to default values + */ + FOCMotor(); + + /** Motor hardware init function */ + virtual void init()=0; + /** Motor disable function */ + virtual void disable()=0; + /** Motor enable function */ + virtual void enable()=0; + + /** + * Function linking a motor and a sensor + * + * @param sensor Sensor class wrapper for the FOC algorihtm to read the motor angle and velocity + */ + void linkSensor(Sensor* sensor); + + + /** + * Function initializing FOC algorithm + * and aligning sensor's and motors' zero position + * + * - If zero_electric_offset parameter is set the alignment procedure is skipped + * + * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position. + * @param sensor_direction sensor natural direction - default is CW + * + */ + virtual int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW)=0; + /** + * Function running FOC algorithm in real-time + * it calculates the gets motor angle and sets the appropriate voltages + * to the phase pwm signals + * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us + */ + virtual void loopFOC()=0; + /** + * Function executing the control loops set by the controller parameter of the BLDCMotor. + * + * @param target Either voltage, angle or velocity based on the motor.controller + * If it is not set the motor will use the target set in its variable motor.target + * + * This function doesn't need to be run upon each loop execution - depends of the use case + */ + virtual void move(float target = NOT_SET)=0; + + // State calculation methods + /** Shaft angle calculation in radians [rad] */ + float shaftAngle(); + /** + * Shaft angle calculation function in radian per second [rad/s] + * It implements low pass filtering + */ + float shaftVelocity(); + + // state variables + float target; //!< current target value - depends of the controller + float shaft_angle;//!< current motor angle + float shaft_velocity;//!< current motor velocity + float shaft_velocity_sp;//!< current target velocity + float shaft_angle_sp;//!< current target angle + float voltage_q;//!< current voltage u_q set + float voltage_d;//!< current voltage u_d set + + // motor configuration parameters + float voltage_sensor_align;//!< sensor and motor align voltage parameter + float velocity_index_search;//!< target velocity for index search + int pole_pairs;//!< Motor pole pairs number + + // limiting variables + float voltage_limit; //!< Voltage limitting variable - global limit + float velocity_limit; //!< Velocity limitting variable - global limit + + float zero_electric_angle;//! _2PI ? a_sin - _2PI : a_sin; + return _sin(a_sin); +} + + +// normalizing radian angle to [0,2PI] +float _normalizeAngle(float angle){ + float a = fmod(angle, _2PI); + return a >= 0 ? a : (a + _2PI); +} + +// Electrical angle calculation +float _electricalAngle(float shaft_angle, int pole_pairs) { + return (shaft_angle * pole_pairs); +} \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_hall/src/common/foc_utils.h b/minimal_project_examples/stm32_bldc_hall/src/common/foc_utils.h new file mode 100644 index 00000000..7da3f7bc --- /dev/null +++ b/minimal_project_examples/stm32_bldc_hall/src/common/foc_utils.h @@ -0,0 +1,55 @@ +#ifndef FOCUTILS_LIB_H +#define FOCUTILS_LIB_H + +#include "Arduino.h" + +// sign function +#define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) ) +#define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) +#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) + +// utility defines +#define _2_SQRT3 1.15470053838 +#define _SQRT3 1.73205080757 +#define _1_SQRT3 0.57735026919 +#define _SQRT3_2 0.86602540378 +#define _SQRT2 1.41421356237 +#define _120_D2R 2.09439510239 +#define _PI 3.14159265359 +#define _PI_2 1.57079632679 +#define _PI_3 1.0471975512 +#define _2PI 6.28318530718 +#define _3PI_2 4.71238898038 + +#define NOT_SET -12345.0 + +/** + * Function approximating the sine calculation by using fixed size array + * - execution time ~40us (Arduino UNO) + * + * @param a angle in between 0 and 2PI + */ +float _sin(float a); +/** + * Function approximating cosine calculation by using fixed size array + * - execution time ~50us (Arduino UNO) + * + * @param a angle in between 0 and 2PI + */ +float _cos(float a); + +/** + * normalizing radian angle to [0,2PI] + * @param angle - angle to be normalized + */ +float _normalizeAngle(float angle); + + +/** + * Electrical angle calculation + * + * @param shaft_angle - shaft angle of the motor + * @param pole_pairs - number of pole pairs + */ +float _electricalAngle(float shaft_angle, int pole_pairs); +#endif \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_hall/src/common/lowpass_filter.cpp b/minimal_project_examples/stm32_bldc_hall/src/common/lowpass_filter.cpp new file mode 100644 index 00000000..9bf4ab7a --- /dev/null +++ b/minimal_project_examples/stm32_bldc_hall/src/common/lowpass_filter.cpp @@ -0,0 +1,25 @@ +#include "lowpass_filter.h" + +LowPassFilter::LowPassFilter(float time_constant) + : Tf(time_constant) + , y_prev(0.0f) +{ + timestamp_prev = _micros(); +} + + +float LowPassFilter::operator() (float x) +{ + unsigned long timestamp = _micros(); + float dt = (timestamp - timestamp_prev)*1e-6f; + + if (dt < 0.0f || dt > 0.5f) + dt = 1e-3f; + + float alpha = Tf/(Tf + dt); + float y = alpha*y_prev + (1.0f - alpha)*x; + + y_prev = y; + timestamp_prev = timestamp; + return y; +} \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_hall/src/common/lowpass_filter.h b/minimal_project_examples/stm32_bldc_hall/src/common/lowpass_filter.h new file mode 100644 index 00000000..7c51be54 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_hall/src/common/lowpass_filter.h @@ -0,0 +1,29 @@ +#ifndef LOWPASS_FILTER_H +#define LOWPASS_FILTER_H + + +#include "time_utils.h" +#include "foc_utils.h" + + +/** + * Low pass filter class + */ +class LowPassFilter +{ +public: + /** + * @param Tf - Low pass filter time constant + */ + LowPassFilter(float Tf); + ~LowPassFilter() = default; + + float operator() (float x); + float Tf; //!< Low pass filter time constant + +protected: + unsigned long timestamp_prev; //!< Last execution timestamp + float y_prev; //!< filtered value in previous execution step +}; + +#endif // LOWPASS_FILTER_H \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_hall/src/common/pid.cpp b/minimal_project_examples/stm32_bldc_hall/src/common/pid.cpp new file mode 100644 index 00000000..1a0a9828 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_hall/src/common/pid.cpp @@ -0,0 +1,56 @@ +#include "pid.h" + +PIDController::PIDController(float P, float I, float D, float ramp, float limit) + : P(P) + , I(I) + , D(D) + , output_ramp(ramp) // output derivative limit [volts/second] + , limit(limit) // output supply limit [volts] + , integral_prev(0.0) + , error_prev(0.0) + , output_prev(0.0) +{ + timestamp_prev = _micros()*1e-6; +} + +// PID controller function +float PIDController::operator() (float error){ + // calculate the time from the last call + unsigned long timestamp_now = _micros(); + float Ts = (timestamp_now - timestamp_prev) * 1e-6; + // quick fix for strange cases (micros overflow) + if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + + // u(s) = (P + I/s + Ds)e(s) + // Discrete implementations + // proportional part + // u_p = P *e(k) + float proportional = P * error; + // Tustin transform of the integral part + // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) + float integral = integral_prev + I*Ts*0.5*(error + error_prev); + // antiwindup - limit the output voltage_q + integral = _constrain(integral, -limit, limit); + // Discrete derivation + // u_dk = D(ek - ek_1)/Ts + float derivative = D*(error - error_prev)/Ts; + + // sum all the components + float output = proportional + integral + derivative; + // antiwindup - limit the output variable + output = _constrain(output, -limit, limit); + + // limit the acceleration by ramping the output + float output_rate = (output - output_prev)/Ts; + if (output_rate > output_ramp) + output = output_prev + output_ramp*Ts; + else if (output_rate < -output_ramp) + output = output_prev - output_ramp*Ts; + + // saving for the next pass + integral_prev = integral; + output_prev = output; + error_prev = error; + timestamp_prev = timestamp_now; + return output; +} \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_hall/src/common/pid.h b/minimal_project_examples/stm32_bldc_hall/src/common/pid.h new file mode 100644 index 00000000..0e9ddf54 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_hall/src/common/pid.h @@ -0,0 +1,40 @@ +#ifndef PID_H +#define PID_H + + +#include "time_utils.h" +#include "foc_utils.h" + +/** + * PID controller class + */ +class PIDController +{ +public: + /** + * + * @param P - Proportional gain + * @param I - Integral gain + * @param D - Derivative gain + * @param ramp - Maximum speed of change of the output value + * @param limit - Maximum output value + */ + PIDController(float P, float I, float D, float ramp, float limit); + ~PIDController() = default; + + float operator() (float error); + + float P; //!< Proportional gain + float I; //!< Integral gain + float D; //!< Derivative gain + float output_ramp; //!< Maximum speed of change of the output value + float limit; //!< Maximum output value + +protected: + float integral_prev; //!< last integral component value + float error_prev; //!< last tracking error value + float timestamp_prev; //!< Last execution timestamp + float output_prev; //!< last pid output value +}; + +#endif // PID_H \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_hall/src/common/time_utils.cpp b/minimal_project_examples/stm32_bldc_hall/src/common/time_utils.cpp new file mode 100644 index 00000000..181d03cf --- /dev/null +++ b/minimal_project_examples/stm32_bldc_hall/src/common/time_utils.cpp @@ -0,0 +1,31 @@ +#include "time_utils.h" + +// function buffering delay() +// arduino uno function doesn't work well with interrupts +void _delay(unsigned long ms){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) + // if arduino uno and other atmega328p chips + // use while instad of delay, + // due to wrong measurement based on changed timer0 + unsigned long t = _micros() + ms*1000; + while( _micros() < t ){}; +#else + // regular micros + delay(ms); +#endif +} + + +// function buffering _micros() +// arduino function doesn't work well with interrupts +unsigned long _micros(){ +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) +// if arduino uno and other atmega328p chips + //return the value based on the prescaler + if((TCCR0B & 0b00000111) == 0x01) return (micros()/32); + else return (micros()); +#else + // regular micros + return micros(); +#endif +} diff --git a/minimal_project_examples/stm32_bldc_hall/src/common/time_utils.h b/minimal_project_examples/stm32_bldc_hall/src/common/time_utils.h new file mode 100644 index 00000000..143d4853 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_hall/src/common/time_utils.h @@ -0,0 +1,22 @@ +#ifndef TIME_UTILS_H +#define TIME_UTILS_H + +#include "foc_utils.h" + +/** + * Function implementing delay() function in milliseconds + * - blocking function + * - hardware specific + + * @param ms number of milliseconds to wait + */ +void _delay(unsigned long ms); + +/** + * Function implementing timestamp getting function in microseconds + * hardware specific + */ +unsigned long _micros(); + + +#endif \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_hall/src/drivers/BLDCDriver3PWM.cpp b/minimal_project_examples/stm32_bldc_hall/src/drivers/BLDCDriver3PWM.cpp new file mode 100644 index 00000000..a3fc052d --- /dev/null +++ b/minimal_project_examples/stm32_bldc_hall/src/drivers/BLDCDriver3PWM.cpp @@ -0,0 +1,72 @@ +#include "BLDCDriver3PWM.h" + +BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en){ + // Pin initialization + pwmA = phA; + pwmB = phB; + pwmC = phC; + + // enable_pin pin + enable_pin = en; + + // default power-supply value + voltage_power_supply = DEF_POWER_SUPPLY; + voltage_limit = NOT_SET; + +} + +// enable motor driver +void BLDCDriver3PWM::enable(){ + // enable_pin the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, HIGH); + // set zero to PWM + setPwm(0,0,0); +} + +// disable motor driver +void BLDCDriver3PWM::disable() +{ + // set zero to PWM + setPwm(0, 0, 0); + // disable the driver - if enable_pin pin available + if ( enable_pin != NOT_SET ) digitalWrite(enable_pin, LOW); + +} + +// init hardware pins +int BLDCDriver3PWM::init() { + // a bit of separation + _delay(1000); + + // PWM pins + pinMode(pwmA, OUTPUT); + pinMode(pwmB, OUTPUT); + pinMode(pwmC, OUTPUT); + if(enable_pin != NOT_SET) pinMode(enable_pin, OUTPUT); + + + // sanity check for the voltage limit configuration + if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; + + // Set the pwm frequency to the pins + // hardware specific function - depending on driver and mcu + _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC); + return 0; +} + + +// Set voltage to the pwm pin +void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { + // limit the voltage in driver + Ua = _constrain(Ua, 0.0, voltage_limit); + Ub = _constrain(Ub, 0.0, voltage_limit); + Uc = _constrain(Uc, 0.0, voltage_limit); + // calculate duty cycle + // limited in [0,1] + float dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 ); + float dc_b = _constrain(Ub / voltage_power_supply, 0.0 , 1.0 ); + float dc_c = _constrain(Uc / voltage_power_supply, 0.0 , 1.0 ); + // hardware specific writing + // hardware specific function - depending on driver and mcu + _writeDutyCycle3PWM(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC); +} \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_hall/src/drivers/BLDCDriver3PWM.h b/minimal_project_examples/stm32_bldc_hall/src/drivers/BLDCDriver3PWM.h new file mode 100644 index 00000000..b6f7d7f8 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_hall/src/drivers/BLDCDriver3PWM.h @@ -0,0 +1,52 @@ +#ifndef BLDCDriver3PWM_h +#define BLDCDriver3PWM_h + +#include "../common/base_classes/BLDCDriver.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" +#include "../common/defaults.h" +#include "hardware_api.h" + +/** + 3 pwm bldc driver class +*/ +class BLDCDriver3PWM: public BLDCDriver +{ + public: + /** + BLDCDriver class constructor + @param phA A phase pwm pin + @param phB B phase pwm pin + @param phC C phase pwm pin + @param en enable pin (optional input) + */ + BLDCDriver3PWM(int phA,int phB,int phC, int en = NOT_SET); + + /** Motor hardware init function */ + int init() override; + /** Motor disable function */ + void disable() override; + /** Motor enable function */ + void enable() override; + + // hardware variables + int pwmA; //!< phase A pwm pin number + int pwmB; //!< phase B pwm pin number + int pwmC; //!< phase C pwm pin number + int enable_pin; //!< enable pin number + + /** + * Set phase voltages to the harware + * + * @param Ua - phase A voltage + * @param Ub - phase B voltage + * @param Uc - phase C voltage + */ + void setPwm(float Ua, float Ub, float Uc) override; + + private: + +}; + + +#endif diff --git a/minimal_project_examples/stm32_bldc_hall/src/drivers/hardware_api.h b/minimal_project_examples/stm32_bldc_hall/src/drivers/hardware_api.h new file mode 100644 index 00000000..01c03867 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_hall/src/drivers/hardware_api.h @@ -0,0 +1,123 @@ +#ifndef HARDWARE_UTILS_H +#define HARDWARE_UTILS_H + +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA bldc driver + * @param pinB pinB bldc driver + */ +void _configure2PWM(long pwm_frequency, const int pinA, const int pinB); + +/** + * Configuring PWM frequency, resolution and alignment + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA bldc driver + * @param pinB pinB bldc driver + * @param pinC pinC bldc driver + */ +void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC); + +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 4PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pin1A pin1A stepper driver + * @param pin1B pin1B stepper driver + * @param pin2A pin2A stepper driver + * @param pin2B pin2B stepper driver + */ +void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B); + +/** + * Configuring PWM frequency, resolution and alignment + * - BLDC driver - 6PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low - if applicable + * @param pinA_h pinA high-side bldc driver + * @param pinA_l pinA low-side bldc driver + * @param pinB_h pinA high-side bldc driver + * @param pinB_l pinA low-side bldc driver + * @param pinC_h pinA high-side bldc driver + * @param pinC_l pinA low-side bldc driver + * + * @return 0 if config good, -1 if failed + */ +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param pinA phase A hardware pin number + * @param pinB phase B hardware pin number + */ +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param pinA phase A hardware pin number + * @param pinB phase B hardware pin number + * @param pinC phase C hardware pin number + */ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC); + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 4PWM setting + * - hardware specific + * + * @param dc_1a duty cycle phase 1A [0, 1] + * @param dc_1b duty cycle phase 1B [0, 1] + * @param dc_2a duty cycle phase 2A [0, 1] + * @param dc_2b duty cycle phase 2B [0, 1] + * @param pin1A phase 1A hardware pin number + * @param pin1B phase 1B hardware pin number + * @param pin2A phase 2A hardware pin number + * @param pin2B phase 2B hardware pin number + */ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B); + + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 6PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low + * @param pinA_h phase A high-side hardware pin number + * @param pinA_l phase A low-side hardware pin number + * @param pinB_h phase B high-side hardware pin number + * @param pinB_l phase B low-side hardware pin number + * @param pinC_h phase C high-side hardware pin number + * @param pinC_l phase C low-side hardware pin number + * + */ +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l); + +#endif \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_hall/src/drivers/hardware_specific/stm32_mcu.cpp b/minimal_project_examples/stm32_bldc_hall/src/drivers/hardware_specific/stm32_mcu.cpp new file mode 100644 index 00000000..0dc83cf7 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_hall/src/drivers/hardware_specific/stm32_mcu.cpp @@ -0,0 +1,326 @@ + +#include "../hardware_api.h" + +#if defined(_STM32_DEF_) + +#define _PWM_RESOLUTION 12 // 12bit +#define _PWM_RANGE 4095.0// 2^12 -1 = 4095 +#define _PWM_FREQUENCY 25000 // 25khz +#define _PWM_FREQUENCY_MAX 50000 // 50khz + + +#define _HARDWARE_6PWM 1 +#define _SOFTWARE_6PWM 0 +#define _ERROR_6PWM -1 + + +// setting pwm to hardware pin - instead analogWrite() +void _setPwm(int ulPin, uint32_t value, int resolution) +{ + PinName pin = digitalPinToPinName(ulPin); + TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); + uint32_t index = get_timer_index(Instance); + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + + uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + HT->setCaptureCompare(channel, value, (TimerCompareFormat_t)resolution); +} + + +// init pin pwm +HardwareTimer* _initPinPWM(uint32_t PWM_freq, int ulPin) +{ + PinName pin = digitalPinToPinName(ulPin); + TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); + + uint32_t index = get_timer_index(Instance); + if (HardwareTimer_Handle[index] == NULL) { + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); + HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); + } + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, pin); + HT->setOverflow(PWM_freq, HERTZ_FORMAT); + HT->pause(); + HT->refresh(); + return HT; +} + + +// init high side pin +HardwareTimer* _initPinPWMHigh(uint32_t PWM_freq, int ulPin) +{ + return _initPinPWM(PWM_freq, ulPin); +} + +// init low side pin +HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) +{ + PinName pin = digitalPinToPinName(ulPin); + TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); + uint32_t index = get_timer_index(Instance); + + if (HardwareTimer_Handle[index] == NULL) { + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); + HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); + TIM_OC_InitTypeDef sConfigOC = TIM_OC_InitTypeDef(); + sConfigOC.OCMode = TIM_OCMODE_PWM2; + sConfigOC.Pulse = 100; + sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; + sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; + sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; + sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET; + sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; + uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel); + } + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM)); + HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, pin); + HT->setOverflow(PWM_freq, HERTZ_FORMAT); + HT->pause(); + HT->refresh(); + return HT; +} + + +// align the timers to end the init +void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3) +{ + HT1->pause(); + HT1->refresh(); + HT2->pause(); + HT2->refresh(); + HT3->pause(); + HT3->refresh(); + HT1->resume(); + HT2->resume(); + HT3->resume(); +} + +// align the timers to end the init +void _alignPWMTimers(HardwareTimer *HT1,HardwareTimer *HT2,HardwareTimer *HT3,HardwareTimer *HT4) +{ + HT1->pause(); + HT1->refresh(); + HT2->pause(); + HT2->refresh(); + HT3->pause(); + HT3->refresh(); + HT4->pause(); + HT4->refresh(); + HT1->resume(); + HT2->resume(); + HT3->resume(); + HT4->resume(); +} + +HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l) +{ + PinName uhPinName = digitalPinToPinName(pinA_h); + PinName ulPinName = digitalPinToPinName(pinA_l); + PinName vhPinName = digitalPinToPinName(pinB_h); + PinName vlPinName = digitalPinToPinName(pinB_l); + PinName whPinName = digitalPinToPinName(pinC_h); + PinName wlPinName = digitalPinToPinName(pinC_l); + + TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM); + + uint32_t index = get_timer_index(Instance); + + if (HardwareTimer_Handle[index] == NULL) { + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM)); + HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); + ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT); + } + HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + + HT->setMode(STM_PIN_CHANNEL(pinmap_function(uhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, uhPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(ulPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, ulPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(vhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vhPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(vlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vlPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(whPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, whPinName); + HT->setMode(STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, wlPinName); + + // dead time is set in nanoseconds + uint32_t dead_time_ns = (float)(1e9/PWM_freq)*dead_zone; + uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns); + LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! + LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N); + + HT->pause(); + HT->refresh(); + HT->resume(); + return HT; +} + + +// returns 0 if each pair of pwm channels has same channel +// returns 1 all the channels belong to the same timer - hardware inverted channels +// returns -1 if neither - avoid configuring - error!!! +int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + PinName nameAH = digitalPinToPinName(pinA_h); + PinName nameAL = digitalPinToPinName(pinA_l); + PinName nameBH = digitalPinToPinName(pinB_h); + PinName nameBL = digitalPinToPinName(pinB_l); + PinName nameCH = digitalPinToPinName(pinC_h); + PinName nameCL = digitalPinToPinName(pinC_l); + int tim1 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAH, PinMap_PWM)); + int tim2 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAL, PinMap_PWM)); + int tim3 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBH, PinMap_PWM)); + int tim4 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBL, PinMap_PWM)); + int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM)); + int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM)); + if(tim1 == tim2 && tim2==tim3 && tim3==tim4 && tim4==tim5 && tim5==tim6) + return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer + else if(tim1 == tim2 && tim3==tim4 && tim5==tim6) + return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer + else + return _ERROR_6PWM; // might be error avoid configuration +} + + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); + HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); + // allign the timers + _alignPWMTimers(HT1, HT2, HT2); +} + + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); + HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); + HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); + // allign the timers + _alignPWMTimers(HT1, HT2, HT3); +} + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 4PWM setting +// - hardware speciffic +void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + + HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); + HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); + HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); + HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinD); + // allign the timers + _alignPWMTimers(HT1, HT2, HT3, HT4); +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +//- hardware speciffic +void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ + // transform duty cycle from [0,1] to [0,4095] + _setPwm(pinA, _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(pinB, _PWM_RANGE*dc_b, _PWM_RESOLUTION); +} + +// function setting the pwm duty cycle to the hardware +// - BLDC motor - 3PWM setting +//- hardware speciffic +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ + // transform duty cycle from [0,1] to [0,4095] + _setPwm(pinA, _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(pinB, _PWM_RANGE*dc_b, _PWM_RESOLUTION); + _setPwm(pinC, _PWM_RANGE*dc_c, _PWM_RESOLUTION); +} + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 4PWM setting +//- hardware speciffic +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + // transform duty cycle from [0,1] to [0,4095] + _setPwm(pin1A, _PWM_RANGE*dc_1a, _PWM_RESOLUTION); + _setPwm(pin1B, _PWM_RANGE*dc_1b, _PWM_RESOLUTION); + _setPwm(pin2A, _PWM_RANGE*dc_2a, _PWM_RESOLUTION); + _setPwm(pin2B, _PWM_RANGE*dc_2b, _PWM_RESOLUTION); +} + + + + +// Configuring PWM frequency, resolution and alignment +// - BLDC driver - 6PWM setting +// - hardware specific +int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ + if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to |%0kHz max + // center-aligned frequency is uses two periods + pwm_frequency *=2; + + // find configuration + int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); + // configure accordingly + switch(config){ + case _ERROR_6PWM: + return -1; // fail + break; + case _HARDWARE_6PWM: + _initHardware6PWMInterface(pwm_frequency, dead_zone, pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); + break; + case _SOFTWARE_6PWM: + HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinA_h); + _initPinPWMLow(pwm_frequency, pinA_l); + HardwareTimer* HT2 = _initPinPWMHigh(pwm_frequency,pinB_h); + _initPinPWMLow(pwm_frequency, pinB_l); + HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency,pinC_h); + _initPinPWMLow(pwm_frequency, pinC_l); + _alignPWMTimers(HT1, HT2, HT3); + break; + } + return 0; // success +} + +// Function setting the duty cycle to the pwm pin (ex. analogWrite()) +// - BLDC driver - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + // find configuration + int config = _interfaceType(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); + // set pwm accordingly + switch(config){ + case _HARDWARE_6PWM: + _setPwm(pinA_h, _PWM_RANGE*dc_a, _PWM_RESOLUTION); + _setPwm(pinB_h, _PWM_RANGE*dc_b, _PWM_RESOLUTION); + _setPwm(pinC_h, _PWM_RANGE*dc_c, _PWM_RESOLUTION); + break; + case _SOFTWARE_6PWM: + _setPwm(pinA_l, _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinA_h, _constrain(dc_a - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinB_l, _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinB_h, _constrain(dc_b - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinC_l, _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinC_h, _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + break; + } +} +#endif \ No newline at end of file diff --git a/minimal_project_examples/stm32_bldc_hall/src/sensors/HallSensor.cpp b/minimal_project_examples/stm32_bldc_hall/src/sensors/HallSensor.cpp new file mode 100644 index 00000000..2c33a41e --- /dev/null +++ b/minimal_project_examples/stm32_bldc_hall/src/sensors/HallSensor.cpp @@ -0,0 +1,168 @@ +#include "HallSensor.h" + + +/* + HallSensor(int hallA, int hallB , int cpr, int index) + - hallA, hallB, hallC - HallSensor A, B and C pins + - pp - pole pairs +*/ +HallSensor::HallSensor(int _hallA, int _hallB, int _hallC, int _pp){ + + // hardware pins + pinA = _hallA; + pinB = _hallB; + pinC = _hallC; + + // hall has 6 segments per electrical revolution + cpr = _pp * 6; + + // extern pullup as default + pullup = Pullup::EXTERN; +} + +// HallSensor interrupt callback functions +// A channel +void HallSensor::handleA() { + A_active= digitalRead(pinA); + updateState(); +} +// B channel +void HallSensor::handleB() { + B_active = digitalRead(pinB); + updateState(); +} + +// C channel +void HallSensor::handleC() { + C_active = digitalRead(pinC); + updateState(); +} + +/** + * Updates the state and sector following an interrupt + */ +void HallSensor::updateState() { + long new_pulse_timestamp = _micros(); + hall_state = C_active + (B_active << 1) + (A_active << 2); + int8_t new_electric_sector = ELECTRIC_SECTORS[hall_state]; + static Direction old_direction; + if (new_electric_sector - electric_sector > 3) { + //underflow + direction = static_cast(natural_direction * -1); + electric_rotations += direction; + } else if (new_electric_sector - electric_sector < (-3)) { + //overflow + direction = static_cast(natural_direction); + electric_rotations += direction; + } else { + direction = (new_electric_sector > electric_sector)? static_cast(natural_direction) : static_cast(natural_direction * (-1)); + } + electric_sector = new_electric_sector; + if (direction == old_direction) { + // not oscilating or just changed direction + pulse_diff = new_pulse_timestamp - pulse_timestamp; + } else { + pulse_diff = 0; + } + + pulse_timestamp = new_pulse_timestamp; + total_interrupts++; + old_direction = direction; + if (onSectorChange != nullptr) onSectorChange(electric_sector); +} + +/** + * Optionally set a function callback to be fired when sector changes + * void onSectorChange(int sector) { + * ... // for debug or call driver directly? + * } + * sensor.attachSectorCallback(onSectorChange); + */ +void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { + onSectorChange = _onSectorChange; +} + +/* + Shaft angle calculation +*/ +float HallSensor::getAngle() { + return natural_direction * ((electric_rotations * 6 + electric_sector) / cpr) * _2PI; +} + +/* + Shaft velocity calculation + function using mixed time and frequency measurement technique +*/ +float HallSensor::getVelocity(){ + if (pulse_diff == 0 || ((_micros() - pulse_timestamp) > pulse_diff) ) { // last velocity isn't accurate if too old + return 0; + } else { + return direction * (_2PI / cpr) / (pulse_diff / 1000000.0); + } + +} + +// getter for index pin +// return -1 if no index +int HallSensor::needsAbsoluteZeroSearch(){ + return 0; +} + +int HallSensor::hasAbsoluteZero(){ + return 1; +} + +// set current angle as zero angle +// return the angle [rad] difference +float HallSensor::initRelativeZero(){ + + // nothing to do. The interrupts should have changed sector. + electric_rotations = 0; + return 0; + +} + +// set absolute zero angle as zero angle +// return the angle [rad] difference +float HallSensor::initAbsoluteZero(){ + + return -getAngle(); + +} + +// HallSensor initialisation of the hardware pins +// and calculation variables +void HallSensor::init(){ + + // HallSensor - check if pullup needed for your HallSensor + if(pullup == Pullup::INTERN){ + pinMode(pinA, INPUT_PULLUP); + pinMode(pinB, INPUT_PULLUP); + pinMode(pinC, INPUT_PULLUP); + }else{ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + pinMode(pinC, INPUT); + } + + // init hall_state + A_active= digitalRead(pinA); + B_active = digitalRead(pinB); + C_active = digitalRead(pinC); + updateState(); + + pulse_timestamp = _micros(); + +} + +// function enabling hardware interrupts for the callback provided +// if callback is not provided then the interrupt is not enabled +void HallSensor::enableInterrupts(void (*doA)(), void(*doB)(), void(*doC)()){ + // attach interrupt if functions provided + + // A, B and C callback + if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE); + if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE); + if(doC != nullptr) attachInterrupt(digitalPinToInterrupt(pinC), doC, CHANGE); +} + diff --git a/minimal_project_examples/stm32_bldc_hall/src/sensors/HallSensor.h b/minimal_project_examples/stm32_bldc_hall/src/sensors/HallSensor.h new file mode 100644 index 00000000..ad5673e3 --- /dev/null +++ b/minimal_project_examples/stm32_bldc_hall/src/sensors/HallSensor.h @@ -0,0 +1,116 @@ +#ifndef HALL_SENSOR_LIB_H +#define HALL_SENSOR_LIB_H + +#include "Arduino.h" +#include "../common/base_classes/Sensor.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + +// seq 1 > 5 > 4 > 6 > 2 > 3 > 1 000 001 010 011 100 101 110 111 +const int8_t ELECTRIC_SECTORS[8] = { -1, 0, 4, 5, 2, 1, 3 , -1 }; + +class HallSensor: public Sensor{ + public: + /** + HallSensor class constructor + @param encA HallSensor A pin + @param encB HallSensor B pin + @param encC HallSensor C pin + @param pp pole pairs (e.g hoverboard motor has 15pp and small gimbals often have 7pp) + @param index index pin number (optional input) + */ + HallSensor(int encA, int encB, int encC, int pp); + + /** HallSensor initialise pins */ + void init(); + /** + * function enabling hardware interrupts for the HallSensor channels with provided callback functions + * if callback is not provided then the interrupt is not enabled + * + * @param doA pointer to the A channel interrupt handler function + * @param doB pointer to the B channel interrupt handler function + * @param doIndex pointer to the Index channel interrupt handler function + * + */ + void enableInterrupts(void (*doA)() = nullptr, void(*doB)() = nullptr, void(*doC)() = nullptr); + + // HallSensor interrupt callback functions + /** A channel callback function */ + void handleA(); + /** B channel callback function */ + void handleB(); + /** C channel callback function */ + void handleC(); + + + // pins A and B + int pinA; //!< HallSensor hardware pin A + int pinB; //!< HallSensor hardware pin B + int pinC; //!< HallSensor hardware pin C + + // HallSensor configuration + Pullup pullup; //!< Configuration parameter internal or external pullups + float cpr;//!< HallSensor cpr number + + // Abstract functions of the Sensor class implementation + /** get current angle (rad) */ + float getAngle() override; + /** get current angular velocity (rad/s) */ + float getVelocity() override; + /** + * set current angle as zero angle + * return the angle [rad] difference + */ + float initRelativeZero() override; + /** + * set index angle as zero angle + * return the angle [rad] difference + */ + float initAbsoluteZero() override; + /** + * returns 0 if it has no index + * 0 - HallSensor without index + * 1 - HallSensor with index + */ + int hasAbsoluteZero() override; + /** + * returns 0 if it does need search for absolute zero + * 0 - HallSensor without index + * 1 - ecoder with index + */ + int needsAbsoluteZeroSearch() override; + + // whether last step was CW (+1) or CCW (-1) direction + Direction direction; + + void attachSectorCallback(void (*onSectorChange)(int a) = nullptr); + + // the current 3bit state of the hall sensors + volatile int8_t hall_state; + // the current sector of the sensor. Each sector is 60deg electrical + volatile int8_t electric_sector; + // the number of electric rotations + volatile long electric_rotations; + // this is sometimes useful to identify interrupt issues (e.g. weak or no pullup resulting in 1000s of interrupts) + volatile long total_interrupts; + + private: + + Direction decodeDirection(int oldState, int newState); + void updateState(); + + int zero_offset; + volatile long pulse_timestamp;//!< last impulse timestamp in us + volatile int A_active; //!< current active states of A channel + volatile int B_active; //!< current active states of B channel + volatile int C_active; //!< current active states of C channel + + // function pointer for on sector change call back + void (*onSectorChange)(int sector) = nullptr; + + volatile long pulse_diff; + +}; + + +#endif diff --git a/minimal_project_examples/stm32_bldc_hall/stm32_bldc_hall.ino b/minimal_project_examples/stm32_bldc_hall/stm32_bldc_hall.ino new file mode 100644 index 00000000..bd105e0a --- /dev/null +++ b/minimal_project_examples/stm32_bldc_hall/stm32_bldc_hall.ino @@ -0,0 +1,148 @@ +/** + * Comprehensive BLDC motor control example using Hall sensor + * + * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: + * - configure PID controller constants + * - change motion control loops + * - monitor motor variabels + * - set target values + * - check all the configuration values + * + * To check the config value just enter the command letter. + * For example: - to read velocity PI controller P gain run: P + * - to set velocity PI controller P gain to 1.2 run: P1.2 + * + * To change the target value just enter a number in the terminal: + * For example: - to change the target value to -0.1453 enter: -0.1453 + * - to get the current target value enter: V3 + * + * List of commands: + * - P: velocity PID controller P gain + * - I: velocity PID controller I gain + * - D: velocity PID controller D gain + * - R: velocity PID controller voltage ramp + * - F: velocity Low pass filter time constant + * - K: angle P controller P gain + * - N: angle P controller velocity limit + * - L: system voltage limit + * - C: control loop + * - 0: voltage + * - 1: velocity + * - 2: angle + * - V: get motor variables + * - 0: currently set voltage + * - 1: current velocity + * - 2: current angle + * - 3: current target value + * + */ +#include "src/BLDCMotor.h" +#include "src/sensors/HallSensor.h" +#include "src/drivers/BLDCDriver3PWM.h" + +// 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, B and C callbacks +void doA(){sensor.handleA();} +void doB(){sensor.handleB();} +void doC(){sensor.handleC();} +void setup() { + + // initialize encoder sensor hardware + sensor.init(); + sensor.enableInterrupts(doA, doB, doC); + // link the motor to the sensor + motor.linkSensor(&sensor); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link driver + motor.linkDriver(&driver); + + + // set control loop type to be used + motor.controller = ControlType::voltage; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.2; + motor.PID_velocity.I = 20; + motor.PID_velocity.D = 0; + // default voltage_power_supply + motor.voltage_limit = 12; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01; + + // angle loop controller + motor.P_angle.P = 20; + // angle loop velocity limit + motor.velocity_limit = 50; + + // use monitoring with serial for motor init + // monitoring port + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + // initialise motor + motor.init(); + // align encoder and start FOC + motor.initFOC(); + + // set the inital target value + motor.target = 2; + + + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + Serial.println("Motor commands sketch | Initial motion control > torque/voltage : target 2V."); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor.loopFOC(); + + // iterative function setting the outter loop target + // velocity, position or voltage + // if tatget not set in parameter uses motor.target variable + motor.move(); + + // user communication + motor.command(serialReceiveUserCommand()); +} + +// utility function enabling serial communication the user +String serialReceiveUserCommand() { + + // a string to hold incoming data + static String received_chars; + + String command = ""; + + while (Serial.available()) { + // get the new byte: + char inChar = (char)Serial.read(); + // add it to the string buffer: + received_chars += inChar; + + // end of user input + if (inChar == '\n') { + + // execute the user command + command = received_chars; + + // reset the command buffer + received_chars = ""; + } + } + return command; +} \ No newline at end of file diff --git a/src/ArduinoFOC.h b/src/ArduinoFOC.h deleted file mode 100644 index ecb06bd6..00000000 --- a/src/ArduinoFOC.h +++ /dev/null @@ -1,7 +0,0 @@ -#ifndef ArduinoFOC_h -#define ArduinoFOC_h - -#include "BLDCMotor.h" -#include "Encoder.h" - -#endif \ No newline at end of file diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp deleted file mode 100644 index f683d3de..00000000 --- a/src/BLDCMotor.cpp +++ /dev/null @@ -1,382 +0,0 @@ -#include "BLDCMotor.h" - - -/* - BLDCMotor( int phA, int phB, int phC, int pp, int encA, int encB , int cpr, int en) - - phA, phB, phC - motor A,B,C phase pwm pins - - pp - pole pair number - - encA, encB - encoder A and B pins - - cpr - counts per rotation number (cpm=ppm*4) - - enable pin - (optional input) -*/ -BLDCMotor::BLDCMotor(int phA, int phB, int phC, int pp, int en) -{ - // Pin intialization - pwmA = phA; - pwmB = phB; - pwmC = phC; - pole_pairs = pp; - - // enable_pin pin - enable_pin = en; - - // Power supply woltage - power_supply_voltage = DEF_POWER_SUPPLY; - - // Velocity loop config - // PI contoroller constant - PI_velocity.K = DEF_PI_VEL_K; - PI_velocity.Ti = DEF_PI_VEL_TI; - PI_velocity.timestamp = micros(); - PI_velocity.u_limit = -1; - - // Ultra slow velocity - // PI contoroller - PI_velocity_ultra_slow.K = DEF_PI_VEL_US_K; - PI_velocity_ultra_slow.Ti = DEF_PI_VEL_US_TI; - PI_velocity_ultra_slow.timestamp = micros(); - PI_velocity_ultra_slow.u_limit = -1; - - // position loop config - // P controller constant - P_angle.K = DEF_P_ANGLE_K; - // maximum angular velocity to be used for positioning - P_angle.velocity_limit = DEF_P_ANGLE_VEL_LIM; - - // driver deafault type - driver = DriverType::bipolar; -} - -// init hardware pins -void BLDCMotor::init() { - // PWM pins - pinMode(pwmA, OUTPUT); - pinMode(pwmB, OUTPUT); - pinMode(pwmC, OUTPUT); - pinMode(enable_pin, OUTPUT); - - // Increase PWM frequency to 32 kHz - // make silent - setPwmFrequency(pwmA); - setPwmFrequency(pwmB); - setPwmFrequency(pwmC); - - // check if u_limit configuration has been done. - // if not set it to the power_supply_voltage - if(PI_velocity.u_limit == -1) PI_velocity.u_limit = power_supply_voltage; - if(PI_velocity_ultra_slow.u_limit == -1) PI_velocity_ultra_slow.u_limit = power_supply_voltage; - - delay(500); - // enable motor - enable(); - delay(500); - -} - -/* - initialization function -*/ -void BLDCMotor::initFOC() { - // encoder alignment - delay(500); - alignEncoder(); - delay(500); -} - -/* - disable motor -*/ -void BLDCMotor::disable() -{ - // disable the driver - if enable_pin pin available - if (enable_pin) digitalWrite(enable_pin, LOW); - // set zero to PWM - setPwm(pwmA, 0); - setPwm(pwmB, 0); - setPwm(pwmC, 0); -} -/* - disable motor -*/ -void BLDCMotor::enable() -{ - // set zero to PWM - setPwm(pwmA, 0); - setPwm(pwmB, 0); - setPwm(pwmC, 0); - // enable_pin the driver - if enable_pin pin available - if (enable_pin) digitalWrite(enable_pin, HIGH); - -} - -void BLDCMotor::linkEncoder(Encoder* enc) { - encoder = enc; -} - - -/* - Encoder alignment to electrical 0 angle -*/ -void BLDCMotor::alignEncoder() { - setPhaseVoltage(12, -M_PI/2); - delay(1000); - encoder->setCounterZero(); - - - setPhaseVoltage(0, 0); -} - -/** - State calculation methods -*/ -// shaft angle calculation -float BLDCMotor::shaftAngle() { - return encoder->getAngle(); -} -// shaft velocity calculation -float BLDCMotor::shaftVelocity() { - return encoder->getVelocity(); -} -/* - Electrical angle calculation -*/ -float BLDCMotor::electricAngle(float shaftAngle) { - return normalizeAngle(shaftAngle * pole_pairs); -} -/* - Iterative function looping FOC algorithm, setting Uq on the Motor - The faster it can be run the better -*/ -void BLDCMotor::loopFOC() { - // voltage open loop loop - shaft_angle = shaftAngle(); - setPhaseVoltage(voltage_q, electricAngle(shaft_angle)); -} - -/* - Iterative funciton running outer loop of the FOC algorithm - Bahvior of this funciton is determined by the motor.controller variable - It runs either angle, veloctiy, velocity ultra slow or voltage loop - - needs to be called iteratively it is asynchronious function -*/ -void BLDCMotor::move(float target) { - // get angular velocity - shaft_velocity = shaftVelocity(); - switch (controller) { - case ControlType::velocity_ultra_slow: - // velocity set point - shaft_velocity_sp = target; - voltage_q = velocityUltraSlowPI(shaft_velocity_sp); - break; - case ControlType::angle: - // angle set point - // include angle loop - shaft_angle_sp = target; - shaft_velocity_sp = positionP( shaft_angle_sp - shaft_angle ); - voltage_q = velocityPI(shaft_velocity_sp - shaft_velocity); - break; - case ControlType::velocity: - // velocity set point - // inlcude velocity loop - shaft_velocity_sp = target; - voltage_q = velocityPI(shaft_velocity_sp - shaft_velocity); - break; - case ControlType::voltage: - voltage_q = target; - break; - } -} - - -/** - FOC methods -*/ -/* - Method using FOC to set Uq to the motor at the optimal angle - - for unipolar drivers - only positive values -*/ -void BLDCMotor::setPhaseVoltageUnipolar(double Uq, double angle_el) { - - // Uq sign compensation - float angle = Uq > 0 ? angle_el : normalizeAngle( angle_el + M_PI ); - // Park transform - Ualpha = abs(Uq) * cos(angle); - Ubeta = abs(Uq) * sin(angle); - - // determine the segment I, II, III - if ((angle >= 0) && (angle <= _120_D2R)) { - // section I - Ua = Ualpha + _1_SQRT3 * Ubeta; - Ub = _2_SQRT3 * Ubeta; - Uc = 0; - - } else if ((angle > _120_D2R) && (angle <= (2 * _120_D2R))) { - // section III - Ua = 0; - Ub = _1_SQRT3 * Ubeta - Ualpha; - Uc = -_1_SQRT3 * Ubeta - Ualpha; - - } else if ((angle > (2 * _120_D2R)) && (angle <= (3 * _120_D2R))) { - // section II - Ua = Ualpha - _1_SQRT3 * Ubeta; - Ub = 0; - Uc = - _2_SQRT3 * Ubeta; - } - - // set phase voltages - setPwm(pwmA, Ua); - setPwm(pwmB, Ub); - setPwm(pwmC, Uc); -} -/* - Method using FOC to set Uq to the motor at the optimal angle - - for bipolar drivers - posiitve and negative voltages -*/ -void BLDCMotor::setPhaseVoltageBipolar(double Uq, double angle_el) { - - // q component angle - float angle = angle_el + M_PI/2; - // Uq sign compensation - angle = Uq > 0 ? angle : normalizeAngle( angle + M_PI ); - - // Park transform - Ualpha = abs(Uq) * cos(angle); - Ubeta = abs(Uq) * sin(angle); - - // determine the segment I, II, III - // section I - Ua = Ualpha; - Ub = -0.5 * Ualpha + _SQRT3_2 * Ubeta; - Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta; - - // set phase voltages - setPwm(pwmA, Ua); - setPwm(pwmB, Ub); - setPwm(pwmC, Uc); -} - -/* - Method using FOC to set Uq to the motor at the optimal angle -*/ -void BLDCMotor::setPhaseVoltage(double Uq, double angle_el) { - switch (driver) { - case DriverType::bipolar : - // L6234 - setPhaseVoltageBipolar(Uq, angle_el); - break; - case DriverType::unipolar : - // HMBGC - setPhaseVoltageUnipolar(Uq, angle_el); - break; - } -} - - -/* - Set voltage to the pwm pin -*/ -void BLDCMotor::setPwm(int pinPwm, float U) { - int U_pwm = 0; - int U_max = power_supply_voltage; - // uniploar or bipolar FOC - switch (driver) { - case DriverType::bipolar : - // sets the voltage [-U_max,U_max] to pwm [0,255] - // - U_max you can set in header file - default 12V - // - support for L6234 drive - U_pwm = ((float)U + (float)U_max) / (2.0 * (float)U_max) * 255.0; - break; - case DriverType::unipolar : - // HMBGC - // sets the voltage [0,12V(U_max)] to pwm [0,255] - // - U_max you can set in header file - default 12V - // - support for HMBGC controller - U_pwm = 255.0 * (float)U / (float)U_max; - break; - } - // limit the values between 0 and 255; - U_pwm = U_pwm < 0 ? 0 : U_pwm; - U_pwm = U_pwm > 255 ? 255 : U_pwm; - - // write hardware pwm - analogWrite(pinPwm, U_pwm); -} - -/** - Utility funcitons -*/ -/* - normalizing radian angle to [0,2PI] -*/ -double BLDCMotor::normalizeAngle(double angle) -{ - double a = fmod(angle, 2 * M_PI); - return a >= 0 ? a : (a + 2 * M_PI); -} - - - - -/** - Motor control functions -*/ -float BLDCMotor::velocityPI(float ek) { - float Ts = (micros() - PI_velocity.timestamp) * 1e-6; - - // u(s) = Kr(1 + 1/(Ti.s)) - float uk = PI_velocity.uk_1; - uk += PI_velocity.K * (Ts / (2 * PI_velocity.Ti) + 1) * ek + PI_velocity.K * (Ts / (2 * PI_velocity.Ti) - 1) * PI_velocity.ek_1; - if (abs(uk) > PI_velocity.u_limit) uk = uk > 0 ? PI_velocity.u_limit : -PI_velocity.u_limit; - - PI_velocity.uk_1 = uk; - PI_velocity.ek_1 = ek; - PI_velocity.timestamp = micros(); - return uk; -} - -// PI controller for ultra slow velocity control -float BLDCMotor::velocityUltraSlowPI(float vel) { - float Ts = (micros() - PI_velocity_ultra_slow.timestamp) * 1e-6; - static float angle; - - angle += vel * Ts; - float ek = angle - shaft_angle; - - // u(s) = Kr(1 + 1/(Ti.s)) - float uk = PI_velocity_ultra_slow.uk_1; - uk += PI_velocity_ultra_slow.K * (Ts / (2 * PI_velocity_ultra_slow.Ti) + 1) * ek + PI_velocity_ultra_slow.K * (Ts / (2 * PI_velocity_ultra_slow.Ti) - 1) * PI_velocity_ultra_slow.ek_1; - if (abs(uk) > PI_velocity_ultra_slow.u_limit) uk = uk > 0 ? PI_velocity_ultra_slow.u_limit : -PI_velocity_ultra_slow.u_limit; - - PI_velocity_ultra_slow.uk_1 = uk; - PI_velocity_ultra_slow.ek_1 = ek; - PI_velocity_ultra_slow.timestamp = micros(); - return uk; -} -// P controller for position control loop -float BLDCMotor::positionP(float ek) { - float velk = P_angle.K * ek; - if (abs(velk) > P_angle.velocity_limit) velk = velk > 0 ? P_angle.velocity_limit : -P_angle.velocity_limit; - return velk; -} - - -/* - High PWM frequency -*/ -void setPwmFrequency(int pin) { - if (pin == 5 || pin == 6 || pin == 9 || pin == 10) { - if (pin == 5 || pin == 6) { - TCCR0B = ((TCCR0B & 0b11111000) | 0x01); - } else { - TCCR1B = ((TCCR1B & 0b11111000) | 0x01); - } - } - else if (pin == 3 || pin == 11) { - TCCR2B = ((TCCR2B & 0b11111000) | 0x01); - } -} - - - - - diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h deleted file mode 100644 index 8279a1e1..00000000 --- a/src/BLDCMotor.h +++ /dev/null @@ -1,152 +0,0 @@ -#ifndef BLDCMotor_h -#define BLDCMotor_h - -#include "Arduino.h" -#include "Encoder.h" - -// default configuration values -// power supply voltage -#define DEF_POWER_SUPPLY 12.0 -// velocity PI controller params -#define DEF_PI_VEL_K 1.0 -#define DEF_PI_VEL_TI 0.003 -// ultra slow velocity PI params -#define DEF_PI_VEL_US_K 120.0 -#define DEF_PI_VEL_US_TI 100.0 -// angle P params -#define DEF_P_ANGLE_K 20 -// angle velocity limit default -#define DEF_P_ANGLE_VEL_LIM 20 - -// sign funciton -#define sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) ) -// utility defines -#define _2_SQRT3 1.15470053838 -#define _1_SQRT3 0.57735026919 -#define _SQRT3_2 0.86602540378 -#define _SQRT2 1.41421356237 -#define _120_D2R 2.09439510239 - -// controller type configuration enum -enum ControlType{ - voltage, - velocity, - velocity_ultra_slow, - angle -}; - -// driver type configuration enum -enum DriverType{ - bipolar, // L6234 - unipolar // HMBGC -}; - -// P/PI controller strucutre -struct PI_s{ - float K; - float Ti; - long timestamp; - float uk_1, ek_1; - float u_limit; - float velocity_limit; -}; - -/** - BLDC motor class -*/ -class BLDCMotor -{ - public: - BLDCMotor(int phA,int phB,int phC,int pp, int en = 0); - // change driver state - void init(); - void disable(); - void enable(); - // connect encoder - void linkEncoder(Encoder* enc); - - // initilise FOC - void initFOC(); - // iterative method updating motor angles and velocity measurement - void loopFOC(); - // iterative control loop defined by controller - void move(float target); - - - // hardware variables - int pwmA; - int pwmB; - int pwmC; - int enable_pin; - int pole_pairs; - - // state variables - // current elelctrical angle - float elctric_angle; - // current motor angle - float shaft_angle; - // current motor velocity - float shaft_velocity; - // current target velocity - float shaft_velocity_sp; - // current target angle - float shaft_angle_sp; - // current voltage u_q set - float voltage_q; - - // Power supply woltage - float power_supply_voltage; - - // configuraion structures - DriverType driver; - ControlType controller; - PI_s PI_velocity; - PI_s PI_velocity_ultra_slow; - PI_s P_angle; - - // encoder link - Encoder* encoder; - - - private: - //Encoder alignment to electrical 0 angle - void alignEncoder(); - /** State calculation methods */ - //Shaft angle calculation - float shaftAngle(); - //Shaft velocity calculation - float shaftVelocity(); - - //Electrical angle calculation - float electricAngle(float shaftAngle); - //Set phase voltaget to pwm output - void setPwm(int pinPwm, float U); - - /** FOC methods */ - //Method using FOC to set Uq to the motor at the optimal angle - void setPhaseVoltage(double Uq, double angle_el); - void setPhaseVoltageUnipolar(double Uq, double angle_el); - void setPhaseVoltageBipolar(double Uq, double angle_el); - - /** Utility funcitons */ - //normalizing radian angle to [0,2PI] - double normalizeAngle(double angle); - //Reference low pass filter - float filterLP(float u); - - /** Motor control functions */ - float velocityPI(float ek); - float velocityUltraSlowPI(float ek); - float positionP(float ek); - - float Ua,Ub,Uc; - float Ualpha,Ubeta; -}; - - -/* - High PWM frequency -*/ -void setPwmFrequency(int pin); - -#endif diff --git a/src/Encoder.cpp b/src/Encoder.cpp deleted file mode 100644 index e67a0960..00000000 --- a/src/Encoder.cpp +++ /dev/null @@ -1,135 +0,0 @@ -#include "Encoder.h" - - -/* - Encoder(int encA, int encB , int cpr, int index) - - encA, encB - encoder A and B pins - - cpr - counts per rotation number (cpm=ppm*4) - - index pin - (optional input) -*/ - -Encoder::Encoder(int _encA, int _encB , float _ppr, int _index){ - - // Encoder measurement structure init - // hardware pins - pinA = _encA; - pinB = _encB; - // counter setup - pulse_counter = 0; - pulse_timestamp = 0; - cpr = 4.0*_ppr; - A_active = 0; - B_active = 0; - I_active = 0; - // index pin - index = _index; // its 0 if not used - - // velocity calculation varibles - prev_Th = 0; - pulse_per_second = 0; - prev_pulse_counter = 0; - prev_timestamp_us = micros(); - - // extern pullup as default - pullup = Pullup::EXTERN; -} - -// Encoder interrupt callback functions -// enabling CPR=4xPPR behaviour -// A channel -void Encoder::handleA() { - int A = digitalRead(pinA); - if ( A != A_active ) { - pulse_counter += (A_active == B_active) ? 1 : -1; - pulse_timestamp = micros(); - A_active = A; - } -} -// B channel -void Encoder::handleB() { - int B = digitalRead(pinB); - if ( B != B_active ) { - pulse_counter += (A_active != B_active) ? 1 : -1; - pulse_timestamp = micros(); - B_active = B; - } -} - -/* - Shaft angle calculation -*/ -float Encoder::getAngle(){ - return (pulse_counter) / ((float)cpr) * (2.0 * M_PI); -} -/* - Shaft velocity calculation - funciton using mixed time and frequency measurement technique -*/ -float Encoder::getVelocity(){ - // timestamp - long timestamp_us = micros(); - // sampling time calculation - float Ts = (timestamp_us - prev_timestamp_us) * 1e-6; - // time from last impulse - float Th = (timestamp_us - pulse_timestamp) * 1e-6; - long dN = pulse_counter - prev_pulse_counter; - - // Pulse per second calculation (Eq.3.) - // dN - impulses received - // Ts - sampling time - time in between function calls - // Th - time from last impulse - // Th_1 - time form last impulse of the previous call - // only increment if some impulses received - float dt = Ts + prev_Th - Th; - pulse_per_second = (dN != 0 && dt > Ts/2) ? dN / dt : pulse_per_second; - - // if more than 0.3 passed in between impulses - if ( Th > 0.1) pulse_per_second = 0; - - // velocity calculation - float velocity = pulse_per_second / ((float)cpr) * (2.0 * M_PI); - - // save variables for next pass - prev_timestamp_us = timestamp_us; - // save velocity calculation variables - prev_Th = Th; - prev_pulse_counter = pulse_counter; - return (velocity); -} - -// intialise counter to zero -void Encoder::setCounterZero(){ - pulse_counter = 0; -} - - -void Encoder::init(void (*doA)(), void(*doB)()){ - - // Encoder - check if pullup needed for your encoder - if(pullup == INTERN){ - pinMode(pinA, INPUT_PULLUP); - pinMode(pinB, INPUT_PULLUP); - }else{ - pinMode(pinA, INPUT); - pinMode(pinB, INPUT); - } - // if index used intialise it - if(index) pinMode(index,INPUT); - - // counter setup - pulse_counter = 0; - pulse_timestamp = micros(); - // velocity calculation varibles - prev_Th = 0; - pulse_per_second = 0; - prev_pulse_counter = 0; - prev_timestamp_us = micros(); - - - // attach interrupt if functions provided - if(doA != nullptr){ - // A callback and B callback - attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE); - attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE); - } -} diff --git a/src/Encoder.h b/src/Encoder.h deleted file mode 100644 index 07dfa273..00000000 --- a/src/Encoder.h +++ /dev/null @@ -1,63 +0,0 @@ -#ifndef ENCODER_LIB_H -#define ENCODER_LIB_H - -#include "Arduino.h" - - -// Pullup configuation structure -enum Pullup{ - INTERN, - EXTERN -}; - -class Encoder{ - public: - /* - Encoder(int encA, int encB , int cpr, int index) - - encA, encB - encoder A and B pins - - ppr - impulses per rotation (cpr=ppr*4) - - index pin - (optional input) - */ - Encoder(int encA, int encB , float ppr, int index = 0); - - // encoder initialise pins - void init(void (*doA)() = nullptr, void(*doB)() = nullptr); - - // Encoder interrupt callback functions - // enabling CPR=4xPPR behaviour - // A channel - void handleA(); - // B channel - void handleB(); - - // encoder getters - // shaft velocity getter - float getVelocity(); - float getAngle(); - - // setter for counter to zero - void setCounterZero(); - - // pins A and B - int pinA, pinB; // encoder hardware pins - // index pin - int index; - // encoder pullup type - Pullup pullup; - - - private: - long pulse_counter; // current pulse counter - long pulse_timestamp; // last impulse timestamp in us - float cpr; // impulse cpr - int A_active, B_active; // current active states of A and B line - int I_active; // index active - - // velocity calculation varibles - float prev_Th, pulse_per_second; - long prev_pulse_counter, prev_timestamp_us; - -}; - - -#endif