v1.4实现机器人一起踢乒乓球功能，使用maixcam，离线运行。
This commit is contained in:
myperworld committed
1 parent
9d3aa2684b
commit
ce8dc19088
1854 files changed
+14918
-23
No files matched your search
@@ -0,0 +1,21 @@
|
||||
MIT License
|
||||
|
||||
Copyright (c) 2020 Antun Skuric
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all
|
||||
copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
SOFTWARE.
|
||||
@@ -0,0 +1,218 @@
|
||||
# Arduino Simple Field Oriented Control (FOC) library
|
||||
|
||||
|
||||
|
||||
[](https://opensource.org/licenses/MIT)
|
||||
[](https://www.ardu-badge.com/badge/Simple%20FOC.svg)
|
||||
[](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d)
|
||||
|
||||
We live in very exciting times 😃! BLDC motors are entering the hobby community more and more and many great projects have already emerged leveraging their far superior dynamics and power capabilities. BLDC motors have numerous advantages over regular DC motors but they have one big disadvantage, the complexity of control. Even though it has become relatively easy to design and manufacture PCBs and create our own hardware solutions for driving BLDC motors the proper low-cost solutions are yet to come. One of the reasons for this is the apparent complexity of writing the BLDC driving algorithms, Field oriented control (FOC) being an example of one of the most efficient ones.
|
||||
The solutions that can be found on-line are almost exclusively very specific for certain hardware configuration and the microcontroller architecture used.
|
||||
Additionally, most of the efforts at this moment are still channeled towards the high-power applications of the BLDC motors and proper low-cost and low-power FOC supporting boards are very hard to find today and even may not exist. <br>
|
||||
Therefore this is an attempt to:
|
||||
- 🎯 Demystify FOC algorithm and make a robust but simple Arduino library: [Arduino *SimpleFOClibrary*](https://docs.simplefoc.com/arduino_simplefoc_library_showcase)
|
||||
- <i>Support as many <b>motor + sensor + driver + mcu</b> combinations out there</i>
|
||||
- 🎯 Develop a modular FOC supporting BLDC driver boards:
|
||||
- *Low-power* gimbal driver (<5Amps) : [*Arduino Simple**FOC**Shield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase).
|
||||
- ***NEW*** 📢: *Medium-power* BLDC driver (<30Amps): [Arduino <span class="simple">Simple<b>FOC</b>PowerShield</span> ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield).
|
||||
- See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller)
|
||||
|
||||
|
||||
|
||||
<blockquote class="info">
|
||||
<p class="heading">NEW RELEASE 📢: <span class="simple">Simple<span class="foc">FOC</span>library</span> v2.2.1 <a href="https://github.com/simplefoc/Arduino-FOC/releases/tag/v2.2.1">see release</a></p>
|
||||
<ul>
|
||||
<li>Sensor class init bugfix <a href="https://github.com/simplefoc/Arduino-FOC/issues/121">#121</a></li>
|
||||
<li>Voltage/current limit handling bugs <a href="https://github.com/simplefoc/Arduino-FOC/issues/118">#118</a></li>
|
||||
<li>Added the new motion control interface to the commander <a href="https://docs.simplefoc.com/commander_target">see docs</a>
|
||||
<ul>
|
||||
<li>New target setting - possible to set the position, velocity and torque target at once</li>
|
||||
<li>Separated the motion control interface from full motor callback - only motion control and torque control type, enable disable and target setting</li>
|
||||
</ul>
|
||||
</li>
|
||||
<li>New MCU support <a href="https://docs.simplefoc.com/microcontrollers">see docs</a>
|
||||
<ul>
|
||||
<li>NRF52 series mcus support by <a href="https://github.com/Polyphe">@Polyphe</a></li>
|
||||
<li><b>esp32 arduino package transfer to v2.0.1+</b> - helpful <a href="https://github.com/simplefoc/Arduino-FOC/pull/92/149">PR#149</a> by <a href="https://github.com/samguns">samguns</a></li>
|
||||
<li>Initial support for esp32s2 and esp32s3 - separation of the esp32 mcpwm and led implementation</li>
|
||||
</ul>
|
||||
</li>
|
||||
<li>Generic sensor class - to implement a new sensor only implement one function <a href="https://docs.simplefoc.com/generic_sensor">see docs</a></li>
|
||||
</ul>
|
||||
</blockquote>
|
||||
|
||||
## Arduino *SimpleFOClibrary* v2.2
|
||||
|
||||
<p align="">
|
||||
<a href="https://youtu.be/Y5kLeqTc6Zk">
|
||||
<img src="https://docs.simplefoc.com/extras/Images/youtube.png" height="320px">
|
||||
</a>
|
||||
</p>
|
||||
|
||||
This video demonstrates the *Simple**FOC**library* basic usage, electronic connections and shows its capabilities.
|
||||
|
||||
|
||||
### Features
|
||||
- **Arduino compatible**:
|
||||
- Arduino library code
|
||||
- Arduino Library Manager integration
|
||||
- **Open-Source**: Full code and documentation available on github
|
||||
- **Easy to setup and configure**:
|
||||
- Easy hardware configuration
|
||||
- Easy [tuning the control loops](https://docs.simplefoc.com/motion_control)
|
||||
- **Modular**:
|
||||
- Supports as many [sensors, BLDC motors and driver boards](https://docs.simplefoc.com/supported_hardware) as possible
|
||||
- Supports multiple [MCU architectures](https://docs.simplefoc.com/microcontrollers):
|
||||
- Arduino: UNO, MEGA, any board with ATMega328 chips
|
||||
- STM32 boards: [Nucleo](https://www.st.com/en/evaluation-tools/stm32-nucleo-boards.html), [Bluepill](https://stm32-base.org/boards/STM32F103C8T6-Blue-Pill.html) ...
|
||||
- ESP32
|
||||
- Teensy boards
|
||||
- **Plug & play**: Arduino <span class="simple">Simple<span class="foc">FOC</span>Shield</span>
|
||||
|
||||
<p align=""> <img src="https://docs.simplefoc.com/extras/Images/uno_l6234.jpg" height="170px"> <img src="https://docs.simplefoc.com/extras/Images/hmbgc_v22.jpg" height="170px"> <img src="https://docs.simplefoc.com/extras/Images/foc_shield_v13.jpg" height="170px"></p>
|
||||
|
||||
## Arduino *SimpleFOCShield* v2.0.4
|
||||
<p align="">
|
||||
<a href="https://youtu.be/G5pbo0C6ujE">
|
||||
<img src="https://docs.simplefoc.com/extras/Images/foc_shield_video.jpg" height="320px">
|
||||
</a>
|
||||
</p>
|
||||
|
||||
### Features
|
||||
- **Plug & play**: In combination with Arduino *Simple**FOC**library* - [github](https://github.com/simplefoc/Arduino-FOC)
|
||||
- **Low-cost**: Price of €15 - [Check the pricing](https://www.simplefoc.com/shop)
|
||||
- **In-line current sensing**: Up to 3Amps/5Amps bidirectional
|
||||
- configurable: 3.3Amps - 3.3V adc, 5Amps - 5V adc
|
||||
- **Integrated 8V regulator**:
|
||||
- Enable/disable by soldering pads
|
||||
- **Max power 120W** - max current 5A, power-supply 12-24V
|
||||
- Designed for Gimbal motors with the internal resistance >10 Ωs.
|
||||
- **Stackable**: running 2 motors in the same time
|
||||
- **Encoder/Hall sensors interface**: Integrated 3.3kΩ pullups (configurable)
|
||||
- **I2C interface**: Integrated 4.7kΩ pullups (configurable)
|
||||
- **Configurable pinout**: Hardware configuration - soldering connections
|
||||
- **Arduino headers**: Arduino UNO, Arduino MEGA, STM32 Nucleo boards...
|
||||
- **Open Source**: Fully available fabrication files - [how to make it yourself](https://docs.simplefoc.com/arduino_simplefoc_shield_fabrication)
|
||||
|
||||
<p align=""><img src="https://simplefoc.com/assets/img/v2.jpg" height="180px"> <img src="https://simplefoc.com/assets/img/v1.jpg" height="180px"> <img src="https://docs.simplefoc.com/extras/Images/simple_foc_shield_v13_small.gif" height="180x"></p>
|
||||
|
||||
|
||||
|
||||
## Getting Started
|
||||
Depending on if you want to use this library as the plug and play Arduino library or you want to get insight in the algorithm and make changes there are two ways to install this code.
|
||||
|
||||
- Full library installation [Docs](https://docs.simplefoc.com/library_download)
|
||||
- Minimal project builder [Docs](https://docs.simplefoc.com/minimal_download)
|
||||
|
||||
### Arduino *SimpleFOClibrary* installation to Arduino IDE
|
||||
#### Arduino Library Manager
|
||||
The simplest way to get hold of the library is directly by using Arduino IDE and its integrated Library Manager.
|
||||
- Open Arduino IDE and start Arduino Library Manager by clicking: `Tools > Manage Libraries...`.
|
||||
- Search for `Simple FOC` library and install the latest version.
|
||||
- Reopen Arduino IDE and you should have the library examples in `File > Examples > Simple FOC`.
|
||||
|
||||
#### Using Github website
|
||||
- Go to the [github repository](https://github.com/simplefoc/Arduino-FOC)
|
||||
- Click first on `Clone or Download > Download ZIP`.
|
||||
- Unzip it and place it in `Arduino Libraries` folder. Windows: `Documents > Arduino > libraries`.
|
||||
- Reopen Arduino IDE and you should have the library examples in `File > Examples > Simple FOC`.
|
||||
|
||||
#### Using terminal
|
||||
- Open terminal and run
|
||||
```sh
|
||||
cd #Arduino libraries folder
|
||||
git clone https://github.com/simplefoc/Arduino-FOC.git
|
||||
```
|
||||
- Reopen Arduino IDE and you should have the library examples in `File > Examples > Simple FOC`.
|
||||
|
||||
### *SimpleFOClibrary* minimal project builder
|
||||
|
||||
For those willing to experiment and to modify the code I suggest using the minimal project builder [minimal branch](https://github.com/simplefoc/Arduino-FOC/tree/minimal).
|
||||
> This code is completely independent and you can run it as any other Arduino Sketch without the need for any libraries.
|
||||
|
||||
All you need to do is:
|
||||
- Go to [minimal branch](https://github.com/simplefoc/Arduino-FOC/tree/minimal)
|
||||
- Follow the tutorial in the README file and choose only the library files that are necessary for your application.
|
||||
|
||||
## Arduino code example
|
||||
This is a simple Arduino code example implementing the velocity control program of a BLDC motor with encoder.
|
||||
|
||||
NOTE: This program uses all the default control parameters.
|
||||
|
||||
```cpp
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// BLDCMotor( pole_pairs )
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
// BLDCDriver( pin_pwmA, pin_pwmB, pin_pwmC, enable (optional) )
|
||||
BLDCDriver3PWM driver = 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 = MotionControlType::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);
|
||||
}
|
||||
```
|
||||
You can find more details in the [SimpleFOC documentation](https://docs.simplefoc.com/).
|
||||
|
||||
## Example projects
|
||||
Here are some of the *Simple**FOC**library* and *Simple**FOC**Shield* application examples.
|
||||
<p align="center">
|
||||
<a href="https://youtu.be/Ih-izQyXJCI">
|
||||
<img src="https://docs.simplefoc.com/extras/Images/youtube_pendulum.png" height="200px" >
|
||||
</a>
|
||||
<a href="https://youtu.be/xTlv1rPEqv4">
|
||||
<img src="https://docs.simplefoc.com/extras/Images/youtube_haptic.png" height="200px" >
|
||||
</a>
|
||||
<a href="https://youtu.be/RI4nNMF608I">
|
||||
<img src="https://docs.simplefoc.com/extras/Images/youtube_drv8302.png" height="200px" >
|
||||
</a>
|
||||
<a href="https://youtu.be/zcb86TRxTxc">
|
||||
<img src="https://docs.simplefoc.com/extras/Images/youtube_stepper.png" height="200px" >
|
||||
</a>
|
||||
</p>
|
||||
|
||||
|
||||
## Documentation
|
||||
Find out more information about the Arduino SimpleFOC project in [docs website](https://docs.simplefoc.com/)
|
||||
|
||||
|
||||
## Arduino FOC repo structure
|
||||
Branch | Description | Status
|
||||
------------ | ------------- | ------------
|
||||
[master](https://github.com/simplefoc/Arduino-FOC) | Stable and tested library version | 
|
||||
[dev](https://github.com/simplefoc/Arduino-FOC/tree/dev) | Development library version | 
|
||||
[minimal](https://github.com/simplefoc/Arduino-FOC/tree/minimal) | Minimal Arduino example with integrated library | 
|
||||
+112
@@ -0,0 +1,112 @@
|
||||
/**
|
||||
* B-G431B-ESC1 position motion control example with encoder
|
||||
*
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// Motor instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver6PWM driver = BLDCDriver6PWM(PHASE_UH, PHASE_UL, PHASE_VH, PHASE_VL, PHASE_WH, PHASE_WL);
|
||||
LowsideCurrentSense currentSense = LowsideCurrentSense(0.003, -64.0/7.0, OP1_OUT, OP2_OUT, OP3_OUT);
|
||||
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder = Encoder(HALL2, HALL3, 2048, HALL1);
|
||||
|
||||
// Interrupt routine intialisation
|
||||
// channel A and B callbacks
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
void doIndex(){encoder.handleIndex();}
|
||||
|
||||
// angle set point variable
|
||||
float target_angle = 0;
|
||||
// instantiate the commander
|
||||
Commander command = Commander(Serial);
|
||||
void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }
|
||||
|
||||
void 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 the motor and the driver
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// current sensing
|
||||
currentSense.init();
|
||||
// no need for aligning
|
||||
currentSense.skip_align = true;
|
||||
motor.linkCurrentSense(¤tSense);
|
||||
|
||||
// aligning voltage [V]
|
||||
motor.voltage_sensor_align = 3;
|
||||
// index search velocity [rad/s]
|
||||
motor.velocity_index_search = 3;
|
||||
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::velocity;
|
||||
|
||||
// contoller configuration
|
||||
// default parameters in defaults.h
|
||||
|
||||
// velocity PI controller parameters
|
||||
motor.PID_velocity.P = 0.2;
|
||||
motor.PID_velocity.I = 20;
|
||||
// default voltage_power_supply
|
||||
motor.voltage_limit = 6;
|
||||
// jerk control using voltage voltage ramp
|
||||
// default value is 300 volts per sec ~ 0.3V per millisecond
|
||||
motor.PID_velocity.output_ramp = 1000;
|
||||
|
||||
// velocity low pass filtering time constant
|
||||
motor.LPF_velocity.Tf = 0.01;
|
||||
|
||||
// angle P controller
|
||||
motor.P_angle.P = 20;
|
||||
// maximal velocity of the position control
|
||||
motor.velocity_limit = 4;
|
||||
|
||||
|
||||
// use monitoring with serial
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align encoder and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// add target command T
|
||||
command.add('T', doTarget, "target angle");
|
||||
|
||||
Serial.println(F("Motor ready."));
|
||||
Serial.println(F("Set the target angle using serial terminal:"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
// angle set point variable
|
||||
float target_angle = 0;
|
||||
|
||||
void loop() {
|
||||
// main FOC algorithm function
|
||||
|
||||
// Motion control function
|
||||
motor.move(target_angle);
|
||||
|
||||
// function intended to be used with serial plotter to monitor motor variables
|
||||
// significantly slowing the execution down!!!!
|
||||
// motor.monitor();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+1
@@ -0,0 +1 @@
|
||||
-DHAL_OPAMP_MODULE_ENABLED
|
||||
+115
@@ -0,0 +1,115 @@
|
||||
/**
|
||||
*
|
||||
* STM32 Bluepill position motion control example with encoder
|
||||
*
|
||||
* The same example can be ran with any STM32 board - just make sure that put right pin numbers.
|
||||
*
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// Motor instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
// BLDCDriver3PWM(IN1, IN2, IN3, enable(optional))
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(PB6, PB7, PB8, PB5);
|
||||
// BLDCDriver6PWM(IN1_H, IN1_L, IN2_H, IN2_L, IN3_H, IN3_L, enable(optional))
|
||||
//BLDCDriver6PWM driver = BLDCDriver6PWM(PA8, PB13, PA9, PB14, PA10, PB15, PB12);
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder = Encoder(PA8, PA9, 8192, PA10);
|
||||
|
||||
// Interrupt routine intialisation
|
||||
// channel A and B callbacks
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
void doI(){encoder.handleIndex();}
|
||||
|
||||
|
||||
// angle set point variable
|
||||
float target_angle = 0;
|
||||
// instantiate the commander
|
||||
Commander command = Commander(Serial);
|
||||
void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }
|
||||
|
||||
|
||||
void setup() {
|
||||
|
||||
// initialize encoder sensor hardware
|
||||
encoder.init();
|
||||
encoder.enableInterrupts(doA, doB, doI);
|
||||
// link the motor to the sensor
|
||||
motor.linkSensor(&encoder);
|
||||
|
||||
// driver config
|
||||
// power supply voltage [V]
|
||||
driver.voltage_power_supply = 12;
|
||||
driver.init();
|
||||
// link the motor and the driver
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// aligning voltage [V]
|
||||
motor.voltage_sensor_align = 3;
|
||||
// index search velocity [rad/s]
|
||||
motor.velocity_index_search = 3;
|
||||
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::velocity;
|
||||
|
||||
// contoller configuration
|
||||
// default parameters in defaults.h
|
||||
|
||||
// velocity PI controller parameters
|
||||
motor.PID_velocity.P = 0.2f;
|
||||
motor.PID_velocity.I = 20;
|
||||
// default voltage_power_supply
|
||||
motor.voltage_limit = 6;
|
||||
// jerk control using voltage voltage ramp
|
||||
// default value is 300 volts per sec ~ 0.3V per millisecond
|
||||
motor.PID_velocity.output_ramp = 1000;
|
||||
|
||||
// velocity low pass filtering time constant
|
||||
motor.LPF_velocity.Tf = 0.01f;
|
||||
|
||||
// angle P controller
|
||||
motor.P_angle.P = 20;
|
||||
// maximal velocity of the position control
|
||||
motor.velocity_limit = 4;
|
||||
|
||||
|
||||
// use monitoring with serial
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align encoder and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// add target command T
|
||||
command.add('T', doTarget, "target angle");
|
||||
|
||||
Serial.println(F("Motor ready."));
|
||||
Serial.println(F("Set the target angle using serial terminal:"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// main FOC algorithm function
|
||||
// the faster you run this function the better
|
||||
// Arduino UNO loop ~1kHz
|
||||
// Bluepill loop ~10kHz
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
// velocity, position or voltage (defined in motor.controller)
|
||||
// this function can be run at much lower frequency than loopFOC() function
|
||||
// You can also use motor.move() and set the motor.target in the code
|
||||
motor.move(target_angle);
|
||||
|
||||
// function intended to be used with serial plotter to monitor motor variables
|
||||
// significantly slowing the execution down!!!!
|
||||
// motor.monitor();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+115
@@ -0,0 +1,115 @@
|
||||
/**
|
||||
*
|
||||
* STM32 Bluepill position motion control example with magnetic sensor
|
||||
*
|
||||
* The same example can be ran with any STM32 board - just make sure that put right pin numbers.
|
||||
*
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// SPI Magnetic sensor instance (AS5047U example)
|
||||
// MISO PA7
|
||||
// MOSI PA6
|
||||
// SCK PA5
|
||||
MagneticSensorSPI sensor = MagneticSensorSPI(PA4, 14, 0x3FFF);
|
||||
|
||||
// I2C Magnetic sensor instance (AS5600 example)
|
||||
// make sure to use the pull-ups!!
|
||||
// SDA PB7
|
||||
// SCL PB6
|
||||
//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4);
|
||||
|
||||
// Motor instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
// BLDCDriver3PWM(IN1, IN2, IN3, enable(optional))
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(PB6, PB7, PB8, PB5);
|
||||
// BLDCDriver6PWM(IN1_H, IN1_L, IN2_H, IN2_L, IN3_H, IN3_L, enable(optional))
|
||||
//BLDCDriver6PWM driver = BLDCDriver6PWM(PA8, PB13, PA9, PB14, PA10, PB15, PB12);
|
||||
|
||||
|
||||
// angle set point variable
|
||||
float target_angle = 0;
|
||||
// instantiate the commander
|
||||
Commander command = Commander(Serial);
|
||||
void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }
|
||||
|
||||
|
||||
void 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 the motor and the driver
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// choose FOC modulation (optional)
|
||||
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
|
||||
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::angle;
|
||||
|
||||
// contoller configuration
|
||||
// default parameters in defaults.h
|
||||
|
||||
// velocity PI controller parameters
|
||||
motor.PID_velocity.P = 0.2f;
|
||||
motor.PID_velocity.I = 20;
|
||||
// maximal voltage to be set to the motor
|
||||
motor.voltage_limit = 6;
|
||||
|
||||
// velocity low pass filtering time constant
|
||||
// the lower the less filtered
|
||||
motor.LPF_velocity.Tf = 0.01f;
|
||||
|
||||
// angle P controller
|
||||
motor.P_angle.P = 20;
|
||||
// maximal velocity of the position control
|
||||
motor.velocity_limit = 40;
|
||||
|
||||
// use monitoring with serial
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align sensor and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// add target command T
|
||||
command.add('T', doTarget, "target angle");
|
||||
|
||||
Serial.println(F("Motor ready."));
|
||||
Serial.println(F("Set the target angle using serial terminal:"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
|
||||
// main FOC algorithm function
|
||||
// the faster you run this function the better
|
||||
// Arduino UNO loop ~1kHz
|
||||
// Bluepill loop ~10kHz
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
// velocity, position or voltage (defined in motor.controller)
|
||||
// this function can be run at much lower frequency than loopFOC() function
|
||||
// You can also use motor.move() and set the motor.target in the code
|
||||
motor.move(target_angle);
|
||||
|
||||
|
||||
// function intended to be used with serial plotter to monitor motor variables
|
||||
// significantly slowing the execution down!!!!
|
||||
// motor.monitor();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+130
@@ -0,0 +1,130 @@
|
||||
/**
|
||||
* Comprehensive BLDC motor control example using encoder and the DRV8302 board
|
||||
*
|
||||
* Using serial terminal user can send motor commands and configure the motor and FOC in real-time:
|
||||
* - configure PID controller constants
|
||||
* - change motion control loops
|
||||
* - monitor motor variabels
|
||||
* - set target values
|
||||
* - check all the configuration values
|
||||
*
|
||||
* check the https://docs.simplefoc.com for full list of motor commands
|
||||
*
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// DRV8302 pins connections
|
||||
// don't forget to connect the common ground pin
|
||||
#define INH_A 9
|
||||
#define INH_B 10
|
||||
#define INH_C 11
|
||||
|
||||
#define EN_GATE 7
|
||||
#define M_PWM A1
|
||||
#define M_OC A2
|
||||
#define OC_ADJ A3
|
||||
|
||||
// Motor instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(INH_A, INH_B, INH_C, EN_GATE);
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder = Encoder(2, 3, 8192);
|
||||
|
||||
// Interrupt routine intialisation
|
||||
// channel A and B callbacks
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
|
||||
|
||||
// commander interface
|
||||
Commander command = Commander(Serial);
|
||||
void onMotor(char* cmd){ command.motor(&motor, cmd); }
|
||||
|
||||
void setup() {
|
||||
|
||||
// initialize encoder sensor hardware
|
||||
encoder.init();
|
||||
encoder.enableInterrupts(doA, doB);
|
||||
// link the motor to the sensor
|
||||
motor.linkSensor(&encoder);
|
||||
|
||||
// DRV8302 specific code
|
||||
// M_OC - enable overcurrent protection
|
||||
pinMode(M_OC,OUTPUT);
|
||||
digitalWrite(M_OC,LOW);
|
||||
// M_PWM - enable 3pwm mode
|
||||
pinMode(M_PWM,OUTPUT);
|
||||
digitalWrite(M_PWM,HIGH);
|
||||
// OD_ADJ - set the maximum overcurrent limit possible
|
||||
// Better option would be to use voltage divisor to set exact value
|
||||
pinMode(OC_ADJ,OUTPUT);
|
||||
digitalWrite(OC_ADJ,HIGH);
|
||||
|
||||
|
||||
// driver config
|
||||
// power supply voltage [V]
|
||||
driver.voltage_power_supply = 12;
|
||||
driver.init();
|
||||
// link the motor and the driver
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
|
||||
// choose FOC modulation
|
||||
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
|
||||
|
||||
// set control loop type to be used
|
||||
motor.controller = MotionControlType::torque;
|
||||
|
||||
// contoller configuration based on the controll type
|
||||
motor.PID_velocity.P = 0.2f;
|
||||
motor.PID_velocity.I = 20;
|
||||
// default voltage_power_supply
|
||||
motor.voltage_limit = 12;
|
||||
|
||||
// velocity low pass filtering time constant
|
||||
motor.LPF_velocity.Tf = 0.01f;
|
||||
|
||||
// angle loop controller
|
||||
motor.P_angle.P = 20;
|
||||
// angle loop velocity limit
|
||||
motor.velocity_limit = 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;
|
||||
|
||||
// define the motor id
|
||||
command.add('A', onMotor, "motor");
|
||||
|
||||
Serial.println(F("Full control example: "));
|
||||
Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n "));
|
||||
Serial.println(F("Initial motion control loop is voltage loop."));
|
||||
Serial.println(F("Initial target voltage 2V."));
|
||||
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
|
||||
void loop() {
|
||||
// iterative setting FOC phase voltage
|
||||
motor.loopFOC();
|
||||
|
||||
// iterative function setting the outter loop target
|
||||
// velocity, position or voltage
|
||||
// if tatget not set in parameter uses motor.target variable
|
||||
motor.move();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+131
@@ -0,0 +1,131 @@
|
||||
/**
|
||||
* Comprehensive BLDC motor control example using encoder and the DRV8302 board
|
||||
*
|
||||
* Using serial terminal user can send motor commands and configure the motor and FOC in real-time:
|
||||
* - configure PID controller constants
|
||||
* - change motion control loops
|
||||
* - monitor motor variabels
|
||||
* - set target values
|
||||
* - check all the configuration values
|
||||
*
|
||||
* check the https://docs.simplefoc.com for full list of motor commands
|
||||
*
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// DRV8302 pins connections
|
||||
// don't forget to connect the common ground pin
|
||||
#define INH_A 3
|
||||
#define INH_B 5
|
||||
#define INH_C 9
|
||||
#define INL_A 11
|
||||
#define INL_B 6
|
||||
#define INL_C 10
|
||||
|
||||
#define EN_GATE 7
|
||||
#define M_PWM A1
|
||||
#define M_OC A2
|
||||
#define OC_ADJ A3
|
||||
|
||||
// Motor instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver6PWM driver = BLDCDriver6PWM(INH_A,INH_A, INH_B,INH_B, INH_C,INL_C, EN_GATE);
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder = Encoder(2, 3, 8192);
|
||||
|
||||
// Interrupt routine intialisation
|
||||
// channel A and B callbacks
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
|
||||
|
||||
// commander interface
|
||||
Commander command = Commander(Serial);
|
||||
void onMotor(char* cmd){ command.motor(&motor, cmd); }
|
||||
|
||||
void setup() {
|
||||
|
||||
// initialize encoder sensor hardware
|
||||
encoder.init();
|
||||
encoder.enableInterrupts(doA, doB);
|
||||
// link the motor to the sensor
|
||||
motor.linkSensor(&encoder);
|
||||
|
||||
// DRV8302 specific code
|
||||
// M_OC - enable overcurrent protection
|
||||
pinMode(M_OC,OUTPUT);
|
||||
digitalWrite(M_OC,LOW);
|
||||
// M_PWM - disable 3pwm mode
|
||||
pinMode(M_PWM,OUTPUT);
|
||||
digitalWrite(M_PWM, LOW);
|
||||
// OD_ADJ - set the maximum overcurrent limit possible
|
||||
// Better option would be to use voltage divisor to set exact value
|
||||
pinMode(OC_ADJ,OUTPUT);
|
||||
digitalWrite(OC_ADJ,HIGH);
|
||||
|
||||
// driver config
|
||||
// power supply voltage [V]
|
||||
driver.voltage_power_supply = 12;
|
||||
driver.init();
|
||||
// link the motor and the driver
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// choose FOC modulation
|
||||
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
|
||||
|
||||
// set control loop type to be used
|
||||
motor.controller = MotionControlType::torque;
|
||||
|
||||
// contoller configuration based on the controll type
|
||||
motor.PID_velocity.P = 0.2f;
|
||||
motor.PID_velocity.I = 20;
|
||||
// default voltage_power_supply
|
||||
motor.voltage_limit = 12;
|
||||
|
||||
// velocity low pass filtering time constant
|
||||
motor.LPF_velocity.Tf = 0.01f;
|
||||
|
||||
// angle loop controller
|
||||
motor.P_angle.P = 20;
|
||||
// angle loop velocity limit
|
||||
motor.velocity_limit = 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;
|
||||
|
||||
// define the motor id
|
||||
command.add('A', onMotor, "motor");
|
||||
|
||||
Serial.println(F("Full control example: "));
|
||||
Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n "));
|
||||
Serial.println(F("Initial motion control loop is voltage loop."));
|
||||
Serial.println(F("Initial target voltage 2V."));
|
||||
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
|
||||
void loop() {
|
||||
// iterative setting FOC phase voltage
|
||||
motor.loopFOC();
|
||||
|
||||
// iterative function setting the outter loop target
|
||||
// velocity, position or voltage
|
||||
// if tatget not set in parameter uses motor.target variable
|
||||
motor.move();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+164
@@ -0,0 +1,164 @@
|
||||
/**
|
||||
* Comprehensive BLDC motor control example using encoder and the DRV8302 board
|
||||
*
|
||||
* Using serial terminal user can send motor commands and configure the motor and FOC in real-time:
|
||||
* - configure PID controller constants
|
||||
* - change motion control loops
|
||||
* - monitor motor variabels
|
||||
* - set target values
|
||||
* - check all the configuration values
|
||||
*
|
||||
* check the https://docs.simplefoc.com for full list of motor commands
|
||||
*
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// DRV8302 pins connections
|
||||
// don't forget to connect the common ground pin
|
||||
#define INH_A 21
|
||||
#define INH_B 19
|
||||
#define INH_C 18
|
||||
|
||||
#define EN_GATE 5
|
||||
#define M_PWM 25
|
||||
#define M_OC 26
|
||||
#define OC_ADJ 12
|
||||
#define OC_GAIN 14
|
||||
|
||||
#define IOUTA 34
|
||||
#define IOUTB 35
|
||||
#define IOUTC 32
|
||||
|
||||
// Motor instance
|
||||
BLDCMotor motor = BLDCMotor(7);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(INH_A, INH_B, INH_C, EN_GATE);
|
||||
|
||||
// DRV8302 board has 0.005Ohm shunt resistors and the gain of 12.22 V/V
|
||||
LowsideCurrentSense cs = LowsideCurrentSense(0.005f, 12.22f, IOUTA, IOUTB, IOUTC);
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder = Encoder(22, 23, 500);
|
||||
|
||||
// Interrupt routine intialisation
|
||||
// channel A and B callbacks
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
|
||||
|
||||
// commander interface
|
||||
Commander command = Commander(Serial);
|
||||
void onMotor(char* cmd){ command.motor(&motor, cmd); }
|
||||
|
||||
void setup() {
|
||||
|
||||
// initialize encoder sensor hardware
|
||||
encoder.init();
|
||||
encoder.enableInterrupts(doA, doB);
|
||||
// link the motor to the sensor
|
||||
motor.linkSensor(&encoder);
|
||||
|
||||
// DRV8302 specific code
|
||||
// M_OC - enable overcurrent protection
|
||||
pinMode(M_OC,OUTPUT);
|
||||
digitalWrite(M_OC,LOW);
|
||||
// M_PWM - enable 3pwm mode
|
||||
pinMode(M_PWM,OUTPUT);
|
||||
digitalWrite(M_PWM,HIGH);
|
||||
// OD_ADJ - set the maximum overcurrent limit possible
|
||||
// Better option would be to use voltage divisor to set exact value
|
||||
pinMode(OC_ADJ,OUTPUT);
|
||||
digitalWrite(OC_ADJ,HIGH);
|
||||
pinMode(OC_GAIN,OUTPUT);
|
||||
digitalWrite(OC_GAIN,LOW);
|
||||
|
||||
|
||||
// driver config
|
||||
// power supply voltage [V]
|
||||
driver.voltage_power_supply = 12;
|
||||
driver.pwm_frequency = 15000;
|
||||
driver.init();
|
||||
// link the motor and the driver
|
||||
motor.linkDriver(&driver);
|
||||
motor.voltage_sensor_align = 0.5;
|
||||
|
||||
// control loop type and torque mode
|
||||
motor.torque_controller = TorqueControlType::voltage;
|
||||
motor.controller = MotionControlType::torque;
|
||||
motor.motion_downsample = 0.0;
|
||||
|
||||
// velocity loop PID
|
||||
motor.PID_velocity.P = 0.2;
|
||||
motor.PID_velocity.I = 5.0;
|
||||
// Low pass filtering time constant
|
||||
motor.LPF_velocity.Tf = 0.02;
|
||||
// angle loop PID
|
||||
motor.P_angle.P = 20.0;
|
||||
// Low pass filtering time constant
|
||||
motor.LPF_angle.Tf = 0.0;
|
||||
// current q loop PID
|
||||
motor.PID_current_q.P = 3.0;
|
||||
motor.PID_current_q.I = 100.0;
|
||||
// Low pass filtering time constant
|
||||
motor.LPF_current_q.Tf = 0.02;
|
||||
// current d loop PID
|
||||
motor.PID_current_d.P = 3.0;
|
||||
motor.PID_current_d.I = 100.0;
|
||||
// Low pass filtering time constant
|
||||
motor.LPF_current_d.Tf = 0.02;
|
||||
|
||||
// Limits
|
||||
motor.velocity_limit = 100.0; // 100 rad/s velocity limit
|
||||
motor.voltage_limit = 12.0; // 12 Volt limit
|
||||
motor.current_limit = 2.0; // 2 Amp current limit
|
||||
|
||||
|
||||
// use monitoring with serial for motor init
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; // monitor the two currents d and q
|
||||
motor.monitor_downsample = 1000;
|
||||
|
||||
// initialise motor
|
||||
motor.init();
|
||||
|
||||
cs.init();
|
||||
cs.driverSync(&driver);
|
||||
// driver 8302 has inverted gains on all channels
|
||||
cs.gain_a *=-1;
|
||||
cs.gain_b *=-1;
|
||||
cs.gain_c *=-1;
|
||||
motor.linkCurrentSense(&cs);
|
||||
|
||||
// align encoder and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// set the inital target value
|
||||
motor.target = 0;
|
||||
|
||||
// define the motor id
|
||||
command.add('M', onMotor, "motor");
|
||||
|
||||
Serial.println(F("Full control example: "));
|
||||
Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n "));
|
||||
Serial.println(F("Initial motion control loop is voltage loop."));
|
||||
Serial.println(F("Initial target voltage 2V."));
|
||||
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
|
||||
void loop() {
|
||||
// iterative setting FOC phase voltage
|
||||
motor.loopFOC();
|
||||
|
||||
// iterative function setting the outter loop target
|
||||
motor.move();
|
||||
|
||||
// monitoring the state variables
|
||||
motor.monitor();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+107
@@ -0,0 +1,107 @@
|
||||
/**
|
||||
* ESP32 position motion control example with encoder
|
||||
*
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// Motor instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27, 7);
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder = Encoder(4, 2, 1024);
|
||||
|
||||
// Interrupt routine intialisation
|
||||
// channel A and B callbacks
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
|
||||
// angle set point variable
|
||||
float target_angle = 0;
|
||||
// instantiate the commander
|
||||
Commander command = Commander(Serial);
|
||||
void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }
|
||||
|
||||
void 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 the motor and the driver
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// aligning voltage [V]
|
||||
motor.voltage_sensor_align = 3;
|
||||
// index search velocity [rad/s]
|
||||
motor.velocity_index_search = 3;
|
||||
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::velocity;
|
||||
|
||||
// contoller configuration
|
||||
// default parameters in defaults.h
|
||||
|
||||
// velocity PI controller parameters
|
||||
motor.PID_velocity.P = 0.2f;
|
||||
motor.PID_velocity.I = 20;
|
||||
// default voltage_power_supply
|
||||
motor.voltage_limit = 6;
|
||||
// jerk control using voltage voltage ramp
|
||||
// default value is 300 volts per sec ~ 0.3V per millisecond
|
||||
motor.PID_velocity.output_ramp = 1000;
|
||||
|
||||
// velocity low pass filtering time constant
|
||||
motor.LPF_velocity.Tf = 0.01f;
|
||||
|
||||
// angle P controller
|
||||
motor.P_angle.P = 20;
|
||||
// maximal velocity of the position control
|
||||
motor.velocity_limit = 4;
|
||||
|
||||
|
||||
// use monitoring with serial
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align encoder and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// add target command T
|
||||
command.add('T', doTarget, "target angle");
|
||||
|
||||
Serial.println(F("Motor ready."));
|
||||
Serial.println(F("Set the target angle using serial terminal:"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// main FOC algorithm function
|
||||
// the faster you run this function the better
|
||||
// Arduino UNO loop ~1kHz
|
||||
// Bluepill loop ~10kHz
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
// velocity, position or voltage (defined in motor.controller)
|
||||
// this function can be run at much lower frequency than loopFOC() function
|
||||
// You can also use motor.move() and set the motor.target in the code
|
||||
motor.move(target_angle);
|
||||
|
||||
// function intended to be used with serial plotter to monitor motor variables
|
||||
// significantly slowing the execution down!!!!
|
||||
// motor.monitor();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+112
@@ -0,0 +1,112 @@
|
||||
/**
|
||||
* ESP32 position motion control example with magnetic sensor
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// SPI Magnetic sensor instance (AS5047U example)
|
||||
// MISO 12
|
||||
// MOSI 9
|
||||
// SCK 14
|
||||
// magnetic sensor instance - SPI
|
||||
MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 15);
|
||||
|
||||
// I2C Magnetic sensor instance (AS5600 example)
|
||||
// make sure to use the pull-ups!!
|
||||
// SDA 21
|
||||
// SCL 22
|
||||
// magnetic sensor instance - I2C
|
||||
//MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
|
||||
|
||||
// Analog output Magnetic sensor instance (AS5600)
|
||||
// MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
|
||||
|
||||
// Motor instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27, 7);
|
||||
|
||||
|
||||
// angle set point variable
|
||||
float target_angle = 0;
|
||||
// instantiate the commander
|
||||
Commander command = Commander(Serial);
|
||||
void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }
|
||||
|
||||
void 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 the motor and the driver
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// choose FOC modulation (optional)
|
||||
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
|
||||
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::angle;
|
||||
|
||||
// contoller configuration
|
||||
// default parameters in defaults.h
|
||||
|
||||
// velocity PI controller parameters
|
||||
motor.PID_velocity.P = 0.2f;
|
||||
motor.PID_velocity.I = 20;
|
||||
// maximal voltage to be set to the motor
|
||||
motor.voltage_limit = 6;
|
||||
|
||||
// velocity low pass filtering time constant
|
||||
// the lower the less filtered
|
||||
motor.LPF_velocity.Tf = 0.01f;
|
||||
|
||||
// angle P controller
|
||||
motor.P_angle.P = 20;
|
||||
// maximal velocity of the position control
|
||||
motor.velocity_limit = 40;
|
||||
|
||||
// use monitoring with serial
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align sensor and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// add target command T
|
||||
command.add('T', doTarget, "target angle");
|
||||
|
||||
Serial.println(F("Motor ready."));
|
||||
Serial.println(F("Set the target angle using serial terminal:"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
|
||||
// main FOC algorithm function
|
||||
// the faster you run this function the better
|
||||
// Arduino UNO loop ~1kHz
|
||||
// Bluepill loop ~10kHz
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
// velocity, position or voltage (defined in motor.controller)
|
||||
// this function can be run at much lower frequency than loopFOC() function
|
||||
// You can also use motor.move() and set the motor.target in the code
|
||||
motor.move(target_angle);
|
||||
|
||||
|
||||
// function intended to be used with serial plotter to monitor motor variables
|
||||
// significantly slowing the execution down!!!!
|
||||
// motor.monitor();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+127
@@ -0,0 +1,127 @@
|
||||
/**
|
||||
*
|
||||
* HMBGC position motion control example with encoder
|
||||
*
|
||||
* - Motor is connected the MOT1 connector (MOT1 9,10,11; MOT2 3,5,6)
|
||||
* - Encoder is connected to A0 and A1
|
||||
*
|
||||
* This board doesn't have any interrupt pins so we need to run all the encoder channels with the software interrupt library
|
||||
* - For this example we use: PciManager library : https://github.com/prampec/arduino-pcimanager
|
||||
*
|
||||
* See docs.simplefoc.com for more info.
|
||||
*
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
// software interrupt library
|
||||
#include <PciManager.h>
|
||||
#include <PciListenerImp.h>
|
||||
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11);
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder = Encoder(A0, A1, 2048);
|
||||
|
||||
// Interrupt routine intialisation
|
||||
// channel A and B callbacks
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
|
||||
// encoder interrupt init
|
||||
PciListenerImp listenerA(encoder.pinA, doA);
|
||||
PciListenerImp listenerB(encoder.pinB, doB);
|
||||
|
||||
|
||||
// angle set point variable
|
||||
float target_angle = 0;
|
||||
// instantiate the commander
|
||||
Commander command = Commander(Serial);
|
||||
void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }
|
||||
|
||||
void setup() {
|
||||
|
||||
// initialise encoder hardware
|
||||
encoder.init();
|
||||
// interrupt initialization
|
||||
PciManager.registerListener(&listenerA);
|
||||
PciManager.registerListener(&listenerB);
|
||||
// link the motor to the sensor
|
||||
motor.linkSensor(&encoder);
|
||||
|
||||
// driver config
|
||||
// power supply voltage [V]
|
||||
driver.voltage_power_supply = 12;
|
||||
driver.init();
|
||||
// link the motor and the driver
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// aligning voltage [V]
|
||||
motor.voltage_sensor_align = 3;
|
||||
// index search velocity [rad/s]
|
||||
motor.velocity_index_search = 3;
|
||||
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::angle;
|
||||
|
||||
// contoller configuration
|
||||
// default parameters in defaults.h
|
||||
|
||||
// velocity PI controller parameters
|
||||
motor.PID_velocity.P = 0.2f;
|
||||
motor.PID_velocity.I = 20;
|
||||
// default voltage_power_supply
|
||||
motor.voltage_limit = 6;
|
||||
// jerk control using voltage voltage ramp
|
||||
// default value is 300 volts per sec ~ 0.3V per millisecond
|
||||
motor.PID_velocity.output_ramp = 1000;
|
||||
|
||||
// velocity low pass filtering time constant
|
||||
motor.LPF_velocity.Tf = 0.01f;
|
||||
|
||||
// angle P controller
|
||||
motor.P_angle.P = 20;
|
||||
// maximal velocity of the position control
|
||||
motor.velocity_limit = 4;
|
||||
|
||||
|
||||
// use monitoring with serial
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align encoder and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// add target command T
|
||||
command.add('T', doTarget, "target angle");
|
||||
|
||||
Serial.println(F("Motor ready."));
|
||||
Serial.println(F("Set the target angle using serial terminal:"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
|
||||
void loop() {
|
||||
// main FOC algorithm function
|
||||
// the faster you run this function the better
|
||||
// Arduino UNO loop ~1kHz
|
||||
// Bluepill loop ~10kHz
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
// velocity, position or voltage (defined in motor.controller)
|
||||
// this function can be run at much lower frequency than loopFOC() function
|
||||
// You can also use motor.move() and set the motor.target in the code
|
||||
motor.move(target_angle);
|
||||
|
||||
// function intended to be used with serial plotter to monitor motor variables
|
||||
// significantly slowing the execution down!!!!
|
||||
// motor.monitor();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+110
@@ -0,0 +1,110 @@
|
||||
/**
|
||||
*
|
||||
* HMBGC torque control example using voltage control loop.
|
||||
*
|
||||
* - Motor is connected the MOT1 connector (MOT1 9,10,11; MOT2 3,5,6)
|
||||
* - Encoder is connected to A0 and A1
|
||||
*
|
||||
* Most of the low-end BLDC driver boards doesn't have current measurement therefore SimpleFOC offers
|
||||
* you a way to control motor torque by setting the voltage to the motor instead hte current.
|
||||
*
|
||||
* This makes the BLDC motor effectively a DC motor, and you can use it in a same way. position motion control example with encoder
|
||||
*
|
||||
* NOTE:
|
||||
* > HMBGC doesn't have any interrupt pins so we need to run all the encoder channels with the software interrupt library
|
||||
* > - For this example we use: PciManager library : https://github.com/prampec/arduino-pcimanager
|
||||
*
|
||||
* See docs.simplefoc.com for more info.
|
||||
*
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
// software interrupt library
|
||||
#include <PciManager.h>
|
||||
#include <PciListenerImp.h>
|
||||
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11);
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder = Encoder(A0, A1, 8192);
|
||||
|
||||
// Interrupt routine intialisation
|
||||
// channel A and B callbacks
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
|
||||
// encoder interrupt init
|
||||
PciListenerImp listenerA(encoder.pinA, doA);
|
||||
PciListenerImp listenerB(encoder.pinB, doB);
|
||||
|
||||
|
||||
// voltage set point variable
|
||||
float target_voltage = 2;
|
||||
// instantiate the commander
|
||||
Commander command = Commander(Serial);
|
||||
void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); }
|
||||
|
||||
void setup() {
|
||||
|
||||
// initialize encoder sensor hardware
|
||||
encoder.init();
|
||||
// interrupt initialization
|
||||
PciManager.registerListener(&listenerA);
|
||||
PciManager.registerListener(&listenerB);
|
||||
// link the motor to the sensor
|
||||
motor.linkSensor(&encoder);
|
||||
|
||||
// driver config
|
||||
// power supply voltage [V]
|
||||
driver.voltage_power_supply = 12;
|
||||
driver.init();
|
||||
// link the motor and the driver
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// choose FOC modulation (optional)
|
||||
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
|
||||
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::torque;
|
||||
|
||||
// use monitoring with serial for motor init
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
|
||||
// use monitoring with serial
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align sensor and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// add target command T
|
||||
command.add('T', doTarget, "target voltage");
|
||||
|
||||
Serial.println(F("Motor ready."));
|
||||
Serial.println(F("Set the target voltage using serial terminal:"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
|
||||
// main FOC algorithm function
|
||||
// the faster you run this function the better
|
||||
// Arduino UNO loop ~1kHz
|
||||
// Bluepill loop ~10kHz
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
// velocity, position or voltage (defined in motor.controller)
|
||||
// this function can be run at much lower frequency than loopFOC() function
|
||||
// You can also use motor.move() and set the motor.target in the code
|
||||
motor.move(target_voltage);
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+62
@@ -0,0 +1,62 @@
|
||||
|
||||
# SAMD Support
|
||||
|
||||
SimpleFOC supports many SAMD21 MCUs, really any SAMD21 supported by Arduino core should work.
|
||||
|
||||
## Pin assignments
|
||||
|
||||
The SAMD chips have some very powerful PWM features, but do not have flexible pin assignments.
|
||||
|
||||
You should be able to use *most* (but not all!), pin combinations for attaching your motor's PWM pins. Please ignore the board descriptions and pinout diagrammes regarding PWM-pins on SAMD boards. They are pretty much all incorrect to varying degrees of awfulness.
|
||||
|
||||
On SAMD we use TCC and TC timer peripherals (built into the SAMD chip) to control the PWM. Depending on the chip there are various timer units, whose PWM outputs are attached to various different pins, and it is all very complicated. Luckily SimpleFOC sets it all up automatically *if* there is a compatible configuration for those pins.
|
||||
|
||||
Not all timers are created equal. The TCC timers are pretty awesome for PWM motor control, while the TC timers are just ok for the job. So to get best performance, you want to use just TCC timer pins if you can.
|
||||
|
||||
By enabling
|
||||
|
||||
```
|
||||
#define SIMPLEFOC_SAMD_DEBUG
|
||||
```
|
||||
|
||||
in drivers/hardware_specific/samd_mcu.cpp<br>
|
||||
you will see a table of pin assignments printed on the serial console, as well as the timers SimpleFOC was able to find and configure on the pins you specified. You can use this to optimize your choice of pins if you want.
|
||||
|
||||
You can configure up to 12 pins for PWM motor control, i.e. 6x 2-PWM motors, 4x 3-PWM motors, 3x 4-PWM motors or 2x 6-PWM motors.
|
||||
|
||||
## PWM control modes
|
||||
|
||||
All modes (3-PWM, 6-PWM, Stepper 2-PWM and Stepper 4-PWM) are supported.
|
||||
|
||||
For 2-, 3- amd 4- PWM, any valid pin-combinations can be used. If you stick to TCC timers rather than using TC timers, then you'll get getter PWM waveforms. If you use pins which are all on the same TCC unit, you'll get the best result, with the PWM signals all perfectly aligned as well.
|
||||
|
||||
For 6-PWM, the situation is much more complicated:<br>
|
||||
TC timers cannot be used for 6-PWM, only TCC timers.
|
||||
|
||||
For Hardware Dead-Time insertion, you must use H and L pins for one phase from the same TCC unit, and on the same channel, but using complementary WOs (Waveform Outputs, i.e. PWM output pins). Check the table to find pins on the same channel (like TCC0-0) but complementary WOs (like TCC0-0[0] and TCC0-0[4] or TCC1-0[0] and TCC1-0[2]).
|
||||
|
||||
For Software Dead-Time insertion, you must use the same TCC and different channels for the H and L pins of the same phase.
|
||||
|
||||
Note: in all of the above note that you *cannot* set the timers or WOs used - they are fixed, and determined by the pins you selected. SimpleFOC will find the best combination of timers given the pins, trying to use TCC timers before TC, and trying to keep things on the same timers as much as possible. If you configure multiple motors, it will take into account the pins already assigned to other motors.
|
||||
So it is matter of choosing the right pins, nothing else.
|
||||
|
||||
Note also: Unfortunately you can't set the PWM frequency. It is currently fixed at 24KHz. This is a tradeoff between limiting PWM resolution vs
|
||||
increasing frequency, and also due to keeping the pin assignemts flexible, which would not be possible if we ran the timers at different rates.
|
||||
|
||||
## Status
|
||||
|
||||
Currently, SAMD21 is supported, and SAMD51 is unsupported. SAMD51 support is in progress.
|
||||
|
||||
Boards tested:
|
||||
|
||||
* Arduino Nano 33 IoT
|
||||
* Arduino MKR1000
|
||||
* Arduino MKR1010 Wifi
|
||||
* Seeduino XIAO
|
||||
* Feather M0 Basic
|
||||
|
||||
Environments tested:
|
||||
|
||||
* Arduino IDE
|
||||
* Arduino Pro IDE
|
||||
* Sloeber
|
||||
+64
@@ -0,0 +1,64 @@
|
||||
|
||||
// show the infos for SAMD pin assignment on serial console
|
||||
// set this #define SIMPLEFOC_SAMD_DEBUG in drivers/hardware_specific/samd21_mcu.h
|
||||
|
||||
|
||||
#include "Arduino.h"
|
||||
#include <Wire.h>
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// this is for an AS5048B absolute magnetic encoder on I2C address 0x41
|
||||
MagneticSensorI2C sensor = MagneticSensorI2C(0x41, 14, 0xFE, 8);
|
||||
|
||||
// small BLDC gimbal motor, 7 pole-pairs
|
||||
BLDCMotor motor = BLDCMotor(7);
|
||||
// 3-PWM driving on pins 6, 5 and 8 - these are all on the same timer unit (TCC0), but different channels
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(6,5,8);
|
||||
|
||||
// velocity set point variable
|
||||
float target_velocity = 2.0f;
|
||||
// instantiate the commander
|
||||
Commander command = Commander(SerialUSB);
|
||||
void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); }
|
||||
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
delay(1000);
|
||||
Serial.println("Initializing...");
|
||||
|
||||
sensor.init();
|
||||
Wire.setClock(400000);
|
||||
motor.linkSensor(&sensor);
|
||||
driver.voltage_power_supply = 9;
|
||||
driver.init();
|
||||
motor.linkDriver(&driver);
|
||||
motor.controller = MotionControlType::velocity;
|
||||
motor.PID_velocity.P = 0.2f;
|
||||
motor.PID_velocity.I = 20;
|
||||
motor.PID_velocity.D = 0.001f;
|
||||
motor.PID_velocity.output_ramp = 1000;
|
||||
motor.LPF_velocity.Tf = 0.01f;
|
||||
motor.voltage_limit = 9;
|
||||
//motor.P_angle.P = 20;
|
||||
motor.init();
|
||||
motor.initFOC();
|
||||
|
||||
// add target command T
|
||||
command.add('T', doTarget, "target velocity");
|
||||
|
||||
Serial.println(F("Motor ready."));
|
||||
Serial.println(F("Set the target velocity using serial terminal:"));
|
||||
delay(100);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void loop() {
|
||||
// Serial.print("Sensor: ");
|
||||
// Serial.println(sensor.getAngle());
|
||||
motor.loopFOC();
|
||||
motor.move(target_velocity);
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+100
@@ -0,0 +1,100 @@
|
||||
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(7);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8, 4, 7);
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder = Encoder(10, 11, 500);
|
||||
// channel A and B callbacks
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
|
||||
// inline current sensor instance
|
||||
InlineCurrentSense current_sense = InlineCurrentSense(0.001, 50.0, A0, A1);
|
||||
|
||||
// commander communication instance
|
||||
Commander command = Commander(Serial);
|
||||
// void doMotor(char* cmd){ command.motor(&motor, cmd); }
|
||||
void doTarget(char* cmd){ command.scalar(&motor.target, cmd); }
|
||||
|
||||
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 = 20;
|
||||
driver.init();
|
||||
// link driver
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
motor.voltage_sensor_align = 1;
|
||||
// set control loop type to be used
|
||||
motor.torque_controller = TorqueControlType::foc_current;
|
||||
motor.controller = MotionControlType::torque;
|
||||
|
||||
// contoller configuration based on the controll type
|
||||
motor.PID_velocity.P = 0.05f;
|
||||
motor.PID_velocity.I = 1;
|
||||
motor.PID_velocity.D = 0;
|
||||
// default voltage_power_supply
|
||||
motor.voltage_limit = 12;
|
||||
|
||||
// velocity low pass filtering time constant
|
||||
motor.LPF_velocity.Tf = 0.01f;
|
||||
|
||||
// angle loop controller
|
||||
motor.P_angle.P = 20;
|
||||
// angle loop velocity limit
|
||||
motor.velocity_limit = 20;
|
||||
|
||||
// use monitoring with serial for motor init
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
motor.monitor_downsample = 0; // disable intially
|
||||
motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle
|
||||
|
||||
// current sense init and linking
|
||||
current_sense.init();
|
||||
motor.linkCurrentSense(¤t_sense);
|
||||
|
||||
// initialise motor
|
||||
motor.init();
|
||||
// align encoder and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// set the inital target value
|
||||
motor.target = 0;
|
||||
|
||||
// subscribe motor to the commander
|
||||
// command.add('M', doMotor, "motor");
|
||||
command.add('T', doTarget, "target");
|
||||
|
||||
// Run user commands to configure and the motor (find the full command list in docs.simplefoc.com)
|
||||
Serial.println("Motor ready.");
|
||||
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
|
||||
void loop() {
|
||||
// iterative setting FOC phase voltage
|
||||
motor.loopFOC();
|
||||
|
||||
// iterative function setting the outter loop target
|
||||
motor.move();
|
||||
|
||||
// motor monitoring
|
||||
motor.monitor();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+118
@@ -0,0 +1,118 @@
|
||||
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor1 = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver1 = BLDCDriver3PWM(9, 5, 6, 8);
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor2 = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver2 = BLDCDriver3PWM(3, 10, 11, 7);
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder1 = Encoder(2, A3, 500);
|
||||
// channel A and B callbacks
|
||||
void doA1(){encoder1.handleA();}
|
||||
void doB1(){encoder1.handleB();}
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder2 = Encoder(A1, A2, 500);
|
||||
// channel A and B callbacks
|
||||
void doA2(){encoder2.handleA();}
|
||||
void doB2(){encoder2.handleB();}
|
||||
|
||||
// commander communication instance
|
||||
Commander command = Commander(Serial);
|
||||
void doMotor1(char* cmd){ command.motor(&motor1, cmd); }
|
||||
void doMotor2(char* cmd){ command.motor(&motor2, cmd); }
|
||||
|
||||
void setup() {
|
||||
|
||||
// initialize encoder sensor hardware
|
||||
encoder1.init();
|
||||
encoder1.enableInterrupts(doA1, doB1);
|
||||
// initialize encoder sensor hardware
|
||||
encoder2.init();
|
||||
encoder2.enableInterrupts(doA2, doB2);
|
||||
// link the motor to the sensor
|
||||
motor1.linkSensor(&encoder1);
|
||||
motor2.linkSensor(&encoder2);
|
||||
|
||||
|
||||
// driver config
|
||||
// power supply voltage [V]
|
||||
driver1.voltage_power_supply = 12;
|
||||
driver1.init();
|
||||
// link driver
|
||||
motor1.linkDriver(&driver1);
|
||||
// power supply voltage [V]
|
||||
driver2.voltage_power_supply = 12;
|
||||
driver2.init();
|
||||
// link driver
|
||||
motor2.linkDriver(&driver2);
|
||||
|
||||
// set control loop type to be used
|
||||
motor1.controller = MotionControlType::torque;
|
||||
motor2.controller = MotionControlType::torque;
|
||||
|
||||
// contoller configuration based on the controll type
|
||||
motor1.PID_velocity.P = 0.05f;
|
||||
motor1.PID_velocity.I = 1;
|
||||
motor1.PID_velocity.D = 0;
|
||||
// default voltage_power_supply
|
||||
motor1.voltage_limit = 12;
|
||||
// contoller configuration based on the controll type
|
||||
motor2.PID_velocity.P = 0.05f;
|
||||
motor2.PID_velocity.I = 1;
|
||||
motor2.PID_velocity.D = 0;
|
||||
// default voltage_power_supply
|
||||
motor2.voltage_limit = 12;
|
||||
|
||||
// angle loop velocity limit
|
||||
motor1.velocity_limit = 20;
|
||||
motor2.velocity_limit = 20;
|
||||
|
||||
// use monitoring with serial for motor init
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor1.useMonitoring(Serial);
|
||||
motor2.useMonitoring(Serial);
|
||||
|
||||
// initialise motor
|
||||
motor1.init();
|
||||
// align encoder and start FOC
|
||||
motor1.initFOC();
|
||||
|
||||
// initialise motor
|
||||
motor2.init();
|
||||
// align encoder and start FOC
|
||||
motor2.initFOC();
|
||||
|
||||
// set the inital target value
|
||||
motor1.target = 2;
|
||||
motor2.target = 2;
|
||||
|
||||
// subscribe motor to the commander
|
||||
command.add('A', doMotor1, "motor 1");
|
||||
command.add('B', doMotor2, "motor 2");
|
||||
|
||||
// Run user commands to configure and the motor (find the full command list in docs.simplefoc.com)
|
||||
Serial.println(F("Double motor sketch ready."));
|
||||
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
|
||||
void loop() {
|
||||
// iterative setting FOC phase voltage
|
||||
motor1.loopFOC();
|
||||
motor2.loopFOC();
|
||||
|
||||
// iterative function setting the outter loop target
|
||||
motor1.move();
|
||||
motor2.move();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+89
@@ -0,0 +1,89 @@
|
||||
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder = Encoder(2, 3, 500);
|
||||
// channel A and B callbacks
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
|
||||
// commander communication instance
|
||||
Commander command = Commander(Serial);
|
||||
void doMotor(char* cmd){ command.motor(&motor, cmd); }
|
||||
|
||||
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);
|
||||
|
||||
// set control loop type to be used
|
||||
motor.controller = MotionControlType::torque;
|
||||
|
||||
// contoller configuration based on the controll type
|
||||
motor.PID_velocity.P = 0.05f;
|
||||
motor.PID_velocity.I = 1;
|
||||
motor.PID_velocity.D = 0;
|
||||
// default voltage_power_supply
|
||||
motor.voltage_limit = 12;
|
||||
|
||||
// velocity low pass filtering time constant
|
||||
motor.LPF_velocity.Tf = 0.01f;
|
||||
|
||||
// angle loop controller
|
||||
motor.P_angle.P = 20;
|
||||
// angle loop velocity limit
|
||||
motor.velocity_limit = 20;
|
||||
|
||||
// use monitoring with serial for motor init
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
motor.monitor_downsample = 0; // disable intially
|
||||
motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle
|
||||
|
||||
// initialise motor
|
||||
motor.init();
|
||||
// align encoder and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// set the inital target value
|
||||
motor.target = 2;
|
||||
|
||||
// subscribe motor to the commander
|
||||
command.add('M', doMotor, "motor");
|
||||
|
||||
// Run user commands to configure and the motor (find the full command list in docs.simplefoc.com)
|
||||
Serial.println(F("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
|
||||
motor.move();
|
||||
|
||||
// motor monitoring
|
||||
motor.monitor();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+139
@@ -0,0 +1,139 @@
|
||||
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor1 = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver1 = BLDCDriver3PWM(5, 10, 6, 8);
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor2 = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver2 = BLDCDriver3PWM(3, 9, 11, 7);
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder1 = Encoder(12, 2, 500);
|
||||
// channel A and B callbacks
|
||||
void doA1(){encoder1.handleA();}
|
||||
void doB1(){encoder1.handleB();}
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder2 = Encoder(A5, A4, 500);
|
||||
// channel A and B callbacks
|
||||
void doA2(){encoder2.handleA();}
|
||||
void doB2(){encoder2.handleB();}
|
||||
|
||||
|
||||
// inline current sensor instance
|
||||
// check if your board has R010 (0.01 ohm resistor) or R006 (0.006 mOhm resistor)
|
||||
InlineCurrentSense current_sense1 = InlineCurrentSense(0.01, 50.0, A0, A2);
|
||||
|
||||
// inline current sensor instance
|
||||
// check if your board has R010 (0.01 ohm resistor) or R006 (0.006 mOhm resistor)
|
||||
InlineCurrentSense current_sense2 = InlineCurrentSense(0.01, 50.0, A1, A3);
|
||||
|
||||
// commander communication instance
|
||||
Commander command = Commander(Serial);
|
||||
// void doMotor1(char* cmd){ command.motor(&motor1, cmd); }
|
||||
// void doMotor2(char* cmd){ command.motor(&motor2, cmd); }
|
||||
void doTarget1(char* cmd){ command.scalar(&motor1.target, cmd); }
|
||||
void doTarget2(char* cmd){ command.scalar(&motor2.target, cmd); }
|
||||
|
||||
void setup() {
|
||||
|
||||
// initialize encoder sensor hardware
|
||||
encoder1.init();
|
||||
encoder1.enableInterrupts(doA1, doB1);
|
||||
// initialize encoder sensor hardware
|
||||
encoder2.init();
|
||||
encoder2.enableInterrupts(doA2, doB2);
|
||||
// link the motor to the sensor
|
||||
motor1.linkSensor(&encoder1);
|
||||
motor2.linkSensor(&encoder2);
|
||||
|
||||
|
||||
// driver config
|
||||
// power supply voltage [V]
|
||||
driver1.voltage_power_supply = 12;
|
||||
driver1.init();
|
||||
// link driver
|
||||
motor1.linkDriver(&driver1);
|
||||
// power supply voltage [V]
|
||||
driver2.voltage_power_supply = 12;
|
||||
driver2.init();
|
||||
// link driver
|
||||
motor2.linkDriver(&driver2);
|
||||
|
||||
// set control loop type to be used
|
||||
motor1.controller = MotionControlType::torque;
|
||||
motor2.controller = MotionControlType::torque;
|
||||
|
||||
// contoller configuration based on the controll type
|
||||
motor1.PID_velocity.P = 0.05f;
|
||||
motor1.PID_velocity.I = 1;
|
||||
motor1.PID_velocity.D = 0;
|
||||
// default voltage_power_supply
|
||||
motor1.voltage_limit = 12;
|
||||
// contoller configuration based on the controll type
|
||||
motor2.PID_velocity.P = 0.05f;
|
||||
motor2.PID_velocity.I = 1;
|
||||
motor2.PID_velocity.D = 0;
|
||||
// default voltage_power_supply
|
||||
motor2.voltage_limit = 12;
|
||||
|
||||
// angle loop velocity limit
|
||||
motor1.velocity_limit = 20;
|
||||
motor2.velocity_limit = 20;
|
||||
|
||||
// use monitoring with serial for motor init
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor1.useMonitoring(Serial);
|
||||
motor2.useMonitoring(Serial);
|
||||
|
||||
|
||||
// current sense init and linking
|
||||
current_sense1.init();
|
||||
motor1.linkCurrentSense(¤t_sense1);
|
||||
// current sense init and linking
|
||||
current_sense2.init();
|
||||
motor2.linkCurrentSense(¤t_sense2);
|
||||
|
||||
// initialise motor
|
||||
motor1.init();
|
||||
// align encoder and start FOC
|
||||
motor1.initFOC();
|
||||
|
||||
// initialise motor
|
||||
motor2.init();
|
||||
// align encoder and start FOC
|
||||
motor2.initFOC();
|
||||
|
||||
// set the inital target value
|
||||
motor1.target = 2;
|
||||
motor2.target = 2;
|
||||
|
||||
// subscribe motor to the commander
|
||||
// command.add('A', doMotor1, "motor 1");
|
||||
// command.add('B', doMotor2, "motor 2");
|
||||
command.add('A', doTarget1, "target 1");
|
||||
command.add('B', doTarget2, "target 2");
|
||||
|
||||
// Run user commands to configure and the motor (find the full command list in docs.simplefoc.com)
|
||||
Serial.println("Motors ready.");
|
||||
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
|
||||
void loop() {
|
||||
// iterative setting FOC phase voltage
|
||||
motor1.loopFOC();
|
||||
motor2.loopFOC();
|
||||
|
||||
// iterative function setting the outter loop target
|
||||
motor1.move();
|
||||
motor2.move();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+99
@@ -0,0 +1,99 @@
|
||||
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(5, 10, 6, 8);
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder = Encoder(2, 3, 500);
|
||||
// channel A and B callbacks
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
|
||||
// inline current sensor instance
|
||||
// check if your board has R010 (0.01 ohm resistor) or R006 (0.006 mOhm resistor)
|
||||
InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2);
|
||||
|
||||
// commander communication instance
|
||||
Commander command = Commander(Serial);
|
||||
void doMotion(char* cmd){ command.motion(&motor, cmd); }
|
||||
// void doMotor(char* cmd){ command.motor(&motor, cmd); }
|
||||
|
||||
void setup() {
|
||||
|
||||
// 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);
|
||||
|
||||
// set control loop type to be used
|
||||
motor.controller = MotionControlType::torque;
|
||||
|
||||
// contoller configuration based on the controll type
|
||||
motor.PID_velocity.P = 0.05f;
|
||||
motor.PID_velocity.I = 1;
|
||||
motor.PID_velocity.D = 0;
|
||||
// default voltage_power_supply
|
||||
motor.voltage_limit = 12;
|
||||
|
||||
// velocity low pass filtering time constant
|
||||
motor.LPF_velocity.Tf = 0.01f;
|
||||
|
||||
// angle loop controller
|
||||
motor.P_angle.P = 20;
|
||||
// angle loop velocity limit
|
||||
motor.velocity_limit = 20;
|
||||
|
||||
// use monitoring with serial for motor init
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
motor.monitor_downsample = 0; // disable intially
|
||||
motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle
|
||||
|
||||
// current sense init and linking
|
||||
current_sense.init();
|
||||
motor.linkCurrentSense(¤t_sense);
|
||||
|
||||
// initialise motor
|
||||
motor.init();
|
||||
// align encoder and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// set the inital target value
|
||||
motor.target = 2;
|
||||
|
||||
// subscribe motor to the commander
|
||||
command.add('T', doMotion, "motion control");
|
||||
// command.add('M', doMotor, "motor");
|
||||
|
||||
// Run user commands to configure and the motor (find the full command list in docs.simplefoc.com)
|
||||
Serial.println("Motor ready.");
|
||||
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
|
||||
void loop() {
|
||||
// iterative setting FOC phase voltage
|
||||
motor.loopFOC();
|
||||
|
||||
// iterative function setting the outter loop target
|
||||
motor.move();
|
||||
|
||||
// motor monitoring
|
||||
motor.monitor();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+61
@@ -0,0 +1,61 @@
|
||||
/**
|
||||
* Smart Stepper support with SimpleFOClibrary
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// magnetic sensor instance - SPI
|
||||
MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, A2);
|
||||
|
||||
// Stepper motor & driver instance
|
||||
StepperMotor motor = StepperMotor(50);
|
||||
int in1[2] = {5, 6};
|
||||
int in2[2] = {A4, 7};
|
||||
StepperDriver2PWM driver = StepperDriver2PWM(4, in1, 9, in2);
|
||||
|
||||
// instantiate the commander
|
||||
Commander command = Commander(SerialUSB);
|
||||
void doMotor(char* cmd) { command.motor(&motor, cmd); }
|
||||
|
||||
void setup() {
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init();
|
||||
// link the motor to the sensor
|
||||
motor.linkSensor(&sensor);
|
||||
|
||||
// power supply voltage
|
||||
driver.voltage_power_supply = 12;
|
||||
driver.init();
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::torque;
|
||||
|
||||
// use monitoring with SerialUSB
|
||||
SerialUSB.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(SerialUSB);
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align sensor and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// add target command M
|
||||
command.add('M', doMotor, "my motor");
|
||||
|
||||
SerialUSB.println(F("Motor ready."));
|
||||
SerialUSB.println(F("Set the target voltage using Serial terminal:"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// main FOC algorithm function
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
motor.move();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+69
@@ -0,0 +1,69 @@
|
||||
// Open loop motor control example
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
|
||||
// BLDC motor & driver instance
|
||||
// BLDCMotor motor = BLDCMotor(pole pair number);
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
// BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional));
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
|
||||
|
||||
// Stepper motor & driver instance
|
||||
//StepperMotor motor = StepperMotor(50);
|
||||
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
|
||||
|
||||
//target variable
|
||||
float target_position = 0;
|
||||
|
||||
// instantiate the commander
|
||||
Commander command = Commander(Serial);
|
||||
void doTarget(char* cmd) { command.scalar(&target_position, cmd); }
|
||||
void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); }
|
||||
void doVelocity(char* cmd) { command.scalar(&motor.velocity_limit, cmd); }
|
||||
|
||||
void setup() {
|
||||
|
||||
// driver config
|
||||
// power supply voltage [V]
|
||||
driver.voltage_power_supply = 12;
|
||||
// limit the maximal dc voltage the driver can set
|
||||
// as a protection measure for the low-resistance motors
|
||||
// this value is fixed on startup
|
||||
driver.voltage_limit = 6;
|
||||
driver.init();
|
||||
// link the motor and the driver
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// limiting motor movements
|
||||
// limit the voltage to be set to the motor
|
||||
// start very low for high resistance motors
|
||||
// currnet = resistance*voltage, so try to be well under 1Amp
|
||||
motor.voltage_limit = 3; // [V]
|
||||
// limit/set the velocity of the transition in between
|
||||
// target angles
|
||||
motor.velocity_limit = 5; // [rad/s] cca 50rpm
|
||||
// open loop control config
|
||||
motor.controller = MotionControlType::angle_openloop;
|
||||
|
||||
// init motor hardware
|
||||
motor.init();
|
||||
|
||||
// add target command T
|
||||
command.add('T', doTarget, "target angle");
|
||||
command.add('L', doLimit, "voltage limit");
|
||||
command.add('V', doLimit, "movement velocity");
|
||||
|
||||
Serial.begin(115200);
|
||||
Serial.println("Motor ready!");
|
||||
Serial.println("Set target position [rad]");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// open loop angle movements
|
||||
// using motor.voltage_limit and motor.velocity_limit
|
||||
motor.move(target_position);
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+67
@@ -0,0 +1,67 @@
|
||||
// Open loop motor control example
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
|
||||
// BLDC motor & driver instance
|
||||
// BLDCMotor motor = BLDCMotor(pole pair number);
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
// BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional));
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
|
||||
|
||||
// Stepper motor & driver instance
|
||||
//StepperMotor motor = StepperMotor(50);
|
||||
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
|
||||
|
||||
|
||||
//target variable
|
||||
float target_velocity = 0;
|
||||
|
||||
// instantiate the commander
|
||||
Commander command = Commander(Serial);
|
||||
void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); }
|
||||
void doLimit(char* cmd) { command.scalar(&motor.voltage_limit, cmd); }
|
||||
|
||||
void setup() {
|
||||
|
||||
// driver config
|
||||
// power supply voltage [V]
|
||||
driver.voltage_power_supply = 12;
|
||||
// limit the maximal dc voltage the driver can set
|
||||
// as a protection measure for the low-resistance motors
|
||||
// this value is fixed on startup
|
||||
driver.voltage_limit = 6;
|
||||
driver.init();
|
||||
// link the motor and the driver
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// limiting motor movements
|
||||
// limit the voltage to be set to the motor
|
||||
// start very low for high resistance motors
|
||||
// currnet = resistance*voltage, so try to be well under 1Amp
|
||||
motor.voltage_limit = 3; // [V]
|
||||
|
||||
// open loop control config
|
||||
motor.controller = MotionControlType::velocity_openloop;
|
||||
|
||||
// init motor hardware
|
||||
motor.init();
|
||||
|
||||
// add target command T
|
||||
command.add('T', doTarget, "target velocity");
|
||||
command.add('L', doLimit, "voltage limit");
|
||||
|
||||
Serial.begin(115200);
|
||||
Serial.println("Motor ready!");
|
||||
Serial.println("Set target velocity [rad/s]");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
|
||||
// open loop velocity movement
|
||||
// using motor.voltage_limit and motor.velocity_limit
|
||||
motor.move(target_velocity);
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+139
@@ -0,0 +1,139 @@
|
||||
/**
|
||||
*
|
||||
* Position/angle motion control example
|
||||
* Steps:
|
||||
* 1) Configure the motor and encoder
|
||||
* 2) Run the code
|
||||
* 3) Set the target angle (in radians) from serial terminal
|
||||
*
|
||||
*
|
||||
* NOTE :
|
||||
* > Arduino UNO example code for running velocity motion control using an encoder with index significantly
|
||||
* > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager.
|
||||
*
|
||||
* > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins
|
||||
* > you can supply doIndex directly to the encoder.enableInterrupts(doA,doB,doIndex) and avoid using PciManger
|
||||
*
|
||||
* > If you don't want to use index pin initialize the encoder class without index pin number:
|
||||
* > For example:
|
||||
* > - Encoder encoder = Encoder(2, 3, 8192);
|
||||
* > and initialize interrupts like this:
|
||||
* > - encoder.enableInterrupts(doA,doB)
|
||||
*
|
||||
* Check the docs.simplefoc.com for more info about the possible encoder configuration.
|
||||
*
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
// software interrupt library
|
||||
#include <PciManager.h>
|
||||
#include <PciListenerImp.h>
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
|
||||
// Stepper motor & driver instance
|
||||
//StepperMotor motor = StepperMotor(50);
|
||||
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder = Encoder(2, 3, 8192, A0);
|
||||
|
||||
// Interrupt routine intialisation
|
||||
// channel A and B callbacks
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
void doIndex(){encoder.handleIndex();}
|
||||
// If no available hadware interrupt pins use the software interrupt
|
||||
PciListenerImp listenerIndex(encoder.index_pin, doIndex);
|
||||
|
||||
// angle set point variable
|
||||
float target_angle = 0;
|
||||
// instantiate the commander
|
||||
Commander command = Commander(Serial);
|
||||
void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }
|
||||
|
||||
void setup() {
|
||||
|
||||
// initialize encoder sensor hardware
|
||||
encoder.init();
|
||||
encoder.enableInterrupts(doA, doB);
|
||||
// software interrupts
|
||||
PciManager.registerListener(&listenerIndex);
|
||||
// link the motor to the sensor
|
||||
motor.linkSensor(&encoder);
|
||||
|
||||
// driver config
|
||||
// power supply voltage [V]
|
||||
driver.voltage_power_supply = 12;
|
||||
driver.init();
|
||||
// link the motor and the driver
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// aligning voltage [V]
|
||||
motor.voltage_sensor_align = 3;
|
||||
// index search velocity [rad/s]
|
||||
motor.velocity_index_search = 3;
|
||||
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::angle;
|
||||
|
||||
// contoller configuration
|
||||
// default parameters in defaults.h
|
||||
|
||||
// velocity PI controller parameters
|
||||
motor.PID_velocity.P = 0.2f;
|
||||
motor.PID_velocity.I = 20;
|
||||
motor.PID_velocity.D = 0;
|
||||
// default voltage_power_supply
|
||||
motor.voltage_limit = 6;
|
||||
// jerk control using voltage voltage ramp
|
||||
// default value is 300 volts per sec ~ 0.3V per millisecond
|
||||
motor.PID_velocity.output_ramp = 1000;
|
||||
|
||||
// velocity low pass filtering time constant
|
||||
motor.LPF_velocity.Tf = 0.01f;
|
||||
|
||||
// angle P controller
|
||||
motor.P_angle.P = 20;
|
||||
// maximal velocity of the position control
|
||||
motor.velocity_limit = 4;
|
||||
|
||||
|
||||
// use monitoring with serial
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align encoder and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// add target command T
|
||||
command.add('T', doTarget, "target angle");
|
||||
|
||||
Serial.println(F("Motor ready."));
|
||||
Serial.println(F("Set the target angle using serial terminal:"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// main FOC algorithm function
|
||||
// the faster you run this function the better
|
||||
// Arduino UNO loop ~1kHz
|
||||
// Bluepill loop ~10kHz
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
// velocity, position or voltage (defined in motor.controller)
|
||||
// this function can be run at much lower frequency than loopFOC() function
|
||||
// You can also use motor.move() and set the motor.target in the code
|
||||
motor.move(target_angle);
|
||||
|
||||
// function intended to be used with serial plotter to monitor motor variables
|
||||
// significantly slowing the execution down!!!!
|
||||
// motor.monitor();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+133
@@ -0,0 +1,133 @@
|
||||
/**
|
||||
*
|
||||
* Position/angle motion control example
|
||||
* Steps:
|
||||
* 1) Configure the motor and hall sensor
|
||||
* 2) Run the code
|
||||
* 3) Set the target angle (in radians) from serial terminal
|
||||
*
|
||||
*
|
||||
* NOTE :
|
||||
* > This is for Arduino UNO example code for running angle motion control specifically
|
||||
* > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager.
|
||||
*
|
||||
* > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins
|
||||
* > you can supply doIndex directly to the sensr.enableInterrupts(doA,doB,doC) and avoid using PciManger
|
||||
*
|
||||
*
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
// software interrupt library
|
||||
#include <PciManager.h>
|
||||
#include <PciListenerImp.h>
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
|
||||
// Stepper motor & driver instance
|
||||
//StepperMotor motor = StepperMotor(50);
|
||||
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
|
||||
|
||||
// hall sensor instance
|
||||
HallSensor sensor = HallSensor(2, 3, 4, 11);
|
||||
|
||||
// Interrupt routine intialisation
|
||||
// channel A and B callbacks
|
||||
void doA(){sensor.handleA();}
|
||||
void doB(){sensor.handleB();}
|
||||
void doC(){sensor.handleC();}
|
||||
// If no available hadware interrupt pins use the software interrupt
|
||||
PciListenerImp listenC(sensor.pinC, doC);
|
||||
|
||||
// angle set point variable
|
||||
float target_angle = 0;
|
||||
// instantiate the commander
|
||||
Commander command = Commander(Serial);
|
||||
void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }
|
||||
|
||||
void setup() {
|
||||
|
||||
// initialize sensor hardware
|
||||
sensor.init();
|
||||
sensor.enableInterrupts(doA, doB); //, doC);
|
||||
// software interrupts
|
||||
PciManager.registerListener(&listenC);
|
||||
// link the motor to the sensor
|
||||
motor.linkSensor(&sensor);
|
||||
|
||||
// driver config
|
||||
// power supply voltage [V]
|
||||
driver.voltage_power_supply = 12;
|
||||
driver.init();
|
||||
// link the motor and the driver
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
|
||||
// aligning voltage [V]
|
||||
motor.voltage_sensor_align = 3;
|
||||
// index search velocity [rad/s]
|
||||
motor.velocity_index_search = 3;
|
||||
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::angle;
|
||||
|
||||
// contoller configuration
|
||||
// default parameters in defaults.h
|
||||
|
||||
// velocity PI controller parameters
|
||||
motor.PID_velocity.P = 0.2f;
|
||||
motor.PID_velocity.I = 2;
|
||||
motor.PID_velocity.D = 0;
|
||||
// default voltage_power_supply
|
||||
motor.voltage_limit = 6;
|
||||
// jerk control using voltage voltage ramp
|
||||
// default value is 300 volts per sec ~ 0.3V per millisecond
|
||||
motor.PID_velocity.output_ramp = 1000;
|
||||
|
||||
// velocity low pass filtering time constant
|
||||
motor.LPF_velocity.Tf = 0.01f;
|
||||
|
||||
// angle P controller
|
||||
motor.P_angle.P = 20;
|
||||
// maximal velocity of the position control
|
||||
motor.velocity_limit = 4;
|
||||
|
||||
|
||||
// use monitoring with serial
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align sensor and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// add target command T
|
||||
command.add('T', doTarget, "target angle");
|
||||
|
||||
Serial.println(F("Motor ready."));
|
||||
Serial.println(F("Set the target angle using serial terminal:"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// main FOC algorithm function
|
||||
// the faster you run this function the better
|
||||
// Arduino UNO loop ~1kHz
|
||||
// Bluepill loop ~10kHz
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
// velocity, position or voltage (defined in motor.controller)
|
||||
// this function can be run at much lower frequency than loopFOC() function
|
||||
// You can also use motor.move() and set the motor.target in the code
|
||||
motor.move(target_angle);
|
||||
|
||||
// function intended to be used with serial plotter to monitor motor variables
|
||||
// significantly slowing the execution down!!!!
|
||||
// motor.monitor();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+112
@@ -0,0 +1,112 @@
|
||||
/**
|
||||
*
|
||||
* Position/angle motion control example
|
||||
* Steps:
|
||||
* 1) Configure the motor and magnetic sensor
|
||||
* 2) Run the code
|
||||
* 3) Set the target angle (in radians) from serial terminal
|
||||
*
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// magnetic sensor instance - SPI
|
||||
MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10);
|
||||
// magnetic sensor instance - MagneticSensorI2C
|
||||
//MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
|
||||
// magnetic sensor instance - analog output
|
||||
// MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
|
||||
// Stepper motor & driver instance
|
||||
//StepperMotor motor = StepperMotor(50);
|
||||
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
|
||||
|
||||
// angle set point variable
|
||||
float target_angle = 0;
|
||||
// instantiate the commander
|
||||
Commander command = Commander(Serial);
|
||||
void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }
|
||||
|
||||
void 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 the motor and the driver
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// choose FOC modulation (optional)
|
||||
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
|
||||
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::angle;
|
||||
|
||||
// contoller configuration
|
||||
// default parameters in defaults.h
|
||||
|
||||
// velocity PI controller parameters
|
||||
motor.PID_velocity.P = 0.2f;
|
||||
motor.PID_velocity.I = 20;
|
||||
motor.PID_velocity.D = 0;
|
||||
// maximal voltage to be set to the motor
|
||||
motor.voltage_limit = 6;
|
||||
|
||||
// velocity low pass filtering time constant
|
||||
// the lower the less filtered
|
||||
motor.LPF_velocity.Tf = 0.01f;
|
||||
|
||||
// angle P controller
|
||||
motor.P_angle.P = 20;
|
||||
// maximal velocity of the position control
|
||||
motor.velocity_limit = 20;
|
||||
|
||||
// use monitoring with serial
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align sensor and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// add target command T
|
||||
command.add('T', doTarget, "target angle");
|
||||
|
||||
Serial.println(F("Motor ready."));
|
||||
Serial.println(F("Set the target angle using serial terminal:"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
|
||||
void loop() {
|
||||
|
||||
// main FOC algorithm function
|
||||
// the faster you run this function the better
|
||||
// Arduino UNO loop ~1kHz
|
||||
// Bluepill loop ~10kHz
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
// velocity, position or voltage (defined in motor.controller)
|
||||
// this function can be run at much lower frequency than loopFOC() function
|
||||
// You can also use motor.move() and set the motor.target in the code
|
||||
motor.move(target_angle);
|
||||
|
||||
|
||||
// function intended to be used with serial plotter to monitor motor variables
|
||||
// significantly slowing the execution down!!!!
|
||||
// motor.monitor();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+105
@@ -0,0 +1,105 @@
|
||||
/**
|
||||
*
|
||||
* Torque control example using current control loop.
|
||||
*
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder = Encoder(2, 3, 500);
|
||||
// channel A and B callbacks
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
|
||||
// current sensor
|
||||
InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2);
|
||||
|
||||
// current set point variable
|
||||
float target_current = 0;
|
||||
// instantiate the commander
|
||||
Commander command = Commander(Serial);
|
||||
void doTarget(char* cmd) { command.scalar(&target_current, cmd); }
|
||||
|
||||
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);
|
||||
|
||||
// current sense init hardware
|
||||
current_sense.init();
|
||||
// link the current sense to the motor
|
||||
motor.linkCurrentSense(¤t_sense);
|
||||
|
||||
// set torque mode:
|
||||
// TorqueControlType::dc_current
|
||||
// TorqueControlType::voltage
|
||||
// TorqueControlType::foc_current
|
||||
motor.torque_controller = TorqueControlType::foc_current;
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::torque;
|
||||
|
||||
// foc currnet control parameters (Arduino UNO/Mega)
|
||||
motor.PID_current_q.P = 5;
|
||||
motor.PID_current_q.I= 300;
|
||||
motor.PID_current_d.P= 5;
|
||||
motor.PID_current_d.I = 300;
|
||||
motor.LPF_current_q.Tf = 0.01f;
|
||||
motor.LPF_current_d.Tf = 0.01f;
|
||||
// foc currnet control parameters (stm/esp/due/teensy)
|
||||
// motor.PID_current_q.P = 5;
|
||||
// motor.PID_current_q.I= 1000;
|
||||
// motor.PID_current_d.P= 5;
|
||||
// motor.PID_current_d.I = 1000;
|
||||
// motor.LPF_current_q.Tf = 0.002f; // 1ms default
|
||||
// motor.LPF_current_d.Tf = 0.002f; // 1ms default
|
||||
|
||||
// use monitoring with serial
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align sensor and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// add target command T
|
||||
command.add('T', doTarget, "target current");
|
||||
|
||||
Serial.println(F("Motor ready."));
|
||||
Serial.println(F("Set the target current using serial terminal:"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
|
||||
// main FOC algorithm function
|
||||
// the faster you run this function the better
|
||||
// Arduino UNO loop ~1kHz
|
||||
// Bluepill loop ~10kHz
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
// velocity, position or torque (defined in motor.controller)
|
||||
// this function can be run at much lower frequency than loopFOC() function
|
||||
// You can also use motor.move() and set the motor.target in the code
|
||||
motor.move(target_current);
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+92
@@ -0,0 +1,92 @@
|
||||
/**
|
||||
*
|
||||
* Torque control example using voltage control loop.
|
||||
*
|
||||
* Most of the low-end BLDC driver boards doesn't have current measurement therefore SimpleFOC offers
|
||||
* you a way to control motor torque by setting the voltage to the motor instead hte current.
|
||||
*
|
||||
* This makes the BLDC motor effectively a DC motor, and you can use it in a same way.
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
|
||||
// Stepper motor & driver instance
|
||||
//StepperMotor motor = StepperMotor(50);
|
||||
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder = Encoder(2, 3, 8192);
|
||||
|
||||
// Interrupt routine intialisation
|
||||
// channel A and B callbacks
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
|
||||
// voltage set point variable
|
||||
float target_voltage = 2;
|
||||
// instantiate the commander
|
||||
Commander command = Commander(Serial);
|
||||
void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); }
|
||||
|
||||
void setup() {
|
||||
|
||||
// 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);
|
||||
|
||||
|
||||
// aligning voltage
|
||||
motor.voltage_sensor_align = 5;
|
||||
// choose FOC modulation (optional)
|
||||
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
|
||||
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::torque;
|
||||
|
||||
// use monitoring with serial
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align sensor and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// add target command T
|
||||
command.add('T', doTarget, "target voltage");
|
||||
|
||||
Serial.println(F("Motor ready."));
|
||||
Serial.println(F("Set the target voltage using serial terminal:"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
|
||||
// main FOC algorithm function
|
||||
// the faster you run this function the better
|
||||
// Arduino UNO loop ~1kHz
|
||||
// Bluepill loop ~10kHz
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
// velocity, position or voltage (defined in motor.controller)
|
||||
// this function can be run at much lower frequency than loopFOC() function
|
||||
// You can also use motor.move() and set the motor.target in the code
|
||||
motor.move(target_voltage);
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+95
@@ -0,0 +1,95 @@
|
||||
/**
|
||||
*
|
||||
* Torque control example using voltage control loop.
|
||||
*
|
||||
* Most of the low-end BLDC driver boards doesn't have current measurement therefore SimpleFOC offers
|
||||
* you a way to control motor torque by setting the voltage to the motor instead of the current.
|
||||
*
|
||||
* This makes the BLDC motor effectively a DC motor, and you can use it in a same way.
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
|
||||
// Stepper motor & driver instance
|
||||
//StepperMotor motor = StepperMotor(50);
|
||||
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
|
||||
|
||||
// hall sensor instance
|
||||
HallSensor sensor = HallSensor(2, 3, 4, 11);
|
||||
|
||||
// Interrupt routine intialisation
|
||||
// channel A and B callbacks
|
||||
void doA(){sensor.handleA();}
|
||||
void doB(){sensor.handleB();}
|
||||
void doC(){sensor.handleC();}
|
||||
|
||||
|
||||
// voltage set point variable
|
||||
float target_voltage = 2;
|
||||
// instantiate the commander
|
||||
Commander command = Commander(Serial);
|
||||
void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); }
|
||||
|
||||
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);
|
||||
|
||||
// aligning voltage
|
||||
motor.voltage_sensor_align = 3;
|
||||
|
||||
// choose FOC modulation (optional)
|
||||
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
|
||||
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::torque;
|
||||
|
||||
// use monitoring with serial
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align sensor and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// add target command T
|
||||
command.add('T', doTarget, "target voltage");
|
||||
|
||||
Serial.println(F("Motor ready."));
|
||||
Serial.println(F("Set the target voltage using serial terminal:"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
|
||||
void loop() {
|
||||
|
||||
// main FOC algorithm function
|
||||
// the faster you run this function the better
|
||||
// Arduino UNO loop ~1kHz
|
||||
// Bluepill loop ~10kHz
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
// velocity, position or voltage (defined in motor.controller)
|
||||
// this function can be run at much lower frequency than loopFOC() function
|
||||
// You can also use motor.move() and set the motor.target in the code
|
||||
motor.move(target_voltage);
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+84
@@ -0,0 +1,84 @@
|
||||
/**
|
||||
* Torque control example using voltage control loop.
|
||||
*
|
||||
* Most of the low-end BLDC driver boards doesn't have current measurement therefore SimpleFOC offers
|
||||
* you a way to control motor torque by setting the voltage to the motor instead hte current.
|
||||
*
|
||||
* This makes the BLDC motor effectively a DC motor, and you can use it in a same way.
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// magnetic sensor instance - SPI
|
||||
MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10);
|
||||
// magnetic sensor instance - I2C
|
||||
// MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
|
||||
// magnetic sensor instance - analog output
|
||||
// MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
|
||||
// Stepper motor & driver instance
|
||||
//StepperMotor motor = StepperMotor(50);
|
||||
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
|
||||
|
||||
// voltage set point variable
|
||||
float target_voltage = 2;
|
||||
// instantiate the commander
|
||||
Commander command = Commander(Serial);
|
||||
void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); }
|
||||
|
||||
void setup() {
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init();
|
||||
// link the motor to the sensor
|
||||
motor.linkSensor(&sensor);
|
||||
|
||||
// power supply voltage
|
||||
driver.voltage_power_supply = 12;
|
||||
driver.init();
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// aligning voltage
|
||||
motor.voltage_sensor_align = 5;
|
||||
// choose FOC modulation (optional)
|
||||
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::torque;
|
||||
|
||||
// use monitoring with serial
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align sensor and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// add target command T
|
||||
command.add('T', doTarget, "target voltage");
|
||||
|
||||
Serial.println(F("Motor ready."));
|
||||
Serial.println(F("Set the target voltage using serial terminal:"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
|
||||
// main FOC algorithm function
|
||||
// the faster you run this function the better
|
||||
// Arduino UNO loop ~1kHz
|
||||
// Bluepill loop ~10kHz
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
// velocity, position or voltage (defined in motor.controller)
|
||||
// this function can be run at much lower frequency than loopFOC() function
|
||||
// You can also use motor.move() and set the motor.target in the code
|
||||
motor.move(target_voltage);
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+137
@@ -0,0 +1,137 @@
|
||||
/**
|
||||
*
|
||||
* Velocity motion control example
|
||||
* Steps:
|
||||
* 1) Configure the motor and encoder
|
||||
* 2) Run the code
|
||||
* 3) Set the target velocity (in radians per second) from serial terminal
|
||||
*
|
||||
*
|
||||
*
|
||||
* NOTE :
|
||||
* > Arduino UNO example code for running velocity motion control using an encoder with index significantly
|
||||
* > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager.
|
||||
*
|
||||
* > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins
|
||||
* > you can supply doIndex directly to the encoder.enableInterrupts(doA,doB,doIndex) and avoid using PciManger
|
||||
*
|
||||
* > If you don't want to use index pin initialize the encoder class without index pin number:
|
||||
* > For example:
|
||||
* > - Encoder encoder = Encoder(2, 3, 8192);
|
||||
* > and initialize interrupts like this:
|
||||
* > - encoder.enableInterrupts(doA,doB)
|
||||
*
|
||||
* Check the docs.simplefoc.com for more info about the possible encoder configuration.
|
||||
*
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
// software interrupt library
|
||||
#include <PciManager.h>
|
||||
#include <PciListenerImp.h>
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
|
||||
// Stepper motor & driver instance
|
||||
//StepperMotor motor = StepperMotor(50);
|
||||
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder = Encoder(2, 3, 8192, A0);
|
||||
|
||||
// Interrupt routine intialisation
|
||||
// channel A and B callbacks
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
void doIndex(){encoder.handleIndex();}
|
||||
// If no available hadware interrupt pins use the software interrupt
|
||||
PciListenerImp listenerIndex(encoder.index_pin, doIndex);
|
||||
|
||||
|
||||
// velocity set point variable
|
||||
float target_velocity = 0;
|
||||
// instantiate the commander
|
||||
Commander command = Commander(Serial);
|
||||
void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); }
|
||||
|
||||
|
||||
void setup() {
|
||||
|
||||
// initialize encoder sensor hardware
|
||||
encoder.init();
|
||||
encoder.enableInterrupts(doA, doB);
|
||||
// software interrupts
|
||||
PciManager.registerListener(&listenerIndex);
|
||||
// link the motor to the sensor
|
||||
motor.linkSensor(&encoder);
|
||||
|
||||
// driver config
|
||||
// power supply voltage [V]
|
||||
driver.voltage_power_supply = 12;
|
||||
driver.init();
|
||||
// link the motor and the driver
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// aligning voltage [V]
|
||||
motor.voltage_sensor_align = 3;
|
||||
// index search velocity [rad/s]
|
||||
motor.velocity_index_search = 3;
|
||||
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::velocity;
|
||||
|
||||
// contoller configuration
|
||||
// default parameters in defaults.h
|
||||
|
||||
// velocity PI controller parameters
|
||||
motor.PID_velocity.P = 0.2f;
|
||||
motor.PID_velocity.I = 20;
|
||||
motor.PID_velocity.D = 0;
|
||||
// default voltage_power_supply
|
||||
motor.voltage_limit = 6;
|
||||
// jerk control using voltage voltage ramp
|
||||
// default value is 300 volts per sec ~ 0.3V per millisecond
|
||||
motor.PID_velocity.output_ramp = 1000;
|
||||
|
||||
// velocity low pass filtering time constant
|
||||
motor.LPF_velocity.Tf = 0.01f;
|
||||
|
||||
// use monitoring with serial
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align sensor and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// add target command T
|
||||
command.add('T', doTarget, "target velocity");
|
||||
|
||||
Serial.println(F("Motor ready."));
|
||||
Serial.println(F("Set the target velocity using serial terminal:"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
|
||||
void loop() {
|
||||
// main FOC algorithm function
|
||||
// the faster you run this function the better
|
||||
// Arduino UNO loop ~1kHz
|
||||
// Bluepill loop ~10kHz
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
// velocity, position or voltage (defined in motor.controller)
|
||||
// this function can be run at much lower frequency than loopFOC() function
|
||||
// You can also use motor.move() and set the motor.target in the code
|
||||
motor.move(target_velocity);
|
||||
|
||||
// function intended to be used with serial plotter to monitor motor variables
|
||||
// significantly slowing the execution down!!!!
|
||||
// motor.monitor();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+125
@@ -0,0 +1,125 @@
|
||||
/**
|
||||
*
|
||||
* Velocity motion control example
|
||||
* Steps:
|
||||
* 1) Configure the motor and sensor
|
||||
* 2) Run the code
|
||||
* 3) Set the target velocity (in radians per second) from serial terminal
|
||||
*
|
||||
*
|
||||
*
|
||||
* NOTE :
|
||||
* > Specifically for Arduino UNO example code for running velocity motion control using a hall sensor
|
||||
* > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager.
|
||||
*
|
||||
* > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins
|
||||
* > you can supply doC directly to the sensor.enableInterrupts(doA,doB,doC) and avoid using PciManger
|
||||
*
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
// software interrupt library
|
||||
#include <PciManager.h>
|
||||
#include <PciListenerImp.h>
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
|
||||
// Stepper motor & driver instance
|
||||
//StepperMotor motor = StepperMotor(50);
|
||||
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
|
||||
|
||||
// hall sensor instance
|
||||
HallSensor sensor = HallSensor(2, 3, 4, 11);
|
||||
|
||||
// Interrupt routine intialisation
|
||||
// channel A and B callbacks
|
||||
void doA(){sensor.handleA();}
|
||||
void doB(){sensor.handleB();}
|
||||
void doC(){sensor.handleC();}
|
||||
// If no available hadware interrupt pins use the software interrupt
|
||||
PciListenerImp listenerIndex(sensor.pinC, doC);
|
||||
|
||||
// velocity set point variable
|
||||
float target_velocity = 0;
|
||||
// instantiate the commander
|
||||
Commander command = Commander(Serial);
|
||||
void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); }
|
||||
|
||||
void setup() {
|
||||
|
||||
// initialize sensor sensor hardware
|
||||
sensor.init();
|
||||
sensor.enableInterrupts(doA, doB); //, doC);
|
||||
// software interrupts
|
||||
PciManager.registerListener(&listenerIndex);
|
||||
// link the motor to the sensor
|
||||
motor.linkSensor(&sensor);
|
||||
|
||||
// driver config
|
||||
// power supply voltage [V]
|
||||
driver.voltage_power_supply = 12;
|
||||
driver.init();
|
||||
// link the motor and the driver
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// aligning voltage [V]
|
||||
motor.voltage_sensor_align = 3;
|
||||
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::velocity;
|
||||
|
||||
// contoller configuration
|
||||
// default parameters in defaults.h
|
||||
|
||||
// velocity PI controller parameters
|
||||
motor.PID_velocity.P = 0.2f;
|
||||
motor.PID_velocity.I = 2;
|
||||
motor.PID_velocity.D = 0;
|
||||
// default voltage_power_supply
|
||||
motor.voltage_limit = 6;
|
||||
// jerk control using voltage voltage ramp
|
||||
// default value is 300 volts per sec ~ 0.3V per millisecond
|
||||
motor.PID_velocity.output_ramp = 1000;
|
||||
|
||||
// velocity low pass filtering time constant
|
||||
motor.LPF_velocity.Tf = 0.01f;
|
||||
|
||||
// use monitoring with serial
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align sensor and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// add target command T
|
||||
command.add('T', doTarget, "target voltage");
|
||||
|
||||
Serial.println(F("Motor ready."));
|
||||
Serial.println(F("Set the target velocity using serial terminal:"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
|
||||
void loop() {
|
||||
// main FOC algorithm function
|
||||
// the faster you run this function the better
|
||||
// Arduino UNO loop ~1kHz
|
||||
// Bluepill loop ~10kHz
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
// velocity, position or voltage (defined in motor.controller)
|
||||
// this function can be run at much lower frequency than loopFOC() function
|
||||
// You can also use motor.move() and set the motor.target in the code
|
||||
motor.move(target_velocity);
|
||||
|
||||
// function intended to be used with serial plotter to monitor motor variables
|
||||
// significantly slowing the execution down!!!!
|
||||
// motor.monitor();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+106
@@ -0,0 +1,106 @@
|
||||
/**
|
||||
*
|
||||
* Velocity motion control example
|
||||
* Steps:
|
||||
* 1) Configure the motor and magnetic sensor
|
||||
* 2) Run the code
|
||||
* 3) Set the target velocity (in radians per second) from serial terminal
|
||||
*
|
||||
*
|
||||
* By using the serial terminal set the velocity value you want to motor to obtain
|
||||
*
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// magnetic sensor instance - SPI
|
||||
MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10);
|
||||
// magnetic sensor instance - MagneticSensorI2C
|
||||
//MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
|
||||
// MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
|
||||
// Stepper motor & driver instance
|
||||
//StepperMotor motor = StepperMotor(50);
|
||||
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
|
||||
|
||||
// velocity set point variable
|
||||
float target_velocity = 0;
|
||||
// instantiate the commander
|
||||
Commander command = Commander(Serial);
|
||||
void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); }
|
||||
|
||||
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 the motor and the driver
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::velocity;
|
||||
|
||||
// contoller configuration
|
||||
// default parameters in defaults.h
|
||||
|
||||
// velocity PI controller parameters
|
||||
motor.PID_velocity.P = 0.2f;
|
||||
motor.PID_velocity.I = 20;
|
||||
motor.PID_velocity.D = 0;
|
||||
// default voltage_power_supply
|
||||
motor.voltage_limit = 6;
|
||||
// jerk control using voltage voltage ramp
|
||||
// default value is 300 volts per sec ~ 0.3V per millisecond
|
||||
motor.PID_velocity.output_ramp = 1000;
|
||||
|
||||
// velocity low pass filtering
|
||||
// default 5ms - try different values to see what is the best.
|
||||
// the lower the less filtered
|
||||
motor.LPF_velocity.Tf = 0.01f;
|
||||
|
||||
// use monitoring with serial
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align sensor and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// add target command T
|
||||
command.add('T', doTarget, "target voltage");
|
||||
|
||||
Serial.println(F("Motor ready."));
|
||||
Serial.println(F("Set the target velocity using serial terminal:"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// main FOC algorithm function
|
||||
// the faster you run this function the better
|
||||
// Arduino UNO loop ~1kHz
|
||||
// Bluepill loop ~10kHz
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
// velocity, position or voltage (defined in motor.controller)
|
||||
// this function can be run at much lower frequency than loopFOC() function
|
||||
// You can also use motor.move() and set the motor.target in the code
|
||||
motor.move(target_velocity);
|
||||
|
||||
// function intended to be used with serial plotter to monitor motor variables
|
||||
// significantly slowing the execution down!!!!
|
||||
// motor.monitor();
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+106
@@ -0,0 +1,106 @@
|
||||
/**
|
||||
* 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
|
||||
*
|
||||
* See more info in docs.simplefoc.com/commander_interface
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
|
||||
// Stepper motor & driver instance
|
||||
//StepperMotor motor = StepperMotor(50);
|
||||
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder = Encoder(2, 3, 8192);
|
||||
|
||||
// Interrupt routine intialisation
|
||||
// channel A and B callbacks
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
|
||||
|
||||
// commander interface
|
||||
Commander command = Commander(Serial);
|
||||
void onMotor(char* cmd){ command.motor(&motor, cmd); }
|
||||
|
||||
void setup() {
|
||||
|
||||
// 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 = MotionControlType::torque;
|
||||
|
||||
// contoller configuration based on the controll type
|
||||
motor.PID_velocity.P = 0.2f;
|
||||
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.01f;
|
||||
|
||||
// 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;
|
||||
|
||||
// define the motor id
|
||||
command.add('A', onMotor, "motor");
|
||||
|
||||
// Run user commands to configure and the motor (find the full command list in docs.simplefoc.com)
|
||||
Serial.println(F("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
|
||||
command.run();
|
||||
}
|
||||
+114
@@ -0,0 +1,114 @@
|
||||
/**
|
||||
* 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
|
||||
*
|
||||
* See more info in docs.simplefoc.com/commander_interface
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
// software interrupt library
|
||||
#include <PciManager.h>
|
||||
#include <PciListenerImp.h>
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
|
||||
// Stepper motor & driver instance
|
||||
//StepperMotor motor = StepperMotor(50);
|
||||
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
|
||||
|
||||
// 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();}
|
||||
// If no available hadware interrupt pins use the software interrupt
|
||||
PciListenerImp listenC(sensor.pinC, doC);
|
||||
|
||||
|
||||
// commander interface
|
||||
Commander command = Commander(Serial);
|
||||
void onMotor(char* cmd){ command.motor(&motor, cmd); }
|
||||
|
||||
|
||||
void setup() {
|
||||
|
||||
// initialize encoder sensor hardware
|
||||
sensor.init();
|
||||
sensor.enableInterrupts(doA, doB); //, doC);
|
||||
// software interrupts
|
||||
PciManager.registerListener(&listenC);
|
||||
// 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 = MotionControlType::torque;
|
||||
// contoller configuration based on the controll type
|
||||
motor.PID_velocity.P = 0.2f;
|
||||
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.01f;
|
||||
|
||||
// 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;
|
||||
|
||||
// define the motor id
|
||||
command.add('A', onMotor, "motor");
|
||||
|
||||
// Run user commands to configure and the motor (find the full command list in docs.simplefoc.com)
|
||||
Serial.println(F("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
|
||||
command.run();
|
||||
}
|
||||
+104
@@ -0,0 +1,104 @@
|
||||
/**
|
||||
* 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
|
||||
*
|
||||
* See more info in docs.simplefoc.com/commander_interface
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// magnetic sensor instance - SPI
|
||||
MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10);
|
||||
// magnetic sensor instance - MagneticSensorI2C
|
||||
//MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
|
||||
// magnetic sensor instance - analog output
|
||||
// MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
|
||||
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
|
||||
// Stepper motor & driver instance
|
||||
//StepperMotor motor = StepperMotor(50);
|
||||
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
|
||||
|
||||
// commander interface
|
||||
Commander command = Commander(Serial);
|
||||
void onMotor(char* cmd){ command.motor(&motor, cmd); }
|
||||
|
||||
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 = MotionControlType::torque;
|
||||
|
||||
// contoller configuration based on the control type
|
||||
motor.PID_velocity.P = 0.2f;
|
||||
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.01f;
|
||||
|
||||
// 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;
|
||||
|
||||
// define the motor id
|
||||
command.add('A', onMotor, "motor");
|
||||
|
||||
// Run user commands to configure and the motor (find the full command list in docs.simplefoc.com)
|
||||
Serial.println(F("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
|
||||
command.run();
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
|
||||
# OSC control examples
|
||||
|
||||
OSC - opensoundcontrol.org is a simple message exchange protocol. Widely used in the Audio world it is being hailed as the successor to MIDI. But MIDI itself, and now OSC, has always been about sending and receiving fairly simple control messages, at medium speed, and that makes it well suited for the same kind of task in robotics or other control applications.
|
||||
|
||||
As a protocol it is simple, making implementation quite easy, and while binary, simple enough to be fairly "human readable", which aids in debugging and reduces errors.
|
||||
|
||||
The main advantage of using it is that there is a bunch of ready made software, and also libraries to use in your own programs, neaning you don't have to re-invent these things from scratch.
|
||||
|
||||
## TouchOSC
|
||||
|
||||
The first example shows how to set up control of a motor from TouchOSC. It's a super-fast way to set up a customizable GUI for your motor-project, that runs on your phone...
|
||||
|
||||
## purr-Data
|
||||
|
||||
The second example, "simplefoc\_osc\_esp32\_fullcontrol.ino" allows setting the variables and tuning the motor parameters, in the same way as the serial control but via OSC. The file "osc\_fullcontrol.pd" contains a sample GUI to go with that sketch, allowing the control and tuning of 2 BLDC motors.
|
||||
|
||||
Here a screenshot of what it looks like in purr-Data:
|
||||
|
||||
|
||||
|
||||
|
||||
## Links to software used in examples
|
||||
|
||||
- OSC - opensoundcontrol.org
|
||||
- pD - https://puredata.info
|
||||
- TouchOSC - https://hexler.net/products/touchosc
|
||||
|
||||
BIN
Binary file not shown.
+183
@@ -0,0 +1,183 @@
|
||||
/**
|
||||
* Arduino SimpleFOC + OSC control example
|
||||
*
|
||||
* Simple example to show how you can control SimpleFOC via OSC.
|
||||
*
|
||||
* It's a nice way to work, easier than changing speeds via Seerial text input. It could also be the basis for
|
||||
* a functional UI, for example in a lab, art-gallery or similar situation.
|
||||
*
|
||||
* For this example we used an ESP32 to run the code, a AS5048B I2C absolute encoder
|
||||
* and a generic L298 driver board to drive a Emax 4114 gimbal motor. But there is no reason the OSC part will
|
||||
* not work with any other setup.
|
||||
*
|
||||
* You will need:
|
||||
*
|
||||
* - a working SimpleFOC setup - motor, driver, encoder
|
||||
* - a MCU with WiFi and UDP support, for example an ESP32, or an Arduino with Ethernet Shield
|
||||
* - a device to run an OSC UI
|
||||
* - a configured OSC UI
|
||||
* - a WiFi network
|
||||
*
|
||||
* To do the OSC UI I used TouchOSC from https://hexler.net/products/touchosc
|
||||
* There is an example UI file that works with this sketch, see "layout1.touchosc"
|
||||
* You can open the UI file in 'TouchOSC Editor' (free program) and transfer it to the TouchOSC app on your device.
|
||||
*
|
||||
* Alternatively, there are other OSC UIs which may work, e.g. http://opensoundcontrol.org/implementations
|
||||
*
|
||||
* Using:
|
||||
*
|
||||
* Change the values below to match the WiFi ssid/password of your network.
|
||||
* Load and run the code on your ESP32. Take a note of the IP address of your ESP32.
|
||||
* Load and run the UI in TouchOSC.
|
||||
* Configure TouchOSC to connect to your ESP32.
|
||||
* The first command you send will cause the ESP32 to start sending responses to your TouchOSC device.
|
||||
* After this you will see motor position and speed in the UI.
|
||||
* Have fun controlling your SimpleFOC motors from your smartphone!
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#include "Arduino.h"
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
#include <WiFi.h>
|
||||
#include <WiFiUdp.h>
|
||||
|
||||
#include <OSCMessage.h>
|
||||
#include <OSCBundle.h>
|
||||
#include <OSCBoards.h>
|
||||
#include <Math.h>
|
||||
|
||||
|
||||
const char ssid[] = "myssid"; // your network SSID (name)
|
||||
const char pass[] = "mypassword"; // your network password
|
||||
WiFiUDP Udp;
|
||||
IPAddress outIp(192,168,1,17); // remote IP (not needed for receive)
|
||||
const unsigned int outPort = 8000; // remote port (not needed for receive)
|
||||
const unsigned int inPort = 8000; // local port to listen for UDP packets (here's where we send the packets)
|
||||
|
||||
|
||||
OSCErrorCode error;
|
||||
|
||||
MagneticSensorI2C sensor = MagneticSensorI2C(0x40, 14, 0xFE, 8);
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27);
|
||||
|
||||
// commander interface
|
||||
Commander command = Commander(Serial);
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
|
||||
WiFi.begin(ssid, pass);
|
||||
|
||||
Serial.print("Connecting WiFi ");
|
||||
Serial.println(ssid);
|
||||
while (WiFi.status() != WL_CONNECTED) {
|
||||
delay(500);
|
||||
Serial.print(".");
|
||||
}
|
||||
Udp.begin(inPort);
|
||||
Serial.println();
|
||||
Serial.print("WiFi connected. Local OSC address: ");
|
||||
Serial.print(WiFi.localIP());
|
||||
Serial.print(":");
|
||||
Serial.println(inPort);
|
||||
|
||||
delay(2000);
|
||||
Serial.println("Initializing motor.");
|
||||
|
||||
sensor.init();
|
||||
motor.linkSensor(&sensor);
|
||||
driver.voltage_power_supply = 9;
|
||||
driver.init();
|
||||
motor.linkDriver(&driver);
|
||||
motor.controller = MotionControlType::velocity;
|
||||
motor.PID_velocity.P = 0.2f;
|
||||
motor.PID_velocity.I = 20;
|
||||
motor.PID_velocity.D = 0.001f;
|
||||
motor.PID_velocity.output_ramp = 1000;
|
||||
motor.LPF_velocity.Tf = 0.01f;
|
||||
motor.voltage_limit = 8;
|
||||
//motor.P_angle.P = 20;
|
||||
motor.init();
|
||||
motor.initFOC();
|
||||
|
||||
Serial.println("All initialization complete.");
|
||||
}
|
||||
|
||||
// velocity set point variable
|
||||
float target_velocity = 2.0f;
|
||||
// angle set point variable
|
||||
float target_angle = 1.0f;
|
||||
|
||||
|
||||
void motorControl(OSCMessage &msg){
|
||||
if (msg.isInt(0))
|
||||
target_velocity = radians(msg.getInt(0));
|
||||
else if (msg.isFloat(0))
|
||||
target_velocity = radians(msg.getFloat(0));
|
||||
else if (msg.isDouble(0))
|
||||
target_velocity = radians(msg.getDouble(0));
|
||||
Serial.print("Velocity set to ");
|
||||
Serial.println(target_velocity);
|
||||
}
|
||||
|
||||
void cmdControl(OSCMessage &msg){
|
||||
char cmdStr[16];
|
||||
if (msg.isString(0)) {
|
||||
msg.getString(0,cmdStr,16);
|
||||
command.motor(&motor,cmdStr);
|
||||
}
|
||||
}
|
||||
|
||||
long lastSend = 0;
|
||||
OSCMessage bundleIN;
|
||||
|
||||
void loop() {
|
||||
OSCBundle bundleOUT;
|
||||
|
||||
// FOC algorithm function
|
||||
motor.move(target_velocity);
|
||||
motor.loopFOC();
|
||||
|
||||
|
||||
int size = Udp.parsePacket();
|
||||
if (size > 0) {
|
||||
while (size--) {
|
||||
bundleIN.fill(Udp.read());
|
||||
}
|
||||
if (!bundleIN.hasError()) {
|
||||
bundleIN.dispatch("/mot1/S", motorControl);
|
||||
bundleIN.dispatch("/mot1/C", cmdControl);
|
||||
IPAddress ip = Udp.remoteIP();
|
||||
if (!( ip==outIp )) {
|
||||
Serial.print("New connection from ");
|
||||
Serial.println(ip);
|
||||
outIp = ip;
|
||||
}
|
||||
}
|
||||
else {
|
||||
error = bundleIN.getError();
|
||||
Serial.print("error: ");
|
||||
Serial.println(error);
|
||||
}
|
||||
bundleIN.empty();
|
||||
}
|
||||
else { // don't receive and send in the same loop...
|
||||
long now = millis();
|
||||
if (now - lastSend > 100) {
|
||||
int ang = (int)degrees(motor.shaftAngle()) % 360;
|
||||
if (ang<0) ang = 360-abs(ang);
|
||||
//BOSCBundle's add' returns the OSCMessage so the message's 'add' can be composed together
|
||||
bundleOUT.add("/mot1/A").add((int)ang);
|
||||
bundleOUT.add("/mot1/V").add((int)degrees(motor.shaftVelocity()));
|
||||
Udp.beginPacket(outIp, outPort);
|
||||
bundleOUT.send(Udp);
|
||||
Udp.endPacket();
|
||||
bundleOUT.empty(); // empty the bundle to free room for a new one
|
||||
lastSend = now;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
+351
@@ -0,0 +1,351 @@
|
||||
/**
|
||||
*
|
||||
* Control 2 motors on ESP32 using OSC
|
||||
*
|
||||
* In this example, all the commands available on the serial command interface are also available via OSC.
|
||||
* Also, the example is for 2 motors. If you have only 1 motor, comment out the lines for the second motor.
|
||||
*
|
||||
*
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#include "Arduino.h"
|
||||
#include <Wire.h>
|
||||
#include <SimpleFOC.h>
|
||||
#include "ssid.h" // create this file, which defines the constants MYSSID and MYPASS
|
||||
#include <Wifi.h>
|
||||
#include <WiFiUdp.h>
|
||||
#include <ArduinoOTA.h>
|
||||
|
||||
#include <OSCMessage.h>
|
||||
#include <OSCBundle.h>
|
||||
#include <OSCBoards.h>
|
||||
|
||||
|
||||
const char ssid[] = MYSSID; // your network SSID (name)
|
||||
const char pass[] = MYPASS; // your network password
|
||||
WiFiUDP Udp;
|
||||
IPAddress outIp(192,168,1,13); // remote IP (not needed for receive)
|
||||
const unsigned int outPort = 8000; // remote port (not needed for receive)
|
||||
const unsigned int inPort = 8000; // local port to listen for UDP packets (here's where we send the packets)
|
||||
#define POWER_SUPPLY 7.4f
|
||||
|
||||
|
||||
|
||||
MagneticSensorI2C sensor2 = MagneticSensorI2C(0x40, 14, 0xFE, 8);
|
||||
MagneticSensorI2C sensor1 = MagneticSensorI2C(0x41, 14, 0xFE, 8);
|
||||
BLDCMotor motor1 = BLDCMotor(7);
|
||||
BLDCMotor motor2 = BLDCMotor(7);
|
||||
BLDCDriver3PWM driver1 = BLDCDriver3PWM(25, 26, 27);
|
||||
BLDCDriver3PWM driver2 = BLDCDriver3PWM(0, 4, 16);
|
||||
|
||||
String m1Prefix("/M1");
|
||||
String m2Prefix("/M2");
|
||||
|
||||
|
||||
// we store seperate set-points for each motor, of course
|
||||
float set_point1 = 0.0f;
|
||||
float set_point2 = 0.0f;
|
||||
|
||||
|
||||
OSCErrorCode error;
|
||||
OSCBundle bundleOUT; // outgoing message, gets re-used
|
||||
|
||||
|
||||
|
||||
void setupOTA(const char* hostname) {
|
||||
ArduinoOTA
|
||||
.setPort(8266)
|
||||
.onStart([]() {
|
||||
String type;
|
||||
if (ArduinoOTA.getCommand() == U_FLASH)
|
||||
type = "sketch";
|
||||
else // U_SPIFFS
|
||||
type = "filesystem";
|
||||
|
||||
// NOTE: if updating SPIFFS this would be the place to unmount SPIFFS using SPIFFS.end()
|
||||
Serial.println("Start updating " + type);
|
||||
})
|
||||
.onEnd([]() {
|
||||
Serial.println("\nEnd");
|
||||
})
|
||||
.onProgress([](unsigned int progress, unsigned int total) {
|
||||
Serial.printf("Progress: %u%%\n", (progress / (total / 100)));
|
||||
})
|
||||
.onError([](ota_error_t error) {
|
||||
Serial.printf("Error[%u]: ", error);
|
||||
if (error == OTA_AUTH_ERROR) Serial.println("Auth Failed");
|
||||
else if (error == OTA_BEGIN_ERROR) Serial.println("Begin Failed");
|
||||
else if (error == OTA_CONNECT_ERROR) Serial.println("Connect Failed");
|
||||
else if (error == OTA_RECEIVE_ERROR) Serial.println("Receive Failed");
|
||||
else if (error == OTA_END_ERROR) Serial.println("End Failed");
|
||||
})
|
||||
.setHostname(hostname)
|
||||
.setMdnsEnabled(true);
|
||||
ArduinoOTA.begin();
|
||||
}
|
||||
|
||||
|
||||
void setup() {
|
||||
// set pins low - motor initialization can take some time,
|
||||
// and you don't want current flowing through the other motor while it happens...
|
||||
pinMode(0,OUTPUT);
|
||||
pinMode(4,OUTPUT);
|
||||
pinMode(16,OUTPUT);
|
||||
pinMode(25,OUTPUT);
|
||||
pinMode(26,OUTPUT);
|
||||
pinMode(27,OUTPUT);
|
||||
digitalWrite(0, 0);
|
||||
digitalWrite(4, 0);
|
||||
digitalWrite(16, 0);
|
||||
digitalWrite(25, 0);
|
||||
digitalWrite(26, 0);
|
||||
digitalWrite(27, 0);
|
||||
|
||||
Serial.begin(115200);
|
||||
delay(200);
|
||||
|
||||
WiFi.begin(ssid, pass);
|
||||
|
||||
Serial.print("Connecting WiFi ");
|
||||
Serial.println(ssid);
|
||||
while (WiFi.status() != WL_CONNECTED) {
|
||||
delay(500);
|
||||
Serial.print(".");
|
||||
}
|
||||
Udp.begin(inPort);
|
||||
Serial.println();
|
||||
Serial.print("WiFi connected. Local OSC address: ");
|
||||
Serial.print(WiFi.localIP());
|
||||
Serial.print(":");
|
||||
Serial.println(inPort);
|
||||
|
||||
setupOTA("smallrobot1");
|
||||
|
||||
Serial.println("Initializing motors.");
|
||||
|
||||
Wire.setClock(400000);
|
||||
|
||||
sensor1.init();
|
||||
motor1.linkSensor(&sensor1);
|
||||
driver1.voltage_power_supply = POWER_SUPPLY;
|
||||
driver1.init();
|
||||
motor1.linkDriver(&driver1);
|
||||
motor1.foc_modulation = FOCModulationType::SpaceVectorPWM;
|
||||
motor1.controller = MotionControlType::velocity;
|
||||
motor1.PID_velocity.P = 0.2f;
|
||||
motor1.PID_velocity.I = 20;
|
||||
motor1.PID_velocity.D = 0.001f;
|
||||
motor1.PID_velocity.output_ramp = 1000;
|
||||
motor1.LPF_velocity.Tf = 0.01f;
|
||||
motor1.voltage_limit = POWER_SUPPLY;
|
||||
motor1.P_angle.P = 20;
|
||||
motor1.init();
|
||||
motor1.initFOC();
|
||||
|
||||
sensor2.init();
|
||||
motor2.linkSensor(&sensor2);
|
||||
driver2.voltage_power_supply = POWER_SUPPLY;
|
||||
driver2.init();
|
||||
motor2.linkDriver(&driver2);
|
||||
motor2.foc_modulation = FOCModulationType::SpaceVectorPWM;
|
||||
motor2.controller = MotionControlType::velocity;
|
||||
motor2.PID_velocity.P = 0.2f;
|
||||
motor2.PID_velocity.I = 20;
|
||||
motor2.PID_velocity.D = 0.001f;
|
||||
motor2.PID_velocity.output_ramp = 1000;
|
||||
motor2.LPF_velocity.Tf = 0.01f;
|
||||
motor2.voltage_limit = POWER_SUPPLY;
|
||||
motor2.P_angle.P = 20;
|
||||
motor2.init();
|
||||
motor2.initFOC();
|
||||
|
||||
sendMotorParams(motor1, m1Prefix);
|
||||
sendMotorParams(motor2, m2Prefix);
|
||||
Serial.println("All initialization complete.");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
template <typename T>
|
||||
void sendMessage(String& addr, T datum) {
|
||||
bundleOUT.add(addr.c_str()).add(datum);
|
||||
Udp.beginPacket(outIp, outPort);
|
||||
bundleOUT.send(Udp);
|
||||
Udp.endPacket();
|
||||
bundleOUT.empty(); // empty the bundle to free room for a new one
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
float getNumber(OSCMessage &msg, int index) {
|
||||
if (msg.getType(index)=='i')
|
||||
return msg.getInt(index);
|
||||
if (msg.getType(index)=='f')
|
||||
return msg.getFloat(index);
|
||||
if (msg.getType(index)=='d')
|
||||
return msg.getDouble(index);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void motorCmd(OSCMessage &msg, int offset, BLDCMotor& motor, float* set_point, String& prefix){
|
||||
Serial.print("Command for ");
|
||||
Serial.println(prefix);
|
||||
if (msg.fullMatch("/P", offset)) {
|
||||
motor.PID_velocity.P = getNumber(msg,0);
|
||||
sendMessage(prefix+"/P", motor.PID_velocity.P);
|
||||
}
|
||||
else if (msg.fullMatch("/I", offset)) {
|
||||
motor.PID_velocity.I = getNumber(msg,0);
|
||||
sendMessage(prefix+"/I", motor.PID_velocity.I);
|
||||
}
|
||||
else if (msg.fullMatch("/D", offset)) {
|
||||
motor.PID_velocity.D = getNumber(msg,0);
|
||||
sendMessage(prefix+"/D", motor.PID_velocity.D);
|
||||
}
|
||||
else if (msg.fullMatch("/R", offset)) {
|
||||
motor.PID_velocity.output_ramp = getNumber(msg,0);
|
||||
sendMessage(prefix+"/R", motor.PID_velocity.output_ramp);
|
||||
}
|
||||
else if (msg.fullMatch("/F", offset)) {
|
||||
motor.LPF_velocity.Tf = getNumber(msg,0);
|
||||
sendMessage(prefix+"/F", motor.LPF_velocity.Tf);
|
||||
}
|
||||
else if (msg.fullMatch("/K", offset)) {
|
||||
motor.P_angle.P = getNumber(msg,0);
|
||||
sendMessage(prefix+"/K", motor.P_angle.P);
|
||||
}
|
||||
else if (msg.fullMatch("/N", offset)) {
|
||||
motor.velocity_limit = getNumber(msg,0);
|
||||
sendMessage(prefix+"/N", motor.velocity_limit);
|
||||
}
|
||||
else if (msg.fullMatch("/L", offset)) {
|
||||
motor.voltage_limit = getNumber(msg,0);
|
||||
sendMessage(prefix+"/L", motor.voltage_limit);
|
||||
}
|
||||
else if (msg.fullMatch("/C", offset)) {
|
||||
motor.controller = (MotionControlType)msg.getInt(0);
|
||||
sendMessage(prefix+"/C", motor.controller);
|
||||
}
|
||||
else if (msg.fullMatch("/t", offset)) {
|
||||
*set_point = getNumber(msg,0);
|
||||
sendMessage(prefix+"/3", *set_point); // TODO is the set-point the same as motor.target?
|
||||
Serial.print("Setting set-point to ");
|
||||
Serial.println(*set_point);
|
||||
}
|
||||
else if (msg.fullMatch("/o", offset)) {
|
||||
motor.disable();
|
||||
}
|
||||
else if (msg.fullMatch("/e", offset)) {
|
||||
motor.enable();
|
||||
}
|
||||
else if (msg.fullMatch("/params", offset)) {
|
||||
sendMotorParams(motor, prefix);
|
||||
}
|
||||
else if (msg.fullMatch("/reinit", offset)) {
|
||||
motor.disable();
|
||||
motor.init();
|
||||
motor.initFOC();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void sendMotorMonitoring() {
|
||||
//Serial.println("Sending monitoring...");
|
||||
bundleOUT.add("/M1/0").add(motor1.voltage.q);
|
||||
bundleOUT.add("/M1/1").add(motor1.shaft_velocity);
|
||||
bundleOUT.add("/M1/2").add(motor1.shaft_angle);
|
||||
bundleOUT.add("/M1/3").add(motor1.target);
|
||||
bundleOUT.add("/M2/0").add(motor2.voltage.q);
|
||||
bundleOUT.add("/M2/1").add(motor2.shaft_velocity);
|
||||
bundleOUT.add("/M2/2").add(motor2.shaft_angle);
|
||||
bundleOUT.add("/M2/3").add(motor2.target);
|
||||
// TODO pack it into one message bundleOUT.add("/M2/i").add(motor2.voltage_q).add(motor2.shaft_velocity).add(motor2.shaft_angle).add(motor2.target);
|
||||
Udp.beginPacket(outIp, outPort);
|
||||
bundleOUT.send(Udp);
|
||||
Udp.endPacket();
|
||||
bundleOUT.empty(); // empty the bundle to free room for a new one
|
||||
}
|
||||
|
||||
|
||||
|
||||
void sendMotorParams(BLDCMotor& motor, String& prefix) {
|
||||
bundleOUT.add((prefix+"/P").c_str()).add(motor.PID_velocity.P);
|
||||
bundleOUT.add((prefix+"/I").c_str()).add(motor.PID_velocity.I);
|
||||
bundleOUT.add((prefix+"/D").c_str()).add(motor.PID_velocity.D);
|
||||
bundleOUT.add((prefix+"/R").c_str()).add(motor.PID_velocity.output_ramp);
|
||||
bundleOUT.add((prefix+"/F").c_str()).add(motor.LPF_velocity.Tf);
|
||||
bundleOUT.add((prefix+"/K").c_str()).add(motor.P_angle.P);
|
||||
bundleOUT.add((prefix+"/N").c_str()).add(motor.velocity_limit);
|
||||
bundleOUT.add((prefix+"/L").c_str()).add(motor.voltage_limit);
|
||||
bundleOUT.add((prefix+"/C").c_str()).add(motor.controller);
|
||||
Udp.beginPacket(outIp, outPort);
|
||||
bundleOUT.send(Udp);
|
||||
Udp.endPacket();
|
||||
bundleOUT.empty(); // empty the bundle to free room for a new one
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
long lastSend = 0;
|
||||
OSCMessage bundleIN;
|
||||
int size;
|
||||
|
||||
|
||||
void loop() {
|
||||
|
||||
// FOC algorithm function
|
||||
motor1.loopFOC();
|
||||
motor1.move(set_point1);
|
||||
motor2.loopFOC();
|
||||
motor2.move(set_point2);
|
||||
|
||||
|
||||
int size = Udp.parsePacket();
|
||||
if (size > 0) {
|
||||
while (size--) {
|
||||
bundleIN.fill(Udp.read());
|
||||
}
|
||||
if (!bundleIN.hasError()) {
|
||||
bundleIN.route("/M1", [](OSCMessage& msg, int offset){ motorCmd(msg, offset, motor1, &set_point1, m1Prefix); }, 0);
|
||||
bundleIN.route("/M2", [](OSCMessage& msg, int offset){ motorCmd(msg, offset, motor2, &set_point2, m2Prefix); }, 0);
|
||||
IPAddress ip = Udp.remoteIP();
|
||||
if (!( ip==outIp )) {
|
||||
Serial.print("New connection from ");
|
||||
Serial.println(ip);
|
||||
outIp = ip;
|
||||
sendMotorParams(motor1, m1Prefix);
|
||||
sendMotorParams(motor2, m2Prefix);
|
||||
}
|
||||
}
|
||||
else {
|
||||
error = bundleIN.getError();
|
||||
Serial.print("error: ");
|
||||
Serial.println(error);
|
||||
}
|
||||
bundleIN.empty();
|
||||
}
|
||||
else { // don't receive and send in the same loop...
|
||||
long now = millis();
|
||||
if (now - lastSend > 100) {
|
||||
sendMotorMonitoring();
|
||||
lastSend = now;
|
||||
}
|
||||
}
|
||||
|
||||
ArduinoOTA.handle();
|
||||
}
|
||||
+384
@@ -0,0 +1,384 @@
|
||||
#N struct text float x float y text t;
|
||||
#N canvas 1842 146 1519 1052 12;
|
||||
#X obj 501 697 cnv 15 193 209 empty empty Tuning\ M1 20 12 0 14 #e0e0e0
|
||||
#404040 0;
|
||||
#X obj 787 477 mrpeach/unpackOSC;
|
||||
#X obj 132 586 print oscin;
|
||||
#X obj 787 504 print oscout;
|
||||
#X obj 723 449 spigot;
|
||||
#X obj 774 452 tgl 15 1 empty empty Debug 17 7 0 10 #fcfcfc #000000
|
||||
#000000 1 1;
|
||||
#X msg 591 503 disconnect;
|
||||
#X obj 132 558 spigot;
|
||||
#X obj 114 562 tgl 15 1 empty empty Debug -34 6 0 10 #fcfcfc #000000
|
||||
#000000 0 1;
|
||||
#X obj 132 531 mrpeach/unpackOSC;
|
||||
#X obj 673 477 mrpeach/udpsend;
|
||||
#X obj 132 496 mrpeach/udpreceive 8000;
|
||||
#X obj 673 422 mrpeach/packOSC;
|
||||
#X obj 1043 150 hsl 249 25 -5000 5000 0 0 empty empty Set\ Point\ M1
|
||||
-2 -8 0 10 #fcfcfc #000000 #000000 0 1;
|
||||
#X obj 1044 197 hsl 247 25 -15 15 0 0 empty empty Set\ Point\ M2 -2
|
||||
-8 0 10 #fcfcfc #000000 #000000 12300 1;
|
||||
#X obj 120 153 bng 53 250 50 0 empty empty STOP 14 26 0 10 #fc1204
|
||||
#000000 #ffffff;
|
||||
#X obj 200 102 * 0.10472;
|
||||
#X obj 202 169 hsl 235 30 -520 520 0 0 empty empty Set\ point\ (Velocity)
|
||||
-2 -8 0 10 #fcfcfc #000000 #000000 11500 1;
|
||||
#X obj 673 449 spigot;
|
||||
#X obj 653 452 tgl 15 1 empty empty Enable\ send -71 6 0 10 #fcfcfc
|
||||
#000000 #000000 1 1;
|
||||
#X msg 484 478 connect 192.168.1.43 8000;
|
||||
#X obj 673 395 speedlim 100;
|
||||
#X obj 231 573 mrpeach/routeOSC /M1 /M2;
|
||||
#X obj 231 610 mrpeach/routeOSC /0 /1 /2 /3 /P /I /D /R /F /K /N /L
|
||||
/C;
|
||||
#X obj 326 844 hsl 101 29 0 6.3 0 0 empty empty rad -2 -8 0 10 #fcfcfc
|
||||
#000000 #000000 0 1;
|
||||
#X obj 326 812 % 6.28319;
|
||||
#X obj 326 704 nbx 7 27 -1e+37 1e+37 0 0 empty empty V -5 0 0 18 #fcfcfc
|
||||
#000000 #000000 0.137548 256 3;
|
||||
#X obj 113 804 nbx 7 27 -1e+37 1e+37 0 0 empty empty Set\ point 0 -15
|
||||
0 18 #fcfcfc #000000 #000000 0 256 3;
|
||||
#X obj 326 776 nbx 7 27 -1e+37 1e+37 0 0 empty empty Position -80 0
|
||||
0 18 #fcfcfc #000000 #000000 -348.637 256 3;
|
||||
#X obj 326 740 nbx 7 27 -1e+37 1e+37 0 0 empty empty Velocity -80 0
|
||||
0 18 #fcfcfc #000000 #000000 0.0649328 256 3;
|
||||
#X obj 538 747 nbx 5 14 -1e+37 1e+37 0 0 empty empty P\ gain 0 -8 0
|
||||
10 #fcfcfc #000000 #000000 0.2 256 3;
|
||||
#X obj 537 776 nbx 5 14 -1e+37 1e+37 0 0 empty empty I\ gain 0 -8 0
|
||||
10 #fcfcfc #000000 #000000 20 256 3;
|
||||
#X obj 538 808 nbx 5 14 -1e+37 1e+37 0 0 empty empty D\ gain 0 -8 0
|
||||
10 #fcfcfc #000000 #000000 0.0001 256 3;
|
||||
#X obj 539 838 nbx 5 14 -1e+37 1e+37 0 0 empty empty V\ Ramp 0 -8 0
|
||||
10 #fcfcfc #000000 #000000 1000 256 3;
|
||||
#X obj 539 868 nbx 5 14 -1e+37 1e+37 0 0 empty empty LP\ time 0 -8
|
||||
0 10 #fcfcfc #000000 #000000 0.01 256 3;
|
||||
#X obj 605 747 nbx 5 14 -1e+37 1e+37 0 0 empty empty angP\ gain 0 -8
|
||||
0 10 #fcfcfc #000000 #000000 20 256 3;
|
||||
#X obj 608 779 nbx 5 14 -1e+37 1e+37 0 0 empty empty angP\ lim 0 -8
|
||||
0 10 #fcfcfc #000000 #000000 20 256 3;
|
||||
#X obj 609 836 nbx 5 14 -1e+37 1e+37 0 0 empty empty V\ limit 0 -8
|
||||
0 10 #fcfcfc #000000 #000000 8 256 3;
|
||||
#X obj 122 278 hradio 53 0 1 3 motorselect empty empty 0 -8 0 10 #fcfcfc
|
||||
#000000 #000000 0;
|
||||
#X scalar text 172 305 \; \;;
|
||||
#X obj 122 372 select 0 1 2;
|
||||
#X msg 122 399 prefix /M?;
|
||||
#X msg 149 423 prefix /M1;
|
||||
#X msg 178 445 prefix /M2;
|
||||
#X obj 789 422 mrpeach/packOSC;
|
||||
#X obj 789 395 speedlim 100;
|
||||
#X msg 592 531 typetags \$1;
|
||||
#X obj 571 535 tgl 15 1 empty empty OSC\ type\ tags -80 7 0 10 #ffffff
|
||||
#000000 #000000 1 1;
|
||||
#X text 63 286 Choose Motor, f 7;
|
||||
#X text 137 339 All, f 4;
|
||||
#X text 191 339 M1, f 4;
|
||||
#X text 247 339 M2, f 4;
|
||||
#X text 152 77 RPM;
|
||||
#X obj 1008 148 bng 29 250 50 0 empty empty STOP 2 13 0 10 #fc1204
|
||||
#000000 #ffffff;
|
||||
#X obj 1009 195 bng 29 250 50 0 empty empty STOP 2 13 0 10 #fc1204
|
||||
#000000 #ffffff;
|
||||
#X obj 74 696 hradio 53 0 1 3 empty empty empty 0 -8 0 10 #fcfcfc #000000
|
||||
#000000 1;
|
||||
#X text 8 711 Control;
|
||||
#X text 67 752 Voltage;
|
||||
#X text 124 753 Velocity;
|
||||
#X text 189 754 Position;
|
||||
#X obj 312 101 /;
|
||||
#X obj 312 129 * 6.28319;
|
||||
#X text 424 75 cm, f 4;
|
||||
#X text 393 51 Wheel diameter;
|
||||
#X obj 394 100 * 0.0314159;
|
||||
#X msg 348 636 set \$1;
|
||||
#X msg 376 637 set \$1;
|
||||
#X msg 407 636 set \$1;
|
||||
#X msg 435 636 set \$1;
|
||||
#X msg 466 636 set \$1;
|
||||
#X msg 495 636 set \$1;
|
||||
#X msg 524 637 set \$1;
|
||||
#X msg 554 637 set \$1;
|
||||
#X msg 583 637 set \$1;
|
||||
#X obj 75 898 s osctargetedout;
|
||||
#X obj 75 866 prepend /M1/C;
|
||||
#X obj 773 304 r osctargetedout;
|
||||
#X obj 593 912 prepend /M1/K;
|
||||
#X obj 602 925 prepend /M1/N;
|
||||
#X obj 609 936 prepend /M1/L;
|
||||
#X obj 564 976 s osctargetedout;
|
||||
#X obj 1271 697 cnv 15 193 209 empty empty Tuning\ M2 20 12 0 14 #e0e0e0
|
||||
#404040 0;
|
||||
#X obj 1001 610 mrpeach/routeOSC /0 /1 /2 /3 /P /I /D /R /F /K /N /L
|
||||
/C;
|
||||
#X obj 1096 844 hsl 101 29 0 6.3 0 0 empty empty rad -2 -8 0 10 #fcfcfc
|
||||
#000000 #000000 0 1;
|
||||
#X obj 1096 812 % 6.28319;
|
||||
#X obj 1096 704 nbx 7 27 -1e+37 1e+37 0 0 empty empty V -5 0 0 18 #fcfcfc
|
||||
#000000 #000000 -0.13018 256 3;
|
||||
#X obj 883 804 nbx 7 27 -1e+37 1e+37 0 0 setpointin2 empty Set\ point
|
||||
0 -15 0 18 #fcfcfc #000000 #000000 0 256 3;
|
||||
#X obj 1096 776 nbx 7 27 -1e+37 1e+37 0 0 empty empty Position -80
|
||||
0 0 18 #fcfcfc #000000 #000000 -346.273 256 3;
|
||||
#X obj 1096 740 nbx 7 27 -1e+37 1e+37 0 0 empty empty Velocity -80
|
||||
0 0 18 #fcfcfc #000000 #000000 0.0657255 256 3;
|
||||
#X obj 1308 747 nbx 5 14 -1e+37 1e+37 0 0 empty empty P\ gain 0 -8
|
||||
0 10 #fcfcfc #000000 #000000 0.2 256 3;
|
||||
#X obj 1308 778 nbx 5 14 -1e+37 1e+37 0 0 empty empty I\ gain 0 -8
|
||||
0 10 #fcfcfc #000000 #000000 20 256 3;
|
||||
#X obj 1308 808 nbx 5 14 -1e+37 1e+37 0 0 empty empty D\ gain 0 -8
|
||||
0 10 #fcfcfc #000000 #000000 0.001 256 3;
|
||||
#X obj 1309 838 nbx 5 14 -1e+37 1e+37 0 0 empty empty V\ Ramp 0 -8
|
||||
0 10 #fcfcfc #000000 #000000 1000 256 3;
|
||||
#X obj 1309 868 nbx 5 14 -1e+37 1e+37 0 0 empty empty LP\ time 0 -8
|
||||
0 10 #fcfcfc #000000 #000000 0.01 256 3;
|
||||
#X obj 1375 747 nbx 5 14 -1e+37 1e+37 0 0 empty empty angP\ gain 0
|
||||
-8 0 10 #fcfcfc #000000 #000000 20 256 3;
|
||||
#X obj 1378 779 nbx 5 14 -1e+37 1e+37 0 0 empty empty angP\ lim 0 -8
|
||||
0 10 #fcfcfc #000000 #000000 20 256 3;
|
||||
#X obj 1379 836 nbx 5 14 -1e+37 1e+37 0 0 empty empty V\ limit 0 -8
|
||||
0 10 #fcfcfc #000000 #000000 8 256 3;
|
||||
#X obj 844 696 hradio 53 0 1 3 empty empty empty 0 -8 0 10 #fcfcfc
|
||||
#000000 #000000 1;
|
||||
#X text 778 711 Control;
|
||||
#X text 837 752 Voltage;
|
||||
#X text 894 753 Velocity;
|
||||
#X text 959 754 Position;
|
||||
#X msg 1118 636 set \$1;
|
||||
#X msg 1146 637 set \$1;
|
||||
#X msg 1177 636 set \$1;
|
||||
#X msg 1205 636 set \$1;
|
||||
#X msg 1236 636 set \$1;
|
||||
#X msg 1265 636 set \$1;
|
||||
#X msg 1294 637 set \$1;
|
||||
#X msg 1324 637 set \$1;
|
||||
#X msg 1353 637 set \$1;
|
||||
#X obj 845 898 s osctargetedout;
|
||||
#X obj 1325 976 s osctargetedout;
|
||||
#X obj 1379 936 prepend /M2/L;
|
||||
#X obj 1372 925 prepend /M2/N;
|
||||
#X obj 1364 912 prepend /M2/K;
|
||||
#X obj 1296 947 prepend /M2/F;
|
||||
#X obj 1291 940 prepend /M2/R;
|
||||
#X obj 1287 933 prepend /M2/D;
|
||||
#X obj 1281 925 prepend /M2/I;
|
||||
#X obj 1276 917 prepend /M2/P;
|
||||
#X obj 526 947 prepend /M1/F;
|
||||
#X obj 521 940 prepend /M1/R;
|
||||
#X obj 517 933 prepend /M1/D;
|
||||
#X obj 511 925 prepend /M1/I;
|
||||
#X obj 506 917 prepend /M1/P;
|
||||
#X obj 393 78 nbx 2 14 0 50 0 1 empty empty empty 0 -8 0 10 #fcfcfc
|
||||
#000000 #000000 6 256 2;
|
||||
#X obj 299 71 nbx 5 24 -20 20 0 0 empty empty empty 0 -8 0 10 #fcfcfc
|
||||
#000000 #000000 -0.266666 256 2;
|
||||
#X obj 179 74 nbx 7 23 -5000 5000 0 0 empty empty empty 0 -8 0 10 #fcfcfc
|
||||
#000000 #000000 -84.8824 256 2;
|
||||
#X obj 577 169 hsl 104 30 -3.1415 3.1415 0 0 empty empty Set\ point\ (Position)
|
||||
-2 -8 0 10 #fcfcfc #000000 #000000 10300 1;
|
||||
#X obj 708 68 vsl 31 122 0 12 0 0 empty empty Set\ point\ (Voltage)
|
||||
0 -9 0 10 #fcfcfc #000000 #000000 0 1;
|
||||
#X obj 246 13 / 0.10472;
|
||||
#X msg 318 13 set \$1;
|
||||
#X obj 457 11 *;
|
||||
#X obj 384 11 / 6.28319;
|
||||
#X obj 547 96 /;
|
||||
#X obj 547 125 * 6.28319;
|
||||
#X obj 547 66 nbx 5 24 -100 100 0 0 empty empty empty 0 -8 0 10 #fcfcfc
|
||||
#000000 #000000 0.0599999 256 2;
|
||||
#X text 534 68 m;
|
||||
#X obj 20 6 r setpointin;
|
||||
#X obj 17 36 r setpointin2;
|
||||
#X obj 120 6 spigot;
|
||||
#X obj 120 42 spigot;
|
||||
#X obj 16 63 r motorselect;
|
||||
#X obj 1339 99 r setpointin;
|
||||
#X obj 1345 150 r setpointin2;
|
||||
#X msg 493 11 set \$1;
|
||||
#X obj 22 103 > 1;
|
||||
#X obj 59 103 <= 1;
|
||||
#X msg 1110 529 /M?/params;
|
||||
#X obj 1110 502 loadbang;
|
||||
#X msg 1339 126 set \$1;
|
||||
#X msg 1343 174 set \$1;
|
||||
#X obj 639 9 *;
|
||||
#X obj 566 9 / 6.28319;
|
||||
#X msg 675 9 set \$1;
|
||||
#X obj 483 448 loadbang;
|
||||
#X text 284 74 m/s;
|
||||
#X obj 845 866 prepend /M2/C;
|
||||
#X msg 163 226 sendtyped /M?/t f 0;
|
||||
#X obj 458 224 prepend sendtyped /t f;
|
||||
#X msg 991 301 sendtyped /M2/t f 0;
|
||||
#X msg 983 274 sendtyped /M1/t f 0;
|
||||
#X obj 1051 286 prepend sendtyped /M1/t f;
|
||||
#X obj 1047 322 prepend sendtyped /M2/t f;
|
||||
#X obj 475 171 nbx 7 21 -1e+37 1e+37 0 0 empty empty rad 0 -8 0 10
|
||||
#fcfcfc #000000 #000000 2 256 2;
|
||||
#X connect 1 0 3 0;
|
||||
#X connect 4 0 1 0;
|
||||
#X connect 5 0 4 1;
|
||||
#X connect 6 0 10 0;
|
||||
#X connect 7 0 2 0;
|
||||
#X connect 8 0 7 1;
|
||||
#X connect 9 0 7 0;
|
||||
#X connect 9 0 22 0;
|
||||
#X connect 11 0 9 0;
|
||||
#X connect 12 0 18 0;
|
||||
#X connect 12 0 4 0;
|
||||
#X connect 13 0 163 0;
|
||||
#X connect 14 0 164 0;
|
||||
#X connect 15 0 159 0;
|
||||
#X connect 16 0 17 0;
|
||||
#X connect 17 0 131 0;
|
||||
#X connect 17 0 134 0;
|
||||
#X connect 17 0 160 0;
|
||||
#X connect 18 0 10 0;
|
||||
#X connect 19 0 18 1;
|
||||
#X connect 20 0 10 0;
|
||||
#X connect 21 0 12 0;
|
||||
#X connect 22 0 23 0;
|
||||
#X connect 22 1 82 0;
|
||||
#X connect 23 0 26 0;
|
||||
#X connect 23 1 29 0;
|
||||
#X connect 23 2 28 0;
|
||||
#X connect 23 3 27 0;
|
||||
#X connect 23 4 65 0;
|
||||
#X connect 23 5 66 0;
|
||||
#X connect 23 6 67 0;
|
||||
#X connect 23 7 68 0;
|
||||
#X connect 23 8 69 0;
|
||||
#X connect 23 9 70 0;
|
||||
#X connect 23 10 71 0;
|
||||
#X connect 23 11 72 0;
|
||||
#X connect 23 12 73 0;
|
||||
#X connect 25 0 24 0;
|
||||
#X connect 28 0 25 0;
|
||||
#X connect 30 0 125 0;
|
||||
#X connect 31 0 124 0;
|
||||
#X connect 32 0 123 0;
|
||||
#X connect 33 0 122 0;
|
||||
#X connect 34 0 121 0;
|
||||
#X connect 35 0 77 0;
|
||||
#X connect 36 0 78 0;
|
||||
#X connect 37 0 79 0;
|
||||
#X connect 38 0 40 0;
|
||||
#X connect 40 0 41 0;
|
||||
#X connect 40 1 42 0;
|
||||
#X connect 40 2 43 0;
|
||||
#X connect 41 0 12 0;
|
||||
#X connect 42 0 12 0;
|
||||
#X connect 43 0 12 0;
|
||||
#X connect 44 0 18 0;
|
||||
#X connect 44 0 4 0;
|
||||
#X connect 45 0 44 0;
|
||||
#X connect 46 0 12 0;
|
||||
#X connect 47 0 46 0;
|
||||
#X connect 53 0 162 0;
|
||||
#X connect 54 0 161 0;
|
||||
#X connect 55 0 75 0;
|
||||
#X connect 60 0 61 0;
|
||||
#X connect 61 0 17 0;
|
||||
#X connect 64 0 60 1;
|
||||
#X connect 64 0 133 1;
|
||||
#X connect 64 0 135 1;
|
||||
#X connect 64 0 153 1;
|
||||
#X connect 65 0 30 0;
|
||||
#X connect 66 0 31 0;
|
||||
#X connect 67 0 32 0;
|
||||
#X connect 68 0 33 0;
|
||||
#X connect 69 0 34 0;
|
||||
#X connect 70 0 35 0;
|
||||
#X connect 71 0 36 0;
|
||||
#X connect 72 0 37 0;
|
||||
#X connect 73 0 55 0;
|
||||
#X connect 75 0 74 0;
|
||||
#X connect 76 0 45 0;
|
||||
#X connect 77 0 80 0;
|
||||
#X connect 78 0 80 0;
|
||||
#X connect 79 0 80 0;
|
||||
#X connect 82 0 85 0;
|
||||
#X connect 82 1 88 0;
|
||||
#X connect 82 2 87 0;
|
||||
#X connect 82 3 86 0;
|
||||
#X connect 82 4 102 0;
|
||||
#X connect 82 5 103 0;
|
||||
#X connect 82 6 104 0;
|
||||
#X connect 82 7 105 0;
|
||||
#X connect 82 8 106 0;
|
||||
#X connect 82 9 107 0;
|
||||
#X connect 82 10 108 0;
|
||||
#X connect 82 11 109 0;
|
||||
#X connect 82 12 110 0;
|
||||
#X connect 84 0 83 0;
|
||||
#X connect 87 0 84 0;
|
||||
#X connect 89 0 120 0;
|
||||
#X connect 90 0 119 0;
|
||||
#X connect 91 0 118 0;
|
||||
#X connect 92 0 117 0;
|
||||
#X connect 93 0 116 0;
|
||||
#X connect 94 0 115 0;
|
||||
#X connect 95 0 114 0;
|
||||
#X connect 96 0 113 0;
|
||||
#X connect 97 0 158 0;
|
||||
#X connect 102 0 89 0;
|
||||
#X connect 103 0 90 0;
|
||||
#X connect 104 0 91 0;
|
||||
#X connect 105 0 92 0;
|
||||
#X connect 106 0 93 0;
|
||||
#X connect 107 0 94 0;
|
||||
#X connect 108 0 95 0;
|
||||
#X connect 109 0 96 0;
|
||||
#X connect 110 0 97 0;
|
||||
#X connect 113 0 112 0;
|
||||
#X connect 114 0 112 0;
|
||||
#X connect 115 0 112 0;
|
||||
#X connect 116 0 112 0;
|
||||
#X connect 117 0 112 0;
|
||||
#X connect 118 0 112 0;
|
||||
#X connect 119 0 112 0;
|
||||
#X connect 120 0 112 0;
|
||||
#X connect 121 0 80 0;
|
||||
#X connect 122 0 80 0;
|
||||
#X connect 123 0 80 0;
|
||||
#X connect 124 0 80 0;
|
||||
#X connect 125 0 80 0;
|
||||
#X connect 126 0 64 0;
|
||||
#X connect 127 0 60 0;
|
||||
#X connect 128 0 16 0;
|
||||
#X connect 129 0 165 0;
|
||||
#X connect 130 0 160 0;
|
||||
#X connect 131 0 132 0;
|
||||
#X connect 132 0 128 0;
|
||||
#X connect 133 0 146 0;
|
||||
#X connect 134 0 133 0;
|
||||
#X connect 135 0 136 0;
|
||||
#X connect 136 0 165 0;
|
||||
#X connect 137 0 135 0;
|
||||
#X connect 139 0 141 0;
|
||||
#X connect 140 0 142 0;
|
||||
#X connect 143 0 147 0;
|
||||
#X connect 143 0 148 0;
|
||||
#X connect 144 0 151 0;
|
||||
#X connect 145 0 152 0;
|
||||
#X connect 146 0 127 0;
|
||||
#X connect 147 0 142 1;
|
||||
#X connect 148 0 141 1;
|
||||
#X connect 149 0 44 0;
|
||||
#X connect 150 0 149 0;
|
||||
#X connect 151 0 13 0;
|
||||
#X connect 152 0 14 0;
|
||||
#X connect 153 0 155 0;
|
||||
#X connect 154 0 153 0;
|
||||
#X connect 155 0 137 0;
|
||||
#X connect 156 0 20 0;
|
||||
#X connect 158 0 111 0;
|
||||
#X connect 159 0 44 0;
|
||||
#X connect 160 0 21 0;
|
||||
#X connect 161 0 44 0;
|
||||
#X connect 162 0 44 0;
|
||||
#X connect 163 0 45 0;
|
||||
#X connect 164 0 45 0;
|
||||
#X connect 165 0 160 0;
|
||||
#X connect 165 0 154 0;
|
||||
+4
@@ -0,0 +1,4 @@
|
||||
|
||||
#define MYSSID "yourssid"
|
||||
#define MYPASS "yourpassword"
|
||||
|
||||
BIN
Binary file not shown.
|
After Width: | Height: | Size: 256 KiB |
+124
@@ -0,0 +1,124 @@
|
||||
#include <Arduino.h>
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8);
|
||||
//StepperMotor motor = StepperMotor(50);
|
||||
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
|
||||
MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4);
|
||||
|
||||
|
||||
/**
|
||||
* This measures how closely sensor and electrical angle agree and how much your motor is affected by 'cogging'.
|
||||
* It can be used to investigate how much non linearity there is between what we set (electrical angle) and what we read (sensor angle)
|
||||
* This non linearity could be down to magnet placement, coil winding differences or simply that the magnetic field when travelling through a pole pair is not linear
|
||||
* An alignment error of ~10 degrees and cogging of ~4 degrees is normal for small gimbal.
|
||||
* The following article is an interesting read
|
||||
* https://hackaday.com/2016/02/23/anti-cogging-algorithm-brings-out-the-best-in-your-hobby-brushless-motors/
|
||||
*/
|
||||
void testAlignmentAndCogging(int direction) {
|
||||
|
||||
motor.move(0);
|
||||
_delay(200);
|
||||
|
||||
sensor.update();
|
||||
float initialAngle = sensor.getAngle();
|
||||
|
||||
const int shaft_rotation = 720; // 720 deg test - useful to see repeating cog pattern
|
||||
int sample_count = int(shaft_rotation * motor.pole_pairs); // test every electrical degree
|
||||
|
||||
float stDevSum = 0;
|
||||
|
||||
float mean = 0.0f;
|
||||
float prev_mean = 0.0f;
|
||||
|
||||
|
||||
for (int i = 0; i < sample_count; i++) {
|
||||
|
||||
float electricAngle = (float) direction * i * motor.pole_pairs * shaft_rotation / sample_count;
|
||||
// move and wait
|
||||
motor.move(electricAngle * PI / 180);
|
||||
_delay(5);
|
||||
|
||||
// measure
|
||||
sensor.update();
|
||||
float sensorAngle = (sensor.getAngle() - initialAngle) * 180 / PI;
|
||||
float sensorElectricAngle = sensorAngle * motor.pole_pairs;
|
||||
float electricAngleError = electricAngle - sensorElectricAngle;
|
||||
|
||||
// plot this - especially electricAngleError
|
||||
Serial.print(electricAngle);
|
||||
Serial.print("\t");
|
||||
Serial.print(sensorElectricAngle );
|
||||
Serial.print("\t");
|
||||
Serial.println(electricAngleError);
|
||||
|
||||
// use knuth standard deviation algorithm so that we don't need an array too big for an Uno
|
||||
prev_mean = mean;
|
||||
mean = mean + (electricAngleError-mean)/(i+1);
|
||||
stDevSum = stDevSum + (electricAngleError-mean)*(electricAngleError-prev_mean);
|
||||
|
||||
}
|
||||
|
||||
Serial.println();
|
||||
Serial.println(F("ALIGNMENT AND COGGING REPORT"));
|
||||
Serial.println();
|
||||
Serial.print(F("Direction: "));
|
||||
Serial.println(direction);
|
||||
Serial.print(F("Mean error (alignment): "));
|
||||
Serial.print(mean);
|
||||
Serial.println(" deg (electrical)");
|
||||
Serial.print(F("Standard Deviation (cogging): "));
|
||||
Serial.print(sqrt(stDevSum/sample_count));
|
||||
Serial.println(F(" deg (electrical)"));
|
||||
Serial.println();
|
||||
Serial.println(F("Plotting 3rd column of data (electricAngleError) will likely show sinusoidal cogging pattern with a frequency of 4xpole_pairs per rotation"));
|
||||
Serial.println();
|
||||
|
||||
}
|
||||
|
||||
void setup() {
|
||||
|
||||
Serial.begin(115200);
|
||||
while (!Serial) ;
|
||||
|
||||
// driver config
|
||||
driver.voltage_power_supply = 12;
|
||||
driver.init();
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
motor.voltage_sensor_align = 3;
|
||||
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
|
||||
|
||||
motor.controller = MotionControlType::angle_openloop;
|
||||
motor.voltage_limit=motor.voltage_sensor_align;
|
||||
|
||||
sensor.init();
|
||||
motor.linkSensor(&sensor);
|
||||
|
||||
motor.useMonitoring(Serial);
|
||||
motor.init();
|
||||
motor.initFOC();
|
||||
|
||||
testAlignmentAndCogging(1);
|
||||
|
||||
motor.move(0);
|
||||
Serial.println(F("Press any key to test in CCW direction"));
|
||||
while (!Serial.available()) { }
|
||||
|
||||
testAlignmentAndCogging(-1);
|
||||
|
||||
Serial.println(F("Complete"));
|
||||
|
||||
motor.voltage_limit = 0;
|
||||
motor.move(0);
|
||||
while (true) ; //do nothing;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void loop() {
|
||||
|
||||
}
|
||||
+173
@@ -0,0 +1,173 @@
|
||||
/**
|
||||
* Utility arduino sketch which finds pole pair number of the motor
|
||||
*
|
||||
* To run it just set the correct pin numbers for the BLDC driver and encoder A and B channel as well as the encoder PPR value.
|
||||
*
|
||||
* The program will rotate your motor a specific amount and check how much it moved, and by doing a simple calculation calculate your pole pair number.
|
||||
* The pole pair number will be outputted to the serial terminal.
|
||||
*
|
||||
* If the pole pair number is well estimated your motor will start to spin in voltage mode with 2V target.
|
||||
*
|
||||
* If the code calculates negative pole pair number please invert your encoder A and B channel pins or motor connector.
|
||||
*
|
||||
* Try running this code several times to avoid statistical errors.
|
||||
* > But in general if your motor spins, you have a good pole pairs number.
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// BLDC motor instance
|
||||
// its important to put pole pairs number as 1!!!
|
||||
BLDCMotor motor = BLDCMotor(1);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
|
||||
// Stepper motor instance
|
||||
// its important to put pole pairs number as 1!!!
|
||||
//StepperMotor motor = StepperMotor(1);
|
||||
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
|
||||
|
||||
// Encoder(int encA, int encB , int cpr, int index)
|
||||
Encoder encoder = Encoder(2, 3, 2048);
|
||||
// interrupt routine intialisation
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
|
||||
void setup() {
|
||||
|
||||
// initialise encoder hardware
|
||||
encoder.init();
|
||||
// hardware interrupt enable
|
||||
encoder.enableInterrupts(doA, doB);
|
||||
// link the motor to the sensor
|
||||
motor.linkSensor(&encoder);
|
||||
|
||||
// power supply voltage
|
||||
// default 12V
|
||||
driver.voltage_power_supply = 12;
|
||||
driver.init();
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// pole pairs calculation routine
|
||||
Serial.println("Pole pairs (PP) estimator");
|
||||
Serial.println("-\n");
|
||||
|
||||
float pp_search_voltage = 4; // maximum power_supply_voltage/2
|
||||
float pp_search_angle = 6*M_PI; // search electrical angle to turn
|
||||
|
||||
// move motor to the electrical angle 0
|
||||
motor.controller = MotionControlType::angle_openloop;
|
||||
motor.voltage_limit=pp_search_voltage;
|
||||
motor.move(0);
|
||||
_delay(1000);
|
||||
// read the encoder angle
|
||||
encoder.update();
|
||||
float angle_begin = encoder.getAngle();
|
||||
_delay(50);
|
||||
|
||||
// move the motor slowly to the electrical angle pp_search_angle
|
||||
float motor_angle = 0;
|
||||
while(motor_angle <= pp_search_angle){
|
||||
motor_angle += 0.01f;
|
||||
motor.move(motor_angle);
|
||||
_delay(1);
|
||||
}
|
||||
_delay(1000);
|
||||
// read the encoder value for 180
|
||||
encoder.update();
|
||||
float angle_end = encoder.getAngle();
|
||||
_delay(50);
|
||||
// turn off the motor
|
||||
motor.move(0);
|
||||
_delay(1000);
|
||||
|
||||
// calculate the pole pair number
|
||||
int pp = round((pp_search_angle)/(angle_end-angle_begin));
|
||||
|
||||
Serial.print(F("Estimated PP : "));
|
||||
Serial.println(pp);
|
||||
Serial.println(F("PP = Electrical angle / Encoder angle "));
|
||||
Serial.print(pp_search_angle*180/M_PI);
|
||||
Serial.print("/");
|
||||
Serial.print((angle_end-angle_begin)*180/M_PI);
|
||||
Serial.print(" = ");
|
||||
Serial.println((pp_search_angle)/(angle_end-angle_begin));
|
||||
Serial.println();
|
||||
|
||||
|
||||
// a bit of monitoring the result
|
||||
if(pp <= 0 ){
|
||||
Serial.println(F("PP number cannot be negative"));
|
||||
Serial.println(F(" - Try changing the search_voltage value or motor/encoder configuration."));
|
||||
return;
|
||||
}else if(pp > 30){
|
||||
Serial.println(F("PP number very high, possible error."));
|
||||
}else{
|
||||
Serial.println(F("If PP is estimated well your motor should turn now!"));
|
||||
Serial.println(F(" - If it is not moving try to relaunch the program!"));
|
||||
Serial.println(F(" - You can also try to adjust the target voltage using serial terminal!"));
|
||||
}
|
||||
|
||||
|
||||
// set FOC loop to be used
|
||||
motor.controller = MotionControlType::torque;
|
||||
// set the pole pair number to the motor
|
||||
motor.pole_pairs = pp;
|
||||
//align encoder and start FOC
|
||||
motor.initFOC();
|
||||
_delay(1000);
|
||||
|
||||
Serial.println(F("\n Motor ready."));
|
||||
Serial.println(F("Set the target voltage using serial terminal:"));
|
||||
}
|
||||
|
||||
// uq voltage
|
||||
float target_voltage = 2;
|
||||
|
||||
void loop() {
|
||||
|
||||
// main FOC algorithm function
|
||||
// the faster you run this function the better
|
||||
// Arduino UNO loop ~1kHz
|
||||
// Bluepill loop ~10kHz
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
// velocity, position or voltage (defined in motor.controller)
|
||||
// this function can be run at much lower frequency than loopFOC() function
|
||||
// You can also use motor.move() and set the motor.target in the code
|
||||
motor.move(target_voltage);
|
||||
|
||||
// communicate with the user
|
||||
serialReceiveUserCommand();
|
||||
}
|
||||
|
||||
|
||||
// utility function enabling serial communication with the user to set the target values
|
||||
// this function can be implemented in serialEvent function as well
|
||||
void serialReceiveUserCommand() {
|
||||
|
||||
// a string to hold incoming data
|
||||
static String received_chars;
|
||||
|
||||
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') {
|
||||
|
||||
// change the motor target
|
||||
target_voltage = received_chars.toFloat();
|
||||
Serial.print("Target voltage: ");
|
||||
Serial.println(target_voltage);
|
||||
|
||||
// reset the command buffer
|
||||
received_chars = "";
|
||||
}
|
||||
}
|
||||
}
|
||||
+173
@@ -0,0 +1,173 @@
|
||||
/**
|
||||
* Utility arduino sketch which finds pole pair number of the motor
|
||||
*
|
||||
* To run it just set the correct pin numbers for the BLDC driver and sensor CPR value and chip select pin.
|
||||
*
|
||||
* The program will rotate your motor a specific amount and check how much it moved, and by doing a simple calculation calculate your pole pair number.
|
||||
* The pole pair number will be outputted to the serial terminal.
|
||||
*
|
||||
* If the pole pair number is well estimated your motor will start to spin in voltage mode with 2V target.
|
||||
*
|
||||
* If the code calculates negative pole pair number please invert your motor connector.
|
||||
*
|
||||
* Try running this code several times to avoid statistical errors.
|
||||
* > But in general if your motor spins, you have a good pole pairs number.
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// BLDC motor instance
|
||||
// its important to put pole pairs number as 1!!!
|
||||
BLDCMotor motor = BLDCMotor(1);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
|
||||
// Stepper motor instance
|
||||
// its important to put pole pairs number as 1!!!
|
||||
//StepperMotor motor = StepperMotor(1);
|
||||
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
|
||||
|
||||
// magnetic sensor instance - SPI
|
||||
MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF);
|
||||
// magnetic sensor instance - I2C
|
||||
//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4);
|
||||
// magnetic sensor instance - analog output
|
||||
// MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
|
||||
|
||||
void setup() {
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init();
|
||||
// link the motor to the sensor
|
||||
motor.linkSensor(&sensor);
|
||||
|
||||
// power supply voltage
|
||||
// default 12V
|
||||
driver.voltage_power_supply = 12;
|
||||
driver.init();
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// initialize motor hardware
|
||||
motor.init();
|
||||
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// pole pairs calculation routine
|
||||
Serial.println("Pole pairs (PP) estimator");
|
||||
Serial.println("-\n");
|
||||
|
||||
float pp_search_voltage = 4; // maximum power_supply_voltage/2
|
||||
float pp_search_angle = 6*M_PI; // search electrical angle to turn
|
||||
|
||||
// move motor to the electrical angle 0
|
||||
motor.controller = MotionControlType::angle_openloop;
|
||||
motor.voltage_limit=pp_search_voltage;
|
||||
motor.move(0);
|
||||
_delay(1000);
|
||||
// read the sensor angle
|
||||
sensor.update();
|
||||
float angle_begin = sensor.getAngle();
|
||||
_delay(50);
|
||||
|
||||
// move the motor slowly to the electrical angle pp_search_angle
|
||||
float motor_angle = 0;
|
||||
while(motor_angle <= pp_search_angle){
|
||||
motor_angle += 0.01f;
|
||||
sensor.update(); // keep track of the overflow
|
||||
motor.move(motor_angle);
|
||||
_delay(1);
|
||||
}
|
||||
_delay(1000);
|
||||
// read the sensor value for 180
|
||||
sensor.update();
|
||||
float angle_end = sensor.getAngle();
|
||||
_delay(50);
|
||||
// turn off the motor
|
||||
motor.move(0);
|
||||
_delay(1000);
|
||||
|
||||
// calculate the pole pair number
|
||||
int pp = round((pp_search_angle)/(angle_end-angle_begin));
|
||||
|
||||
Serial.print(F("Estimated PP : "));
|
||||
Serial.println(pp);
|
||||
Serial.println(F("PP = Electrical angle / Encoder angle "));
|
||||
Serial.print(pp_search_angle*180/M_PI);
|
||||
Serial.print(F("/"));
|
||||
Serial.print((angle_end-angle_begin)*180/M_PI);
|
||||
Serial.print(F(" = "));
|
||||
Serial.println((pp_search_angle)/(angle_end-angle_begin));
|
||||
Serial.println();
|
||||
|
||||
|
||||
// a bit of monitoring the result
|
||||
if(pp <= 0 ){
|
||||
Serial.println(F("PP number cannot be negative"));
|
||||
Serial.println(F(" - Try changing the search_voltage value or motor/sensor configuration."));
|
||||
return;
|
||||
}else if(pp > 30){
|
||||
Serial.println(F("PP number very high, possible error."));
|
||||
}else{
|
||||
Serial.println(F("If PP is estimated well your motor should turn now!"));
|
||||
Serial.println(F(" - If it is not moving try to relaunch the program!"));
|
||||
Serial.println(F(" - You can also try to adjust the target voltage using serial terminal!"));
|
||||
}
|
||||
|
||||
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::torque;
|
||||
// set the pole pair number to the motor
|
||||
motor.pole_pairs = pp;
|
||||
//align sensor and start FOC
|
||||
motor.initFOC();
|
||||
_delay(1000);
|
||||
|
||||
Serial.println(F("\n Motor ready."));
|
||||
Serial.println(F("Set the target voltage using serial terminal:"));
|
||||
}
|
||||
|
||||
// uq voltage
|
||||
float target_voltage = 2;
|
||||
|
||||
void loop() {
|
||||
|
||||
// main FOC algorithm function
|
||||
// the faster you run this function the better
|
||||
// Arduino UNO loop ~1kHz
|
||||
// Bluepill loop ~10kHz
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
// velocity, position or voltage (defined in motor.controller)
|
||||
// this function can be run at much lower frequency than loopFOC() function
|
||||
// You can also use motor.move() and set the motor.target in the code
|
||||
motor.move(target_voltage);
|
||||
|
||||
// communicate with the user
|
||||
serialReceiveUserCommand();
|
||||
}
|
||||
|
||||
|
||||
// utility function enabling serial communication with the user to set the target values
|
||||
// this function can be implemented in serialEvent function as well
|
||||
void serialReceiveUserCommand() {
|
||||
|
||||
// a string to hold incoming data
|
||||
static String received_chars;
|
||||
|
||||
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') {
|
||||
|
||||
// change the motor target
|
||||
target_voltage = received_chars.toFloat();
|
||||
Serial.print("Target voltage: ");
|
||||
Serial.println(target_voltage);
|
||||
|
||||
// reset the command buffer
|
||||
received_chars = "";
|
||||
}
|
||||
}
|
||||
}
|
||||
+73
@@ -0,0 +1,73 @@
|
||||
/**
|
||||
* Simple example intended to help users find the zero offset and natural direction of the sensor.
|
||||
*
|
||||
* These values can further be used to avoid motor and sensor alignment procedure.
|
||||
*
|
||||
* motor.initFOC(zero_offset, sensor_direction);
|
||||
*
|
||||
* This will only work for abosolute value sensors - magnetic sensors.
|
||||
* Bypassing the alignment procedure is not possible for the encoders and for the current implementation of the Hall sensors.
|
||||
* library version 1.4.2.
|
||||
*
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// magnetic sensor instance - SPI
|
||||
//MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF);
|
||||
// magnetic sensor instance - I2C
|
||||
//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4);
|
||||
// magnetic sensor instance - analog output
|
||||
MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
|
||||
|
||||
// BLDC motor instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
|
||||
// Stepper motor instance
|
||||
//StepperMotor motor = StepperMotor(50);
|
||||
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
|
||||
|
||||
void setup() {
|
||||
|
||||
// power supply voltage
|
||||
driver.voltage_power_supply = 12;
|
||||
driver.init();
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init();
|
||||
// link the motor to the sensor
|
||||
motor.linkSensor(&sensor);
|
||||
|
||||
// aligning voltage
|
||||
motor.voltage_sensor_align = 7;
|
||||
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::torque;
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align sensor and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
|
||||
Serial.begin(115200);
|
||||
Serial.println("Sensor zero offset is:");
|
||||
Serial.println(motor.zero_electric_angle, 4);
|
||||
Serial.println("Sensor natural direction is: ");
|
||||
Serial.println(motor.sensor_direction == 1 ? "Direction::CW" : "Direction::CCW");
|
||||
|
||||
Serial.println("To use these values provide them to the: motor.initFOC(offset, direction)");
|
||||
_delay(1000);
|
||||
Serial.println("If motor is not moving the alignment procedure was not successfull!!");
|
||||
}
|
||||
|
||||
|
||||
void loop() {
|
||||
|
||||
// main FOC algorithm function
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
motor.move(2);
|
||||
}
|
||||
|
||||
+53
@@ -0,0 +1,53 @@
|
||||
/**
|
||||
* Simple example of custom commands that have nothing to do with the simple foc library
|
||||
*/
|
||||
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// instantiate the commander
|
||||
Commander command = Commander(Serial);
|
||||
|
||||
// led control function
|
||||
void doLed(char* cmd){
|
||||
if(atoi(cmd)) digitalWrite(LED_BUILTIN, HIGH);
|
||||
else digitalWrite(LED_BUILTIN, LOW);
|
||||
};
|
||||
// get analog input
|
||||
void doAnalog(char* cmd){
|
||||
if (cmd[0] == '0') Serial.println(analogRead(A0));
|
||||
else if (cmd[0] == '1') Serial.println(analogRead(A1));
|
||||
else if (cmd[0] == '2') Serial.println(analogRead(A2));
|
||||
else if (cmd[0] == '3') Serial.println(analogRead(A3));
|
||||
else if (cmd[0] == '4') Serial.println(analogRead(A4));
|
||||
};
|
||||
|
||||
void setup() {
|
||||
// define pins
|
||||
pinMode(LED_BUILTIN, OUTPUT);
|
||||
pinMode(A0, INPUT);
|
||||
pinMode(A1, INPUT);
|
||||
pinMode(A2, INPUT);
|
||||
pinMode(A3, INPUT);
|
||||
pinMode(A4, INPUT);
|
||||
|
||||
// Serial port to be used
|
||||
Serial.begin(115200);
|
||||
|
||||
// add new commands
|
||||
command.add('L', doLed, "led on/off");
|
||||
command.add('A', doAnalog, "analog read A0-A4");
|
||||
|
||||
Serial.println(F("Commander listening"));
|
||||
Serial.println(F(" - Send ? to see the node list..."));
|
||||
Serial.println(F(" - Send L0 to turn the led off and L1 to turn it off"));
|
||||
Serial.println(F(" - Send A0-A4 to read the analog pins"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
|
||||
void loop() {
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
_delay(10);
|
||||
}
|
||||
+51
@@ -0,0 +1,51 @@
|
||||
/**
|
||||
* Simple example of how to use the commander without serial - using just strings
|
||||
*/
|
||||
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// instantiate the commander
|
||||
Commander command = Commander();
|
||||
|
||||
// led control function
|
||||
void doLed(char* cmd){
|
||||
if(atoi(cmd)) digitalWrite(LED_BUILTIN, HIGH);
|
||||
else digitalWrite(LED_BUILTIN, LOW);
|
||||
};
|
||||
// get analog input
|
||||
void doAnalog(char* cmd){
|
||||
if (cmd[0] == '0') Serial.println(analogRead(A0));
|
||||
else if (cmd[0] == '1') Serial.println(analogRead(A1));
|
||||
};
|
||||
|
||||
void setup() {
|
||||
// define pins
|
||||
pinMode(LED_BUILTIN, OUTPUT);
|
||||
pinMode(A0, INPUT);
|
||||
pinMode(A1, INPUT);
|
||||
|
||||
// Serial port to be used
|
||||
Serial.begin(115200);
|
||||
|
||||
// add new commands
|
||||
command.add('L', doLed, "led control");
|
||||
command.add('A', doAnalog, "analog read A0-A1");
|
||||
|
||||
Serial.println(F("Commander running"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
|
||||
void loop() {
|
||||
// user communication
|
||||
command.run("?");
|
||||
_delay(2000);
|
||||
command.run("L0");
|
||||
_delay(1000);
|
||||
command.run("A0");
|
||||
_delay(1000);
|
||||
command.run("A1");
|
||||
_delay(1000);
|
||||
command.run("L1");
|
||||
_delay(1000);
|
||||
}
|
||||
+79
@@ -0,0 +1,79 @@
|
||||
/**
|
||||
* A simple example to show how to use the commander with the control loops outside of the scope of the SimpleFOC library
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(5, 10, 6, 8);
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder = Encoder(2, 3, 500);
|
||||
// channel A and B callbacks
|
||||
void doA() { encoder.handleA(); }
|
||||
void doB() { encoder.handleB(); }
|
||||
|
||||
// target voltage to be set to the motor
|
||||
float target_velocity = 0;
|
||||
|
||||
// PID controllers and low pass filters
|
||||
PIDController PIDv{0.05, 1, 0, 100000000, 12};
|
||||
LowPassFilter LPFv{0.01};
|
||||
|
||||
//add communication
|
||||
Commander command = Commander(Serial);
|
||||
void doController(char* cmd) { command.pid(&PIDv, cmd); }
|
||||
void doFilter(char* cmd) { command.lpf(&LPFv, cmd); }
|
||||
void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); }
|
||||
|
||||
|
||||
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.init();
|
||||
// link driver
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// set motion control loop to be used ( doing nothing )
|
||||
motor.torque_controller = TorqueControlType::voltage;
|
||||
motor.controller = MotionControlType::torque;
|
||||
|
||||
// use monitoring with serial
|
||||
Serial.begin(115200);
|
||||
motor.useMonitoring(Serial);
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align sensor and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// subscribe the new commands
|
||||
command.add('C', doController, "tune velocity pid");
|
||||
command.add('F', doFilter, "tune velocity LPF");
|
||||
command.add('T', doTarget, "motor target");
|
||||
|
||||
_delay(1000);
|
||||
Serial.println(F("Commander listening"));
|
||||
Serial.println(F(" - Send ? to see the node list..."));
|
||||
}
|
||||
|
||||
|
||||
|
||||
void loop() {
|
||||
// looping foc
|
||||
motor.loopFOC();
|
||||
|
||||
// calculate voltage
|
||||
float target_voltage = PIDv(target_velocity - LPFv(motor.shaft_velocity));
|
||||
// set the voltage
|
||||
motor.move(target_voltage);
|
||||
|
||||
// user communication
|
||||
command.run();
|
||||
}
|
||||
+36
@@ -0,0 +1,36 @@
|
||||
/**
|
||||
* A simple example of reading step/dir communication
|
||||
* - this example uses hadware interrupts
|
||||
*/
|
||||
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// angle
|
||||
float received_angle = 0;
|
||||
|
||||
// StepDirListener( step_pin, dir_pin, counter_to_value)
|
||||
StepDirListener step_dir = StepDirListener(2, 3, 360.0/200.0); // receive the angle in degrees
|
||||
void onStep() { step_dir.handle(); }
|
||||
|
||||
void setup() {
|
||||
|
||||
Serial.begin(115200);
|
||||
|
||||
// init step and dir pins
|
||||
step_dir.init();
|
||||
// enable interrupts
|
||||
step_dir.enableInterrupt(onStep);
|
||||
// attach the variable to be updated on each step (optional)
|
||||
// the same can be done asynchronously by caling step_dir.getValue();
|
||||
step_dir.attach(&received_angle);
|
||||
|
||||
Serial.println(F("Step/Dir listenning."));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
Serial.print(received_angle);
|
||||
Serial.print("\t");
|
||||
Serial.println(step_dir.getValue());
|
||||
_delay(500);
|
||||
}
|
||||
+44
@@ -0,0 +1,44 @@
|
||||
/**
|
||||
* A simple example of reading step/dir communication
|
||||
* - this example uses software interrupts - this code is intended primarily
|
||||
* for Arduino UNO/Mega and similar boards with very limited number of interrupt pins
|
||||
*/
|
||||
|
||||
#include <SimpleFOC.h>
|
||||
// software interrupt library
|
||||
#include <PciManager.h>
|
||||
#include <PciListenerImp.h>
|
||||
|
||||
|
||||
// angle
|
||||
float received_angle = 0;
|
||||
|
||||
// StepDirListener( step_pin, dir_pin, counter_to_value)
|
||||
StepDirListener step_dir = StepDirListener(4, 5, 2.0f*_PI/200.0); // receive the angle in radians
|
||||
void onStep() { step_dir.handle(); }
|
||||
|
||||
// If no available hadware interrupt pins use the software interrupt
|
||||
PciListenerImp listenStep(step_dir.pin_step, onStep);
|
||||
|
||||
void setup() {
|
||||
|
||||
Serial.begin(115200);
|
||||
|
||||
// init step and dir pins
|
||||
step_dir.init();
|
||||
// enable software interrupts
|
||||
PciManager.registerListener(&listenStep);
|
||||
// attach the variable to be updated on each step (optional)
|
||||
// the same can be done asynchronously by caling step_dir.getValue();
|
||||
step_dir.attach(&received_angle);
|
||||
|
||||
Serial.println(F("Step/Dir listenning."));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
Serial.print(received_angle);
|
||||
Serial.print("\t");
|
||||
Serial.println(step_dir.getValue());
|
||||
_delay(500);
|
||||
}
|
||||
+98
@@ -0,0 +1,98 @@
|
||||
/**
|
||||
* A position control example using step/dir interface to update the motor position
|
||||
*/
|
||||
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// BLDC motor & driver instance
|
||||
BLDCMotor motor = BLDCMotor(11);
|
||||
BLDCDriver3PWM driver = BLDCDriver3PWM(10, 5, 6, 8);
|
||||
|
||||
// Stepper motor & driver instance
|
||||
//StepperMotor motor = StepperMotor(50);
|
||||
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6, 8);
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder = Encoder(2, 3, 500);
|
||||
// channel A and B callbacks
|
||||
void doA() { encoder.handleA(); }
|
||||
void doB() { encoder.handleB(); }
|
||||
|
||||
// StepDirListener( step_pin, dir_pin, counter_to_value)
|
||||
StepDirListener step_dir = StepDirListener(A4, A5, 2.0f*_PI/200.0);
|
||||
void onStep() { step_dir.handle(); }
|
||||
|
||||
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 the motor and the driver
|
||||
motor.linkDriver(&driver);
|
||||
|
||||
// aligning voltage [V]
|
||||
motor.voltage_sensor_align = 3;
|
||||
// index search velocity [rad/s]
|
||||
motor.velocity_index_search = 3;
|
||||
|
||||
// set motion control loop to be used
|
||||
motor.controller = MotionControlType::angle;
|
||||
|
||||
// contoller configuration
|
||||
// default parameters in defaults.h
|
||||
// velocity PI controller parameters
|
||||
motor.PID_velocity.P = 0.2f;
|
||||
motor.PID_velocity.I = 20;
|
||||
motor.PID_velocity.D = 0;
|
||||
// default voltage_power_supply
|
||||
motor.voltage_limit = 12;
|
||||
// jerk control using voltage voltage ramp
|
||||
// default value is 300 volts per sec ~ 0.3V per millisecond
|
||||
motor.PID_velocity.output_ramp = 1000;
|
||||
|
||||
// velocity low pass filtering time constant
|
||||
motor.LPF_velocity.Tf = 0.01f;
|
||||
|
||||
// angle P controller
|
||||
motor.P_angle.P = 10;
|
||||
// maximal velocity of the position control
|
||||
motor.velocity_limit = 100;
|
||||
|
||||
// use monitoring with serial
|
||||
Serial.begin(115200);
|
||||
// comment out if not needed
|
||||
motor.useMonitoring(Serial);
|
||||
|
||||
// initialize motor
|
||||
motor.init();
|
||||
// align encoder and start FOC
|
||||
motor.initFOC();
|
||||
|
||||
// init step and dir pins
|
||||
step_dir.init();
|
||||
// enable interrupts
|
||||
step_dir.enableInterrupt(onStep);
|
||||
// attach the variable to be updated on each step (optional)
|
||||
// the same can be done asynchronously by caling motor.move(step_dir.getValue());
|
||||
step_dir.attach(&motor.target);
|
||||
|
||||
Serial.println(F("Motor ready."));
|
||||
Serial.println(F("Listening to step/dir commands!"));
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
|
||||
// main FOC algorithm function
|
||||
motor.loopFOC();
|
||||
|
||||
// Motion control function
|
||||
motor.move();
|
||||
}
|
||||
+59
@@ -0,0 +1,59 @@
|
||||
/**
|
||||
* An example code for the generic current sensing implementation
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
|
||||
// user defined function for reading the phase currents
|
||||
// returning the value per phase in amps
|
||||
PhaseCurrent_s readCurrentSense(){
|
||||
PhaseCurrent_s c;
|
||||
// dummy example only reading analog pins
|
||||
c.a = analogRead(A0);
|
||||
c.b = analogRead(A1);
|
||||
c.c = analogRead(A2); // if no 3rd current sense set it to 0
|
||||
return(c);
|
||||
}
|
||||
|
||||
// user defined function for intialising the current sense
|
||||
// it is optional and if provided it will be called in current_sense.init()
|
||||
void initCurrentSense(){
|
||||
pinMode(A0,INPUT);
|
||||
pinMode(A1,INPUT);
|
||||
pinMode(A2,INPUT);
|
||||
}
|
||||
|
||||
|
||||
// GenericCurrentSense class constructor
|
||||
// it receives the user defined callback for reading the current sense
|
||||
// and optionally the user defined callback for current sense initialisation
|
||||
GenericCurrentSense current_sense = GenericCurrentSense(readCurrentSense, initCurrentSense);
|
||||
|
||||
|
||||
void setup() {
|
||||
// if callbacks are not provided in the constructor
|
||||
// they can be assigned directly:
|
||||
//current_sense.readCallback = readCurrentSense;
|
||||
//current_sense.initCallback = initCurrentSense;
|
||||
|
||||
// initialise the current sensing
|
||||
current_sense.init();
|
||||
|
||||
|
||||
Serial.begin(115200);
|
||||
Serial.println("Current sense ready.");
|
||||
}
|
||||
|
||||
void loop() {
|
||||
|
||||
PhaseCurrent_s currents = current_sense.getPhaseCurrents();
|
||||
float current_magnitude = current_sense.getDCCurrent();
|
||||
|
||||
Serial.print(currents.a); // milli Amps
|
||||
Serial.print("\t");
|
||||
Serial.print(currents.b); // milli Amps
|
||||
Serial.print("\t");
|
||||
Serial.print(currents.c); // milli Amps
|
||||
Serial.print("\t");
|
||||
Serial.println(current_magnitude); // milli Amps
|
||||
}
|
||||
+36
@@ -0,0 +1,36 @@
|
||||
/**
|
||||
* Testing example code for the Inline current sensing class
|
||||
*/
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// current sensor
|
||||
// shunt resistor value
|
||||
// gain value
|
||||
// pins phase A,B, (C optional)
|
||||
InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2);
|
||||
|
||||
|
||||
void setup() {
|
||||
// initialise the current sensing
|
||||
current_sense.init();
|
||||
|
||||
// for SimpleFOCShield v2.01/v2.0.2
|
||||
current_sense.gain_b *= -1;
|
||||
|
||||
Serial.begin(115200);
|
||||
Serial.println("Current sense ready.");
|
||||
}
|
||||
|
||||
void loop() {
|
||||
|
||||
PhaseCurrent_s currents = current_sense.getPhaseCurrents();
|
||||
float current_magnitude = current_sense.getDCCurrent();
|
||||
|
||||
Serial.print(currents.a*1000); // milli Amps
|
||||
Serial.print("\t");
|
||||
Serial.print(currents.b*1000); // milli Amps
|
||||
Serial.print("\t");
|
||||
Serial.print(currents.c*1000); // milli Amps
|
||||
Serial.print("\t");
|
||||
Serial.println(current_magnitude*1000); // milli Amps
|
||||
}
|
||||
+34
@@ -0,0 +1,34 @@
|
||||
// BLDC driver standalone example
|
||||
#include <SimpleFOC.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);
|
||||
}
|
||||
+35
@@ -0,0 +1,35 @@
|
||||
// BLDC driver standalone example
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// BLDC driver instance
|
||||
BLDCDriver6PWM driver = BLDCDriver6PWM(5, 6, 9,10, 3, 11, 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;
|
||||
// daad_zone [0,1] - default 0.02f - 2%
|
||||
driver.dead_zone = 0.05f;
|
||||
|
||||
// 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);
|
||||
}
|
||||
+39
@@ -0,0 +1,39 @@
|
||||
// Stepper driver standalone example
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
|
||||
// Stepper driver instance
|
||||
// StepperDriver2PWM(pwm1, in1, pwm2, in2, (en1, en2 optional))
|
||||
int in1[] = {4,5};
|
||||
int in2[] = {9,8};
|
||||
StepperDriver2PWM driver = StepperDriver2PWM(3, in1, 10 , in2, 11, 12);
|
||||
|
||||
// StepperDriver2PWM(pwm1, dir1, pwm2, dir2,(en1, en2 optional))
|
||||
// StepperDriver2PWM driver = StepperDriver2PWM(3, 4, 5, 6, 11, 12);
|
||||
|
||||
void setup() {
|
||||
|
||||
// pwm frequency to be used [Hz]
|
||||
// for atmega328 fixed to 32kHz
|
||||
// esp32/stm32/teensy configurable
|
||||
driver.pwm_frequency = 30000;
|
||||
// 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
|
||||
driver.setPwm(3,6);
|
||||
}
|
||||
+34
@@ -0,0 +1,34 @@
|
||||
// Stepper driver standalone example
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
|
||||
// Stepper driver instance
|
||||
// StepperDriver4PWM(ph1A, ph1B, ph2A, ph2B, (en1, en2 optional))
|
||||
StepperDriver4PWM driver = StepperDriver4PWM(5, 6, 9,10, 7, 8);
|
||||
|
||||
void setup() {
|
||||
|
||||
// pwm frequency to be used [Hz]
|
||||
// for atmega328 fixed to 32kHz
|
||||
// esp32/stm32/teensy configurable
|
||||
driver.pwm_frequency = 30000;
|
||||
// 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
|
||||
driver.setPwm(3,6);
|
||||
}
|
||||
+44
@@ -0,0 +1,44 @@
|
||||
/**
|
||||
* Encoder example code
|
||||
*
|
||||
* This is a code intended to test the encoder connections and to demonstrate the encoder setup.
|
||||
*
|
||||
*/
|
||||
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
|
||||
Encoder encoder = Encoder(2, 3, 8192);
|
||||
// interrupt routine intialisation
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// enable/disable quadrature mode
|
||||
encoder.quadrature = Quadrature::ON;
|
||||
|
||||
// check if you need internal pullups
|
||||
encoder.pullup = Pullup::USE_EXTERN;
|
||||
|
||||
// initialise encoder hardware
|
||||
encoder.init();
|
||||
// hardware interrupt enable
|
||||
encoder.enableInterrupts(doA, doB);
|
||||
|
||||
Serial.println("Encoder ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// not doing much for the encoder though
|
||||
encoder.update();
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(encoder.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(encoder.getVelocity());
|
||||
}
|
||||
+57
@@ -0,0 +1,57 @@
|
||||
/**
|
||||
* Encoder example code using only software interrupts
|
||||
*
|
||||
* This is a code intended to test the encoder connections and to
|
||||
* demonstrate the encoder setup fully using software interrupts.
|
||||
* - We use PciManager library: https://github.com/prampec/arduino-pcimanager
|
||||
*
|
||||
* This code will work on Arduino devices but not on STM32 devices
|
||||
*
|
||||
*/
|
||||
|
||||
#include <SimpleFOC.h>
|
||||
// software interrupt library
|
||||
#include <PciManager.h>
|
||||
#include <PciListenerImp.h>
|
||||
|
||||
// encoder instance
|
||||
Encoder encoder = Encoder(A0, A1, 2048);
|
||||
// interrupt routine intialisation
|
||||
void doA(){encoder.handleA();}
|
||||
void doB(){encoder.handleB();}
|
||||
|
||||
// encoder interrupt init
|
||||
PciListenerImp listenerA(encoder.pinA, doA);
|
||||
PciListenerImp listenerB(encoder.pinB, doB);
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// enable/disable quadrature mode
|
||||
encoder.quadrature = Quadrature::ON;
|
||||
|
||||
// check if you need internal pullups
|
||||
encoder.pullup = Pullup::USE_EXTERN;
|
||||
|
||||
// initialise encoder hardware
|
||||
encoder.init();
|
||||
|
||||
// interrupt initialization
|
||||
PciManager.registerListener(&listenerA);
|
||||
PciManager.registerListener(&listenerB);
|
||||
|
||||
Serial.println("Encoder ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// not doing much for the encoder though
|
||||
encoder.update();
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(encoder.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(encoder.getVelocity());
|
||||
}
|
||||
+51
@@ -0,0 +1,51 @@
|
||||
/**
|
||||
* Generic sensor example code
|
||||
*
|
||||
* This is a code intended to demonstrate how to implement the generic sensor class
|
||||
*
|
||||
*/
|
||||
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// sensor reading function example
|
||||
// for the magnetic sensor with analog communication
|
||||
// returning an angle in radians in between 0 and 2PI
|
||||
float readSensor(){
|
||||
return analogRead(A0)*_2PI/1024.0;
|
||||
}
|
||||
|
||||
// sensor intialising function
|
||||
void initSensor(){
|
||||
pinMode(A0,INPUT);
|
||||
}
|
||||
|
||||
// generic sensor class contructor
|
||||
// - read sensor callback
|
||||
// - init sensor callback (optional)
|
||||
GenericSensor sensor = GenericSensor(readSensor, initSensor);
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// if callbacks are not provided in the constructor
|
||||
// they can be assigned directly:
|
||||
//sensor.readCallback = readSensor;
|
||||
//sensor.initCallback = initSensor;
|
||||
|
||||
sensor.init();
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
sensor.update();
|
||||
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(sensor.getVelocity());
|
||||
}
|
||||
+47
@@ -0,0 +1,47 @@
|
||||
/**
|
||||
* Hall sensor example code
|
||||
*
|
||||
* This is a code intended to test the hall sensors connections and to demonstrate the hall sensor setup.
|
||||
*
|
||||
*/
|
||||
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// Hall sensor instance
|
||||
// HallSensor(int hallA, int hallB , int cpr, int index)
|
||||
// - hallA, hallB, hallC - HallSensor A, B and C pins
|
||||
// - pp - pole pairs
|
||||
HallSensor sensor = HallSensor(2, 3, 4, 11);
|
||||
|
||||
// Interrupt routine intialisation
|
||||
// channel A and B callbacks
|
||||
void doA(){sensor.handleA();}
|
||||
void doB(){sensor.handleB();}
|
||||
void doC(){sensor.handleC();}
|
||||
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// check if you need internal pullups
|
||||
sensor.pullup = Pullup::USE_EXTERN;
|
||||
|
||||
// initialise encoder hardware
|
||||
sensor.init();
|
||||
// hardware interrupt enable
|
||||
sensor.enableInterrupts(doA, doB, doC);
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
sensor.update();
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(sensor.getVelocity());
|
||||
}
|
||||
+59
@@ -0,0 +1,59 @@
|
||||
/**
|
||||
* Hall sensors example code using only software interrupts
|
||||
*
|
||||
* This is a code intended to test the hall sensor connections and to
|
||||
* demonstrate the hall sensor setup fully using software interrupts.
|
||||
* - We use PciManager library: https://github.com/prampec/arduino-pcimanager
|
||||
*
|
||||
* This code will work on Arduino devices but not on STM32 devices
|
||||
*/
|
||||
|
||||
#include <SimpleFOC.h>
|
||||
// software interrupt library
|
||||
#include <PciManager.h>
|
||||
#include <PciListenerImp.h>
|
||||
|
||||
// Hall sensor instance
|
||||
// HallSensor(int hallA, int hallB , int cpr, int index)
|
||||
// - hallA, hallB, hallC - HallSensor A, B and C pins
|
||||
// - pp - pole pairs
|
||||
HallSensor sensor = HallSensor(2, 3, 4, 11);
|
||||
|
||||
// Interrupt routine intialisation
|
||||
// channel A and B callbacks
|
||||
void doA(){sensor.handleA();}
|
||||
void doB(){sensor.handleB();}
|
||||
void doC(){sensor.handleC();}
|
||||
// If no available hadware interrupt pins use the software interrupt
|
||||
PciListenerImp listenA(sensor.pinA, doA);
|
||||
PciListenerImp listenB(sensor.pinB, doB);
|
||||
PciListenerImp listenC(sensor.pinC, doC);
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// check if you need internal pullups
|
||||
sensor.pullup = Pullup::USE_EXTERN;
|
||||
|
||||
// initialise encoder hardware
|
||||
sensor.init();
|
||||
|
||||
// software interrupts
|
||||
PciManager.registerListener(&listenA);
|
||||
PciManager.registerListener(&listenB);
|
||||
PciManager.registerListener(&listenC);
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
sensor.update();
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(sensor.getVelocity());
|
||||
}
|
||||
+56
@@ -0,0 +1,56 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
/**
|
||||
* An example to find out the raw max and min count to be provided to the constructor
|
||||
* Spin your motor/sensor/magnet to see what is the maximum output of the sensor and what is the minimum value
|
||||
* And replace values 14 and 1020 with new values. Once when you replace them make sure there is no jump in the angle reading sensor.getAngle().
|
||||
* If there is a jump that means you can still find better values.
|
||||
*/
|
||||
|
||||
/**
|
||||
* Magnetic sensor reading analog voltage on pin A1. This voltage is proportional to rotation position.
|
||||
* Tested on AS5600 magnetic sensor running in 'analog mode'. Note AS5600 works better in 'i2C mode' (less noise) but only supports one sensor per i2c bus.
|
||||
*
|
||||
* MagneticSensorAnalog(uint8_t _pinAnalog, int _min, int _max)
|
||||
* - pinAnalog - the pin that is reading the pwm from magnetic sensor
|
||||
* - 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
|
||||
* - max_raw_count - the largest value read. whilst you might expect it to be 2^10 = 1023 it is often ~ 1020. Note ESP32 will be closer to 4096 with its 12bit ADC
|
||||
*/
|
||||
MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init();
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
int max_count = 0;
|
||||
int min_count = 100000;
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor.update();
|
||||
|
||||
// keep track of min and max
|
||||
if(sensor.raw_count > max_count) max_count = sensor.raw_count;
|
||||
else if(sensor.raw_count < min_count) min_count = sensor.raw_count;
|
||||
|
||||
// display the raw count, and max and min raw count
|
||||
Serial.print("angle:");
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t, raw:");
|
||||
Serial.print(sensor.raw_count);
|
||||
Serial.print("\t, min:");
|
||||
Serial.print(min_count);
|
||||
Serial.print("\t, max:");
|
||||
Serial.println(max_count);
|
||||
delay(100);
|
||||
}
|
||||
+37
@@ -0,0 +1,37 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Magnetic sensor reading analog voltage on pin A1. This voltage is proportional to rotation position.
|
||||
* Tested on AS5600 magnetic sensor running in 'analog mode'. Note AS5600 works better in 'i2C mode' (less noise) but only supports one sensor per i2c bus.
|
||||
*
|
||||
* MagneticSensorAnalog(uint8_t _pinAnalog, int _min, int _max)
|
||||
* - pinAnalog - the pin that is reading the pwm from magnetic sensor
|
||||
* - 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
|
||||
* - max_raw_count - the largest value read. whilst you might expect it to be 2^10 = 1023 it is often ~ 1020. Note ESP32 will be closer to 4096 with its 12bit ADC
|
||||
*/
|
||||
MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init();
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor.update();
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(sensor.getVelocity());
|
||||
}
|
||||
+40
@@ -0,0 +1,40 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
/** Annoyingly some i2c sensors (e.g. AS5600) have a fixed chip address. This means only one of these devices can be addressed on a single bus
|
||||
* This example shows how a second i2c bus can be used to communicate with a second sensor.
|
||||
*/
|
||||
|
||||
MagneticSensorI2C sensor0 = MagneticSensorI2C(AS5600_I2C);
|
||||
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
|
||||
|
||||
|
||||
void setup() {
|
||||
|
||||
Serial.begin(115200);
|
||||
_delay(750);
|
||||
|
||||
Wire.setClock(400000);
|
||||
Wire1.setClock(400000);
|
||||
|
||||
// Normally SimpleFOC will call begin for i2c but with esp32 begin() is the only way to set pins!
|
||||
// It seems safe to call begin multiple times
|
||||
Wire1.begin(19, 23, (uint32_t)400000);
|
||||
|
||||
sensor0.init();
|
||||
sensor1.init(&Wire1);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor0.update();
|
||||
sensor1.update();
|
||||
|
||||
_delay(200);
|
||||
Serial.print(sensor0.getAngle());
|
||||
Serial.print(" - ");
|
||||
Serial.print(sensor1.getAngle());
|
||||
Serial.println();
|
||||
}
|
||||
+39
@@ -0,0 +1,39 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
/** Annoyingly some i2c sensors (e.g. AS5600) have a fixed chip address. This means only one of these devices can be addressed on a single bus
|
||||
* This example shows how a second i2c bus can be used to communicate with a second sensor.
|
||||
*/
|
||||
|
||||
MagneticSensorI2C sensor0 = MagneticSensorI2C(AS5600_I2C);
|
||||
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
|
||||
|
||||
// example of stm32 defining 2nd bus
|
||||
TwoWire Wire1(PB11, PB10);
|
||||
|
||||
|
||||
void setup() {
|
||||
|
||||
Serial.begin(115200);
|
||||
_delay(750);
|
||||
|
||||
Wire.setClock(400000);
|
||||
Wire1.setClock(400000);
|
||||
|
||||
sensor0.init();
|
||||
sensor1.init(&Wire1);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor0.update();
|
||||
sensor1.update();
|
||||
|
||||
_delay(200);
|
||||
Serial.print(sensor0.getAngle());
|
||||
Serial.print(" - ");
|
||||
Serial.print(sensor1.getAngle());
|
||||
Serial.println();
|
||||
}
|
||||
+43
@@ -0,0 +1,43 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// MagneticSensorI2C(uint8_t _chip_address, float _cpr, uint8_t _angle_register_msb)
|
||||
// chip_address I2C chip address
|
||||
// bit_resolution resolution of the sensor
|
||||
// angle_register_msb angle read register msb
|
||||
// bits_used_msb number of used bits in msb register
|
||||
//
|
||||
// make sure to read the chip address and the chip angle register msb value from the datasheet
|
||||
// also in most cases you will need external pull-ups on SDA and SCL lines!!!!!
|
||||
//
|
||||
// For AS5058B
|
||||
// MagneticSensorI2C sensor = MagneticSensorI2C(0x40, 14, 0xFE, 8);
|
||||
|
||||
// Example of AS5600 configuration
|
||||
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
|
||||
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// configure i2C
|
||||
Wire.setClock(400000);
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init();
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor.update();
|
||||
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(sensor.getVelocity());
|
||||
}
|
||||
+49
@@ -0,0 +1,49 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
|
||||
/**
|
||||
* An example to find out the raw max and min count to be provided to the constructor
|
||||
* SPin your motor/sensor/magnet to see what is the maximum output of the sensor and what is the minimum value
|
||||
* And replace values 4 and 904 with new values. Once when you replace them make sure there is no jump in the angle reading sensor.getAngle().
|
||||
* If there is a jump that means you can still find better values.
|
||||
*/
|
||||
MagneticSensorPWM sensor = MagneticSensorPWM(2, 4, 904);
|
||||
void doPWM(){sensor.handlePWM();}
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init();
|
||||
// comment out to use sensor in blocking (non-interrupt) way
|
||||
sensor.enableInterrupt(doPWM);
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
int max_pulse= 0;
|
||||
int min_pulse = 10000;
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor.update();
|
||||
|
||||
// keep track of min and max
|
||||
if(sensor.pulse_length_us > max_pulse) max_pulse = sensor.pulse_length_us;
|
||||
else if(sensor.pulse_length_us < min_pulse) min_pulse = sensor.pulse_length_us;
|
||||
|
||||
// display the raw count, and max and min raw count
|
||||
Serial.print("angle:");
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t, raw:");
|
||||
Serial.print(sensor.pulse_length_us);
|
||||
Serial.print("\t, min:");
|
||||
Serial.print(min_pulse);
|
||||
Serial.print("\t, max:");
|
||||
Serial.println(max_pulse);
|
||||
}
|
||||
+38
@@ -0,0 +1,38 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
|
||||
/**
|
||||
* Magnetic sensor reading pwm signal on pin 2. The pwm duty cycle is proportional to the sensor angle.
|
||||
*
|
||||
* MagneticSensorPWM(uint8_t MagneticSensorPWM, int _min, int _max)
|
||||
* - pinPWM - the pin that is reading the pwm from magnetic sensor
|
||||
* - min_raw_count - the smallest expected reading. Whilst you might expect it to be 0 it is often ~5. Getting this wrong results in a small click once per revolution
|
||||
* - max_raw_count - the largest value read. whilst you might expect it to be 1kHz = 1000 it is often ~910. depending on the exact frequency and saturation
|
||||
*/
|
||||
MagneticSensorPWM sensor = MagneticSensorPWM(2, 4, 904);
|
||||
void doPWM(){sensor.handlePWM();}
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init();
|
||||
// comment out to use sensor in blocking (non-interrupt) way
|
||||
sensor.enableInterrupt(doPWM);
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor.update();
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(sensor.getVelocity());
|
||||
}
|
||||
+44
@@ -0,0 +1,44 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// software interrupt library
|
||||
#include <PciManager.h>
|
||||
#include <PciListenerImp.h>
|
||||
|
||||
/**
|
||||
* Magnetic sensor reading analog voltage on pin which does not have hardware interrupt support. Such as A0.
|
||||
*
|
||||
* MagneticSensorPWM(uint8_t MagneticSensorPWM, int _min, int _max)
|
||||
* - pinPWM - the pin that is reading the pwm from magnetic sensor
|
||||
* - min_raw_count - the smallest expected reading. Whilst you might expect it to be 0 it is often ~5. Getting this wrong results in a small click once per revolution
|
||||
* - max_raw_count - the largest value read. whilst you might expect it to be 1kHz = 1000 it is often ~910. depending on the exact frequency and saturation
|
||||
*/
|
||||
MagneticSensorPWM sensor = MagneticSensorPWM(A0, 4, 904);
|
||||
void doPWM(){sensor.handlePWM();}
|
||||
|
||||
// encoder interrupt init
|
||||
PciListenerImp listenerPWM(sensor.pinPWM, doPWM);
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init();
|
||||
// comment out to use sensor in blocking (non-interrupt) way
|
||||
PciManager.registerListener(&listenerPWM);
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor.update();
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(sensor.getVelocity());
|
||||
}
|
||||
+41
@@ -0,0 +1,41 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// alternative pinout
|
||||
#define HSPI_MISO 12
|
||||
#define HSPI_MOSI 13
|
||||
#define HSPI_SCLK 14
|
||||
#define HSPI_SS 15
|
||||
|
||||
// MagneticSensorSPI(int cs, float _cpr, int _angle_register)
|
||||
// config - SPI config
|
||||
// cs - SPI chip select pin
|
||||
MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, HSPI_SS);
|
||||
|
||||
// for esp 32, it has 2 spi interfaces VSPI (default) and HPSI as the second one
|
||||
// to enable it instatiate the object
|
||||
SPIClass SPI_2(HSPI);
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// start the newly defined spi communication
|
||||
SPI_2.begin(HSPI_SCLK, HSPI_MISO, HSPI_MOSI, HSPI_SS); //SCLK, MISO, MOSI, SS
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init(&SPI_2);
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor.update();
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(sensor.getVelocity());
|
||||
}
|
||||
+32
@@ -0,0 +1,32 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// MagneticSensorSPI(int cs, float _cpr, int _angle_register)
|
||||
// config - SPI config
|
||||
// cs - SPI chip select pin
|
||||
MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, PA15);
|
||||
|
||||
// these are valid pins (mosi, miso, sclk) for 2nd SPI bus on storm32 board (stm32f107rc)
|
||||
SPIClass SPI_2(PB15, PB14, PB13);
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init(&SPI_2);
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor.update();
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(sensor.getVelocity());
|
||||
}
|
||||
+32
@@ -0,0 +1,32 @@
|
||||
#include <SimpleFOC.h>
|
||||
|
||||
// MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs)
|
||||
// config - SPI config
|
||||
// cs - SPI chip select pin
|
||||
// magnetic sensor instance - SPI
|
||||
MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10);
|
||||
// alternative constructor (chipselsect, bit_resolution, angle_read_register, )
|
||||
// MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF);
|
||||
|
||||
void setup() {
|
||||
// monitoring port
|
||||
Serial.begin(115200);
|
||||
|
||||
// initialise magnetic sensor hardware
|
||||
sensor.init();
|
||||
|
||||
Serial.println("Sensor ready");
|
||||
_delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// iterative function updating the sensor internal variables
|
||||
// it is usually called in motor.loopFOC()
|
||||
// this function reads the sensor hardware and
|
||||
// has to be called before getAngle nad getVelocity
|
||||
sensor.update();
|
||||
// display the angle and the angular velocity to the terminal
|
||||
Serial.print(sensor.getAngle());
|
||||
Serial.print("\t");
|
||||
Serial.println(sensor.getVelocity());
|
||||
}
|
||||
@@ -0,0 +1,245 @@
|
||||
ArduinoFOC KEYWORD1
|
||||
SimpleFOC KEYWORD1
|
||||
BLDCMotor KEYWORD1
|
||||
FOCMotor KEYWORD1
|
||||
StepperMotor KEYWORD1
|
||||
Encoder KEYWORD1
|
||||
HallSensors KEYWORD1
|
||||
MagneticSensorSPI KEYWORD1
|
||||
MagneticSensorI2C KEYWORD1
|
||||
MagneticSensorAnalog KEYWORD1
|
||||
MagneticSensorPWM KEYWORD1
|
||||
BLDCDriver3PWM KEYWORD1
|
||||
BLDCDriver6PWM KEYWORD1
|
||||
BLDCDriver KEYWORD1
|
||||
StepperDriver4PWM KEYWORD1
|
||||
StepperDriver2PWM KEYWORD1
|
||||
StepperDriver KEYWORD1
|
||||
PIDController KEYWORD1
|
||||
LowPassFilter KEYWORD1
|
||||
InlineCurrentSense KEYWORD1
|
||||
LowsideCurrentSense KEYWORD1
|
||||
CurrentSense KEYWORD1
|
||||
Commander KEYWORD1
|
||||
StepDirListener KEYWORD1
|
||||
GenericCurrentSense KEYWORD1
|
||||
GenericSensor KEYWORD1
|
||||
|
||||
|
||||
initFOC KEYWORD2
|
||||
loopFOC KEYWORD2
|
||||
disable KEYWORD2
|
||||
|
||||
_delay KEYWORD3
|
||||
_sqrt KEYWORD3
|
||||
_micros KEYWORD3
|
||||
_sin KEYWORD3
|
||||
_cos KEYWORD3
|
||||
_setPwmFrequency KEYWORD3
|
||||
_writeDutyCycle KEYWORD3
|
||||
_round KEYWORD3
|
||||
_sign KEYWORD3
|
||||
_constrain KEYWORD3
|
||||
monitor KEYWORD3
|
||||
command KEYWORD3
|
||||
|
||||
PID_velocity KEYWORD2
|
||||
PID_current_q KEYWORD2
|
||||
PID_current_d KEYWORD2
|
||||
LPF_velocity KEYWORD2
|
||||
LPF_current_q KEYWORD2
|
||||
LPF_current_d KEYWORD2
|
||||
P_angle KEYWORD2
|
||||
LPF_angle KEYWORD2
|
||||
lpf_a KEYWORD2
|
||||
lpf_b KEYWORD2
|
||||
lpf_c KEYWORD2
|
||||
|
||||
MotionControlType KEYWORD1
|
||||
TorqueControlType KEYWORD1
|
||||
FOCModulationType KEYWORD2
|
||||
Quadrature KEYWORD1
|
||||
Pullup KEYWORD1
|
||||
Direction KEYWORD1
|
||||
MagneticSensorI2CConfig_s KEYWORD1
|
||||
MagneticSensorSPIConfig_s KEYWORD1
|
||||
DQVoltage_s KEYWORD1
|
||||
DQCurrent_s KEYWORD1
|
||||
PhaseCurrent_s KEYWORD1
|
||||
|
||||
linkDriver KEYWORD2
|
||||
linkSensor KEYWORD2
|
||||
linkCurrentSense KEYWORD2
|
||||
handleA KEYWORD2
|
||||
handleB KEYWORD2
|
||||
handleIndex KEYWORD2
|
||||
handleC KEYWORD2
|
||||
enableInterrupts KEYWORD2
|
||||
ISR KEYWORD2
|
||||
getVelocity KEYWORD2
|
||||
setPhaseVoltage KEYWORD2
|
||||
getAngle KEYWORD2
|
||||
getMechanicalAngle KEYWORD2
|
||||
getSensorAngle KEYWORD2
|
||||
update KEYWORD2
|
||||
needsSearch KEYWORD2
|
||||
useMonitoring KEYWORD2
|
||||
angleOpenloop KEYWORD2
|
||||
velocityOpenloop KEYWORD2
|
||||
getPhaseCurrents KEYWORD2
|
||||
getFOCCurrents KEYWORD2
|
||||
getDCCurrent KEYWORD2
|
||||
setPwm KEYWORD2
|
||||
driverAlign KEYWORD2
|
||||
driverSync KEYWORD2
|
||||
add KEYWORD2
|
||||
run KEYWORD2
|
||||
attach KEYWORD2
|
||||
enableInterrupt KEYWORD2
|
||||
getValue KEYWORD2
|
||||
handle KEYWORD2
|
||||
scalar KEYWORD2
|
||||
pid KEYWORD2
|
||||
lpf KEYWORD2
|
||||
motor KEYWORD2
|
||||
handlePWM KEYWORD2
|
||||
enableInterrupt KEYWORD2
|
||||
readCallback KEYWORD2
|
||||
initCallback KEYWORD2
|
||||
|
||||
|
||||
|
||||
current KEYWORD2
|
||||
current_measured KEYWORD2
|
||||
shaft_angle_sp KEYWORD2
|
||||
electrical_angle KEYWORD2
|
||||
shaft_velocity_sp KEYWORD2
|
||||
shaft_angle KEYWORD2
|
||||
shaft_velocity KEYWORD2
|
||||
torque_controller KEYWORD2
|
||||
controller KEYWORD2
|
||||
pullup KEYWORD2
|
||||
quadrature KEYWORD2
|
||||
foc_modulation KEYWORD2
|
||||
target KEYWORD2
|
||||
motion KEYWORD2
|
||||
pwm_frequency KEYWORD2
|
||||
dead_zone KEYWORD2
|
||||
gain_a KEYWORD2
|
||||
gain_b KEYWORD2
|
||||
gain_c KEYWORD2
|
||||
skip_align KEYWORD2
|
||||
sensor_direction KEYWORD2
|
||||
sensor_offset KEYWORD2
|
||||
zero_electric_angle KEYWORD2
|
||||
verbose KEYWORD2
|
||||
decimal_places KEYWORD2
|
||||
phase_resistance KEYWORD2
|
||||
modulation_centered KEYWORD2
|
||||
|
||||
|
||||
|
||||
voltage KEYWORD2
|
||||
velocity KEYWORD2
|
||||
velocity_openloop KEYWORD2
|
||||
angle KEYWORD2
|
||||
angle_openloop KEYWORD2
|
||||
torque KEYWORD2
|
||||
dc_current KEYWORD2
|
||||
foc_current KEYWORD2
|
||||
|
||||
|
||||
ON KEYWORD2
|
||||
OFF KEYWORD2
|
||||
CW KEYWORD2
|
||||
CCW KEYWORD2
|
||||
UNKNOWN KEYWORD2
|
||||
|
||||
P KEYWORD2
|
||||
I KEYWORD2
|
||||
D KEYWORD2
|
||||
Tf KEYWORD2
|
||||
voltage_limit KEYWORD2
|
||||
current_limit KEYWORD2
|
||||
output_ramp KEYWORD2
|
||||
limit KEYWORD2
|
||||
velocity_limit KEYWORD2
|
||||
voltage_power_supply KEYWORD2
|
||||
voltage_sensor_align KEYWORD2
|
||||
velocity_index_search KEYWORD2
|
||||
monitor_downsample KEYWORD2
|
||||
monitor_variables KEYWORD2
|
||||
motion_downsample KEYWORD2
|
||||
|
||||
pinA KEYWORD2
|
||||
pinB KEYWORD2
|
||||
pinC KEYWORD2
|
||||
index_pin KEYWORD2
|
||||
|
||||
USE_INTERN KEYWORD2
|
||||
USE_EXTERN KEYWORD2
|
||||
DISABLE KEYWORD2
|
||||
ENABLE KEYWORD2
|
||||
SpaceVectorPWM KEYWORD2
|
||||
SinePWM KEYWORD2
|
||||
Trapezoid_120 KEYWORD2
|
||||
Trapezoid_150 KEYWORD2
|
||||
|
||||
pwmA KEYWORD2
|
||||
pwmB KEYWORD2
|
||||
pwmC KEYWORD2
|
||||
pwm1A KEYWORD2
|
||||
pwm1B KEYWORD2
|
||||
pwm2A KEYWORD2
|
||||
pwm2B KEYWORD2
|
||||
Ualpha KEYWORD2
|
||||
Ubeta KEYWORD2
|
||||
Ua KEYWORD2
|
||||
Ub KEYWORD2
|
||||
Uc KEYWORD2
|
||||
enable_pin KEYWORD2
|
||||
enable_pin1 KEYWORD2
|
||||
enable_pin2 KEYWORD2
|
||||
pole_pairs KEYWORD2
|
||||
dc_a KEYWORD2
|
||||
dc_b KEYWORD2
|
||||
dc_c KEYWORD2
|
||||
|
||||
DEF_POWER_SUPPLY LITERAL1
|
||||
DEF_PID_VEL_P LITERAL1
|
||||
DEF_PID_VEL_I LITERAL1
|
||||
DEF_PID_VEL_D LITERAL1
|
||||
DEF_PID_VEL_RAMP LITERAL1
|
||||
DEF_P_ANGLE_P LITERAL1
|
||||
DEF_PID_VEL_LIMIT LITERAL1
|
||||
DEF_INDEX_SEARCH_TARGET_VELOCITY LITERAL1
|
||||
DEF_VOLTAGE_SENSOR_ALIGN LITERAL1
|
||||
DEF_VEL_FILTER_Tf LITERAL1
|
||||
DEF_CURRENT_LIM LITERAL1
|
||||
DEF_CURR_FILTER_Tf LITERAL1
|
||||
DEF_PID_CURR_LIMIT LITERAL1
|
||||
DEF_PID_CURR_RAMP LITERAL1
|
||||
DEF_PID_CURR_D LITERAL1
|
||||
DEF_PID_CURR_I LITERAL1
|
||||
DEF_PID_CURR_P LITERAL1
|
||||
_2_SQRT3 LITERAL1
|
||||
_1_SQRT3 LITERAL1
|
||||
_SQRT3_2 LITERAL1
|
||||
_SQRT2 LITERAL1
|
||||
_120_D2R LITERAL1
|
||||
_PI_2 LITERAL1
|
||||
_PI_6 LITERAL1
|
||||
_2PI LITERAL1
|
||||
_3PI_2 LITERAL1
|
||||
_PI_3 LITERAL1
|
||||
_SQRT3 LITERAL1
|
||||
_PI LITERAL1
|
||||
_HIGH_Z LITERAL1
|
||||
|
||||
_MON_TARGET LITERAL1
|
||||
_MON_VOLT_Q LITERAL1
|
||||
_MON_VOLT_D LITERAL1
|
||||
_MON_CURR_Q LITERAL1
|
||||
_MON_CURR_D LITERAL1
|
||||
_MON_VEL LITERAL1
|
||||
_MON_ANGLE LITERAL1
|
||||
@@ -0,0 +1,10 @@
|
||||
name=Simple FOC
|
||||
version=2.2.1
|
||||
author=Simplefoc <info@simplefoc.com>
|
||||
maintainer=Simplefoc <info@simplefoc.com>
|
||||
sentence=A library demistifying FOC for BLDC motors
|
||||
paragraph=Simple library intended for hobby comunity to run the BLDC and Stepper motor using FOC algorithm. It is intended to support as many BLDC/Stepper motor+sensor+driver combinations as possible and in the same time maintain simplicity of usage. Library supports Arudino devices such as Arduino UNO, MEGA, NANO and similar, stm32 boards such as Nucleo and Bluepill, ESP32 and Teensy boards.
|
||||
category=Device Control
|
||||
url=https://docs.simplefoc.com
|
||||
architectures=*
|
||||
includes=SimpleFOC.h
|
||||
@@ -0,0 +1,670 @@
|
||||
#include "BLDCMotor.h"
|
||||
|
||||
// BLDCMotor( int pp , float R)
|
||||
// - pp - pole pair number
|
||||
// - R - motor phase resistance
|
||||
BLDCMotor::BLDCMotor(int pp, float _R)
|
||||
: FOCMotor()
|
||||
{
|
||||
// save pole pairs number
|
||||
pole_pairs = pp;
|
||||
// save phase resistance number
|
||||
phase_resistance = _R;
|
||||
// torque control type is voltage by default
|
||||
torque_controller = TorqueControlType::voltage;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
Link the driver which controls the motor
|
||||
*/
|
||||
void BLDCMotor::linkDriver(BLDCDriver* _driver) {
|
||||
driver = _driver;
|
||||
}
|
||||
|
||||
// init hardware pins
|
||||
void BLDCMotor::init() {
|
||||
if (!driver || !driver->initialized) {
|
||||
motor_status = FOCMotorStatus::motor_init_failed;
|
||||
if(monitor_port) monitor_port->println(F("MOT: Init not possible, driver not initialized"));
|
||||
}
|
||||
motor_status = FOCMotorStatus::motor_initializing;
|
||||
if(monitor_port) monitor_port->println(F("MOT: Init"));
|
||||
|
||||
// if no current sensing and the user has set the phase resistance of the motor use current limit to calculate the voltage limit
|
||||
if( !current_sense && _isset(phase_resistance)) {
|
||||
float new_voltage_limit = current_limit * (phase_resistance); // v_lim = current_lim / (3/2 phase resistance) - worst case
|
||||
// use it if it is less then voltage_limit set by the user
|
||||
voltage_limit = new_voltage_limit < voltage_limit ? new_voltage_limit : voltage_limit;
|
||||
}
|
||||
// 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
|
||||
if(current_sense){
|
||||
// current control loop controls voltage
|
||||
PID_current_q.limit = voltage_limit;
|
||||
PID_current_d.limit = voltage_limit;
|
||||
}
|
||||
if(_isset(phase_resistance) || torque_controller != TorqueControlType::voltage){
|
||||
// velocity control loop controls current
|
||||
PID_velocity.limit = current_limit;
|
||||
}else{
|
||||
// velocity control loop controls the voltage
|
||||
PID_velocity.limit = voltage_limit;
|
||||
}
|
||||
P_angle.limit = velocity_limit;
|
||||
|
||||
_delay(500);
|
||||
// enable motor
|
||||
if(monitor_port) monitor_port->println(F("MOT: Enable driver."));
|
||||
enable();
|
||||
_delay(500);
|
||||
motor_status = FOCMotorStatus::motor_uncalibrated;
|
||||
}
|
||||
|
||||
|
||||
// disable motor driver
|
||||
void BLDCMotor::disable()
|
||||
{
|
||||
// set zero to PWM
|
||||
driver->setPwm(0, 0, 0);
|
||||
// disable the driver
|
||||
driver->disable();
|
||||
// motor status update
|
||||
enabled = 0;
|
||||
}
|
||||
// enable motor driver
|
||||
void BLDCMotor::enable()
|
||||
{
|
||||
// enable the driver
|
||||
driver->enable();
|
||||
// set zero to PWM
|
||||
driver->setPwm(0, 0, 0);
|
||||
// motor status update
|
||||
enabled = 1;
|
||||
}
|
||||
|
||||
/**
|
||||
FOC functions
|
||||
*/
|
||||
// FOC initialization function
|
||||
int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction) {
|
||||
int exit_flag = 1;
|
||||
|
||||
motor_status = FOCMotorStatus::motor_calibrating;
|
||||
|
||||
// align motor if necessary
|
||||
// alignment necessary for encoders!
|
||||
if(_isset(zero_electric_offset)){
|
||||
// abosolute zero offset provided - no need to align
|
||||
zero_electric_angle = zero_electric_offset;
|
||||
// set the sensor direction - default CW
|
||||
sensor_direction = _sensor_direction;
|
||||
}
|
||||
|
||||
// sensor and motor alignment - can be skipped
|
||||
// by setting motor.sensor_direction and motor.zero_electric_angle
|
||||
_delay(500);
|
||||
if(sensor){
|
||||
exit_flag *= alignSensor();
|
||||
// added the shaft_angle update
|
||||
sensor->update();
|
||||
shaft_angle = shaftAngle();
|
||||
}else if(monitor_port) monitor_port->println(F("MOT: No sensor."));
|
||||
|
||||
// aligning the current sensor - can be skipped
|
||||
// checks if driver phases are the same as current sense phases
|
||||
// and checks the direction of measuremnt.
|
||||
_delay(500);
|
||||
if(exit_flag){
|
||||
if(current_sense) exit_flag *= alignCurrentSense();
|
||||
else if(monitor_port) monitor_port->println(F("MOT: No current sense."));
|
||||
}
|
||||
|
||||
if(exit_flag){
|
||||
if(monitor_port) monitor_port->println(F("MOT: Ready."));
|
||||
motor_status = FOCMotorStatus::motor_ready;
|
||||
}else{
|
||||
if(monitor_port) monitor_port->println(F("MOT: Init FOC failed."));
|
||||
disable();
|
||||
motor_status = FOCMotorStatus::motor_calib_failed;
|
||||
}
|
||||
|
||||
return exit_flag;
|
||||
}
|
||||
|
||||
// Calibarthe the motor and current sense phases
|
||||
int BLDCMotor::alignCurrentSense() {
|
||||
int exit_flag = 1; // success
|
||||
|
||||
if(monitor_port) monitor_port->println(F("MOT: Align current sense."));
|
||||
|
||||
// align current sense and the driver
|
||||
exit_flag = current_sense->driverAlign(driver, voltage_sensor_align);
|
||||
if(!exit_flag){
|
||||
// error in current sense - phase either not measured or bad connection
|
||||
if(monitor_port) monitor_port->println(F("MOT: Align error!"));
|
||||
exit_flag = 0;
|
||||
}else{
|
||||
// output the alignment status flag
|
||||
if(monitor_port) monitor_port->print(F("MOT: Success: "));
|
||||
if(monitor_port) monitor_port->println(exit_flag);
|
||||
}
|
||||
|
||||
return exit_flag > 0;
|
||||
}
|
||||
|
||||
// Encoder alignment to electrical 0 angle
|
||||
int BLDCMotor::alignSensor() {
|
||||
int exit_flag = 1; //success
|
||||
if(monitor_port) monitor_port->println(F("MOT: Align sensor."));
|
||||
|
||||
// if unknown natural direction
|
||||
if(!_isset(sensor_direction)){
|
||||
// check if sensor needs zero search
|
||||
if(sensor->needsSearch()) exit_flag = absoluteZeroSearch();
|
||||
// stop init if not found index
|
||||
if(!exit_flag) return exit_flag;
|
||||
|
||||
// find natural direction
|
||||
// move one electrical revolution forward
|
||||
for (int i = 0; i <=500; i++ ) {
|
||||
float angle = _3PI_2 + _2PI * i / 500.0f;
|
||||
setPhaseVoltage(voltage_sensor_align, 0, angle);
|
||||
_delay(2);
|
||||
}
|
||||
// take and angle in the middle
|
||||
sensor->update();
|
||||
float mid_angle = sensor->getAngle();
|
||||
// move one electrical revolution backwards
|
||||
for (int i = 500; i >=0; i-- ) {
|
||||
float angle = _3PI_2 + _2PI * i / 500.0f ;
|
||||
setPhaseVoltage(voltage_sensor_align, 0, angle);
|
||||
_delay(2);
|
||||
}
|
||||
sensor->update();
|
||||
float end_angle = sensor->getAngle();
|
||||
setPhaseVoltage(0, 0, 0);
|
||||
_delay(200);
|
||||
// determine the direction the sensor moved
|
||||
if (mid_angle == end_angle) {
|
||||
if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement"));
|
||||
return 0; // failed calibration
|
||||
} else if (mid_angle < end_angle) {
|
||||
if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW"));
|
||||
sensor_direction = Direction::CCW;
|
||||
} else{
|
||||
if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW"));
|
||||
sensor_direction = Direction::CW;
|
||||
}
|
||||
// check pole pair number
|
||||
if(monitor_port) monitor_port->print(F("MOT: PP check: "));
|
||||
float moved = fabs(mid_angle - end_angle);
|
||||
if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher!
|
||||
if(monitor_port) monitor_port->print(F("fail - estimated pp:"));
|
||||
if(monitor_port) monitor_port->println(_2PI/moved,4);
|
||||
}else if(monitor_port) monitor_port->println(F("OK!"));
|
||||
|
||||
}else if(monitor_port) monitor_port->println(F("MOT: Skip dir calib."));
|
||||
|
||||
// zero electric angle not known
|
||||
if(!_isset(zero_electric_angle)){
|
||||
// align the electrical phases of the motor and sensor
|
||||
// set angle -90(270 = 3PI/2) degrees
|
||||
setPhaseVoltage(voltage_sensor_align, 0, _3PI_2);
|
||||
_delay(700);
|
||||
// read the sensor
|
||||
sensor->update();
|
||||
// get the current zero electric angle
|
||||
zero_electric_angle = 0;
|
||||
zero_electric_angle = electricalAngle();
|
||||
//zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs));
|
||||
_delay(20);
|
||||
if(monitor_port){
|
||||
monitor_port->print(F("MOT: Zero elec. angle: "));
|
||||
monitor_port->println(zero_electric_angle);
|
||||
}
|
||||
// stop everything
|
||||
setPhaseVoltage(0, 0, 0);
|
||||
_delay(200);
|
||||
}else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib."));
|
||||
return exit_flag;
|
||||
}
|
||||
|
||||
// Encoder alignment the absolute zero angle
|
||||
// - to the index
|
||||
int BLDCMotor::absoluteZeroSearch() {
|
||||
// sensor precision: this is all ok, as the search happens near the 0-angle, where the precision
|
||||
// of float is sufficient.
|
||||
if(monitor_port) monitor_port->println(F("MOT: Index search..."));
|
||||
// search the absolute zero with small velocity
|
||||
float limit_vel = velocity_limit;
|
||||
float limit_volt = voltage_limit;
|
||||
velocity_limit = velocity_index_search;
|
||||
voltage_limit = voltage_sensor_align;
|
||||
shaft_angle = 0;
|
||||
while(sensor->needsSearch() && shaft_angle < _2PI){
|
||||
angleOpenloop(1.5f*_2PI);
|
||||
// call important for some sensors not to loose count
|
||||
// not needed for the search
|
||||
sensor->update();
|
||||
}
|
||||
// disable motor
|
||||
setPhaseVoltage(0, 0, 0);
|
||||
// reinit the limits
|
||||
velocity_limit = limit_vel;
|
||||
voltage_limit = limit_volt;
|
||||
// check if the zero found
|
||||
if(monitor_port){
|
||||
if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!"));
|
||||
else monitor_port->println(F("MOT: Success!"));
|
||||
}
|
||||
return !sensor->needsSearch();
|
||||
}
|
||||
|
||||
// Iterative function looping FOC algorithm, setting Uq on the Motor
|
||||
// The faster it can be run the better
|
||||
void BLDCMotor::loopFOC() {
|
||||
// update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track
|
||||
// of full rotations otherwise.
|
||||
if (sensor) sensor->update();
|
||||
|
||||
// if open-loop do nothing
|
||||
if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return;
|
||||
|
||||
// if disabled do nothing
|
||||
if(!enabled) return;
|
||||
|
||||
// Needs the update() to be called first
|
||||
// This function will not have numerical issues because it uses Sensor::getMechanicalAngle()
|
||||
// which is in range 0-2PI
|
||||
electrical_angle = electricalAngle();
|
||||
switch (torque_controller) {
|
||||
case TorqueControlType::voltage:
|
||||
// no need to do anything really
|
||||
break;
|
||||
case TorqueControlType::dc_current:
|
||||
if(!current_sense) return;
|
||||
// read overall current magnitude
|
||||
current.q = current_sense->getDCCurrent(electrical_angle);
|
||||
// filter the value values
|
||||
current.q = LPF_current_q(current.q);
|
||||
// calculate the phase voltage
|
||||
voltage.q = PID_current_q(current_sp - current.q);
|
||||
voltage.d = 0;
|
||||
break;
|
||||
case TorqueControlType::foc_current:
|
||||
if(!current_sense) return;
|
||||
// read dq currents
|
||||
current = current_sense->getFOCCurrents(electrical_angle);
|
||||
// filter values
|
||||
current.q = LPF_current_q(current.q);
|
||||
current.d = LPF_current_d(current.d);
|
||||
// calculate the phase voltages
|
||||
voltage.q = PID_current_q(current_sp - current.q);
|
||||
voltage.d = PID_current_d(-current.d);
|
||||
break;
|
||||
default:
|
||||
// no torque control selected
|
||||
if(monitor_port) monitor_port->println(F("MOT: no torque control selected!"));
|
||||
break;
|
||||
}
|
||||
|
||||
// set the phase voltage - FOC heart function :)
|
||||
setPhaseVoltage(voltage.q, voltage.d, electrical_angle);
|
||||
}
|
||||
|
||||
// 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 torque 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) {
|
||||
|
||||
// downsampling (optional)
|
||||
if(motion_cnt++ < motion_downsample) return;
|
||||
motion_cnt = 0;
|
||||
|
||||
// shaft angle/velocity need the update() to be called first
|
||||
// get shaft angle
|
||||
// TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float
|
||||
// For this reason it is NOT precise when the angles become large.
|
||||
// Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem
|
||||
// when switching to a 2-component representation.
|
||||
if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop )
|
||||
shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode
|
||||
// get angular velocity
|
||||
shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated
|
||||
|
||||
// if disabled do nothing
|
||||
if(!enabled) return;
|
||||
// set internal target variable
|
||||
if(_isset(new_target)) target = new_target;
|
||||
|
||||
switch (controller) {
|
||||
case MotionControlType::torque:
|
||||
if(torque_controller == TorqueControlType::voltage){ // if voltage torque control
|
||||
if(!_isset(phase_resistance)) voltage.q = target;
|
||||
else voltage.q = target*phase_resistance;
|
||||
voltage.d = 0;
|
||||
}else{
|
||||
current_sp = target; // if current/foc_current torque control
|
||||
}
|
||||
break;
|
||||
case MotionControlType::angle:
|
||||
// TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when
|
||||
// the angles are large. This results in not being able to command small changes at high position values.
|
||||
// to solve this, the delta-angle has to be calculated in a numerically precise way.
|
||||
// angle set point
|
||||
shaft_angle_sp = target;
|
||||
// calculate velocity set point
|
||||
shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle );
|
||||
// calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation
|
||||
current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control
|
||||
// if torque controlled through voltage
|
||||
if(torque_controller == TorqueControlType::voltage){
|
||||
// use voltage if phase-resistance not provided
|
||||
if(!_isset(phase_resistance)) voltage.q = current_sp;
|
||||
else voltage.q = current_sp*phase_resistance;
|
||||
voltage.d = 0;
|
||||
}
|
||||
break;
|
||||
case MotionControlType::velocity:
|
||||
// velocity set point - sensor precision: this calculation is numerically precise.
|
||||
shaft_velocity_sp = target;
|
||||
// calculate the torque command
|
||||
current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control
|
||||
// if torque controlled through voltage control
|
||||
if(torque_controller == TorqueControlType::voltage){
|
||||
// use voltage if phase-resistance not provided
|
||||
if(!_isset(phase_resistance)) voltage.q = current_sp;
|
||||
else voltage.q = current_sp*phase_resistance;
|
||||
voltage.d = 0;
|
||||
}
|
||||
break;
|
||||
case MotionControlType::velocity_openloop:
|
||||
// velocity control in open loop - sensor precision: this calculation is numerically precise.
|
||||
shaft_velocity_sp = target;
|
||||
voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor
|
||||
voltage.d = 0;
|
||||
break;
|
||||
case MotionControlType::angle_openloop:
|
||||
// angle control in open loop -
|
||||
// TODO sensor precision: this calculation NOT numerically precise, and subject
|
||||
// to the same problems in small set-point changes at high angles
|
||||
// as the closed loop version.
|
||||
shaft_angle_sp = target;
|
||||
voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor
|
||||
voltage.d = 0;
|
||||
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) {
|
||||
|
||||
float center;
|
||||
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] = {
|
||||
{_HIGH_IMPEDANCE,1,-1},{-1,1,_HIGH_IMPEDANCE},{-1,_HIGH_IMPEDANCE,1},{_HIGH_IMPEDANCE,-1,1},{1,-1,_HIGH_IMPEDANCE},{1,_HIGH_IMPEDANCE,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z
|
||||
};
|
||||
// static int trap_120_state = 0;
|
||||
sector = 6 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes
|
||||
// centering the voltages around either
|
||||
// modulation_centered == true > driver.volage_limit/2
|
||||
// modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0
|
||||
center = modulation_centered ? (driver->voltage_limit)/2 : Uq;
|
||||
|
||||
if(trap_120_map[sector][0] == _HIGH_IMPEDANCE){
|
||||
Ua= center;
|
||||
Ub = trap_120_map[sector][1] * Uq + center;
|
||||
Uc = trap_120_map[sector][2] * Uq + center;
|
||||
driver->setPhaseState(_HIGH_IMPEDANCE, _ACTIVE, _ACTIVE); // disable phase if possible
|
||||
}else if(trap_120_map[sector][1] == _HIGH_IMPEDANCE){
|
||||
Ua = trap_120_map[sector][0] * Uq + center;
|
||||
Ub = center;
|
||||
Uc = trap_120_map[sector][2] * Uq + center;
|
||||
driver->setPhaseState(_ACTIVE, _HIGH_IMPEDANCE, _ACTIVE);// disable phase if possible
|
||||
}else{
|
||||
Ua = trap_120_map[sector][0] * Uq + center;
|
||||
Ub = trap_120_map[sector][1] * Uq + center;
|
||||
Uc = center;
|
||||
driver->setPhaseState(_ACTIVE,_ACTIVE, _HIGH_IMPEDANCE);// disable phase if possible
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case FOCModulationType::Trapezoid_150 :
|
||||
// see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8
|
||||
static int trap_150_map[12][3] = {
|
||||
{_HIGH_IMPEDANCE,1,-1},{-1,1,-1},{-1,1,_HIGH_IMPEDANCE},{-1,1,1},{-1,_HIGH_IMPEDANCE,1},{-1,-1,1},{_HIGH_IMPEDANCE,-1,1},{1,-1,1},{1,-1,_HIGH_IMPEDANCE},{1,-1,-1},{1,_HIGH_IMPEDANCE,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z
|
||||
};
|
||||
// static int trap_150_state = 0;
|
||||
sector = 12 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes
|
||||
// centering the voltages around either
|
||||
// modulation_centered == true > driver.volage_limit/2
|
||||
// modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0
|
||||
center = modulation_centered ? (driver->voltage_limit)/2 : Uq;
|
||||
|
||||
if(trap_150_map[sector][0] == _HIGH_IMPEDANCE){
|
||||
Ua= center;
|
||||
Ub = trap_150_map[sector][1] * Uq + center;
|
||||
Uc = trap_150_map[sector][2] * Uq + center;
|
||||
driver->setPhaseState(_HIGH_IMPEDANCE, _ACTIVE, _ACTIVE); // disable phase if possible
|
||||
}else if(trap_150_map[sector][1] == _HIGH_IMPEDANCE){
|
||||
Ua = trap_150_map[sector][0] * Uq + center;
|
||||
Ub = center;
|
||||
Uc = trap_150_map[sector][2] * Uq + center;
|
||||
driver->setPhaseState(_ACTIVE, _HIGH_IMPEDANCE, _ACTIVE);// disable phase if possible
|
||||
}else{
|
||||
Ua = trap_150_map[sector][0] * Uq + center;
|
||||
Ub = trap_150_map[sector][1] * Uq + center;
|
||||
Uc = center;
|
||||
driver->setPhaseState(_ACTIVE, _ACTIVE, _HIGH_IMPEDANCE);// disable phase if possible
|
||||
}
|
||||
|
||||
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);
|
||||
_ca = _cos(angle_el);
|
||||
_sa = _sin(angle_el);
|
||||
// Inverse park transform
|
||||
Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq;
|
||||
Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq;
|
||||
|
||||
// center = modulation_centered ? (driver->voltage_limit)/2 : Uq;
|
||||
center = driver->voltage_limit/2;
|
||||
// Clarke transform
|
||||
Ua = Ualpha + center;
|
||||
Ub = -0.5f * Ualpha + _SQRT3_2 * Ubeta + center;
|
||||
Uc = -0.5f * Ualpha - _SQRT3_2 * Ubeta + center;
|
||||
|
||||
if (!modulation_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
|
||||
|
||||
// the algorithm goes
|
||||
// 1) Ualpha, Ubeta
|
||||
// 2) Uout = sqrt(Ualpha^2 + Ubeta^2)
|
||||
// 3) angle_el = atan2(Ubeta, Ualpha)
|
||||
//
|
||||
// equivalent to 2) because the magnitude does not change is:
|
||||
// Uout = sqrt(Ud^2 + Uq^2)
|
||||
// equivalent to 3) is
|
||||
// angle_el = angle_el + atan2(Uq,Ud)
|
||||
|
||||
float Uout;
|
||||
// a bit of optitmisation
|
||||
if(Ud){ // only if Ud and Uq set
|
||||
// _sqrt is an approx of sqrt (3-4% error)
|
||||
Uout = _sqrt(Ud*Ud + Uq*Uq) / driver->voltage_limit;
|
||||
// angle normalisation in between 0 and 2pi
|
||||
// only necessary if using _sin and _cos - approximation functions
|
||||
angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud));
|
||||
}else{// only Uq available - no need for atan2 and sqrt
|
||||
Uout = Uq / driver->voltage_limit;
|
||||
// angle normalisation in between 0 and 2pi
|
||||
// only necessary if using _sin and _cos - approximation functions
|
||||
angle_el = _normalizeAngle(angle_el + _PI_2);
|
||||
}
|
||||
// find the sector we are in currently
|
||||
sector = floor(angle_el / _PI_3) + 1;
|
||||
// calculate the duty cycles
|
||||
float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uout;
|
||||
float T2 = _SQRT3*_sin(angle_el - (sector-1.0f)*_PI_3) * Uout;
|
||||
// two versions possible
|
||||
float T0 = 0; // pulled to 0 - better for low power supply voltage
|
||||
if (modulation_centered) {
|
||||
T0 = 1 - T1 - T2; // modulation_centered around driver->voltage_limit/2
|
||||
}
|
||||
|
||||
// calculate the duty cycles(times)
|
||||
float Ta,Tb,Tc;
|
||||
switch(sector){
|
||||
case 1:
|
||||
Ta = T1 + T2 + T0/2;
|
||||
Tb = T2 + T0/2;
|
||||
Tc = T0/2;
|
||||
break;
|
||||
case 2:
|
||||
Ta = T1 + T0/2;
|
||||
Tb = T1 + T2 + T0/2;
|
||||
Tc = T0/2;
|
||||
break;
|
||||
case 3:
|
||||
Ta = T0/2;
|
||||
Tb = T1 + T2 + T0/2;
|
||||
Tc = T2 + T0/2;
|
||||
break;
|
||||
case 4:
|
||||
Ta = T0/2;
|
||||
Tb = T1+ T0/2;
|
||||
Tc = T1 + T2 + T0/2;
|
||||
break;
|
||||
case 5:
|
||||
Ta = T2 + T0/2;
|
||||
Tb = T0/2;
|
||||
Tc = T1 + T2 + T0/2;
|
||||
break;
|
||||
case 6:
|
||||
Ta = T1 + T2 + T0/2;
|
||||
Tb = T0/2;
|
||||
Tc = T1 + T0/2;
|
||||
break;
|
||||
default:
|
||||
// possible error state
|
||||
Ta = 0;
|
||||
Tb = 0;
|
||||
Tc = 0;
|
||||
}
|
||||
|
||||
// calculate the phase voltages and center
|
||||
Ua = Ta*driver->voltage_limit;
|
||||
Ub = Tb*driver->voltage_limit;
|
||||
Uc = Tc*driver->voltage_limit;
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
// set the voltages in driver
|
||||
driver->setPwm(Ua, Ub, Uc);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Function (iterative) generating open loop movement for target velocity
|
||||
// - target_velocity - rad/s
|
||||
// it uses voltage_limit variable
|
||||
float 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-6f;
|
||||
// quick fix for strange cases (micros overflow + timestamp not defined)
|
||||
if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
|
||||
|
||||
// calculate the necessary angle to achieve target velocity
|
||||
shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts);
|
||||
// for display purposes
|
||||
shaft_velocity = target_velocity;
|
||||
|
||||
// use voltage limit or current limit
|
||||
float Uq = voltage_limit;
|
||||
if(_isset(phase_resistance)) Uq = current_limit*phase_resistance;
|
||||
|
||||
// set the maximal allowed voltage (voltage_limit) with the necessary angle
|
||||
setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs));
|
||||
|
||||
// save timestamp for next call
|
||||
open_loop_timestamp = now_us;
|
||||
|
||||
return Uq;
|
||||
}
|
||||
|
||||
// Function (iterative) generating open loop movement towards the target angle
|
||||
// - target_angle - rad
|
||||
// it uses voltage_limit and velocity_limit variables
|
||||
float 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-6f;
|
||||
// quick fix for strange cases (micros overflow + timestamp not defined)
|
||||
if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
|
||||
|
||||
// calculate the necessary angle to move from current position towards target angle
|
||||
// with maximal velocity (velocity_limit)
|
||||
// TODO sensor precision: this calculation is not numerically precise. The angle can grow to the point
|
||||
// where small position changes are no longer captured by the precision of floats
|
||||
// when the total position is large.
|
||||
if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)){
|
||||
shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts;
|
||||
shaft_velocity = velocity_limit;
|
||||
}else{
|
||||
shaft_angle = target_angle;
|
||||
shaft_velocity = 0;
|
||||
}
|
||||
|
||||
|
||||
// use voltage limit or current limit
|
||||
float Uq = voltage_limit;
|
||||
if(_isset(phase_resistance)) Uq = current_limit*phase_resistance;
|
||||
// set the maximal allowed voltage (voltage_limit) with the necessary angle
|
||||
// sensor precision: this calculation is OK due to the normalisation
|
||||
setPhaseVoltage(Uq, 0, _electricalAngle(_normalizeAngle(shaft_angle), pole_pairs));
|
||||
|
||||
// save timestamp for next call
|
||||
open_loop_timestamp = now_us;
|
||||
|
||||
return Uq;
|
||||
}
|
||||
@@ -0,0 +1,112 @@
|
||||
#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
|
||||
@param R motor phase resistance
|
||||
*/
|
||||
BLDCMotor(int pp, float R = NOT_SET);
|
||||
|
||||
/**
|
||||
* Function linking a motor and a foc driver
|
||||
*
|
||||
* @param driver BLDCDriver class implementing all the hardware specific functions necessary PWM setting
|
||||
*/
|
||||
void linkDriver(BLDCDriver* driver);
|
||||
|
||||
/**
|
||||
* 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();
|
||||
/** Current sense and motor phase alignment */
|
||||
int alignCurrentSense();
|
||||
/** Motor and sensor alignment to the sensors absolute 0 angle */
|
||||
int absoluteZeroSearch();
|
||||
|
||||
|
||||
// Open loop motion control
|
||||
/**
|
||||
* Function (iterative) generating open loop movement for target velocity
|
||||
* it uses voltage_limit variable
|
||||
*
|
||||
* @param target_velocity - rad/s
|
||||
*/
|
||||
float 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
|
||||
*/
|
||||
float angleOpenloop(float target_angle);
|
||||
// open loop variables
|
||||
long open_loop_timestamp;
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,118 @@
|
||||
/*!
|
||||
* @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.<br> 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 <SimpleFOC.h>
|
||||
|
||||
// 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 = MotionControlType::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/MagneticSensorPWM.h"
|
||||
#include "sensors/HallSensor.h"
|
||||
#include "sensors/GenericSensor.h"
|
||||
#include "drivers/BLDCDriver3PWM.h"
|
||||
#include "drivers/BLDCDriver6PWM.h"
|
||||
#include "drivers/StepperDriver4PWM.h"
|
||||
#include "drivers/StepperDriver2PWM.h"
|
||||
#include "current_sense/InlineCurrentSense.h"
|
||||
#include "current_sense/LowsideCurrentSense.h"
|
||||
#include "current_sense/GenericCurrentSense.h"
|
||||
#include "communication/Commander.h"
|
||||
#include "communication/StepDirListener.h"
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,417 @@
|
||||
#include "StepperMotor.h"
|
||||
|
||||
// StepperMotor(int pp)
|
||||
// - pp - pole pair number
|
||||
// - R - motor phase resistance
|
||||
StepperMotor::StepperMotor(int pp, float _R)
|
||||
: FOCMotor()
|
||||
{
|
||||
// number od pole pairs
|
||||
pole_pairs = pp;
|
||||
// save phase resistance number
|
||||
phase_resistance = _R;
|
||||
|
||||
// torque control type is voltage by default
|
||||
// current and foc_current not supported yet
|
||||
torque_controller = TorqueControlType::voltage;
|
||||
}
|
||||
|
||||
/**
|
||||
Link the driver which controls the motor
|
||||
*/
|
||||
void StepperMotor::linkDriver(StepperDriver* _driver) {
|
||||
driver = _driver;
|
||||
}
|
||||
|
||||
// init hardware pins
|
||||
void StepperMotor::init() {
|
||||
if (!driver || !driver->initialized) {
|
||||
motor_status = FOCMotorStatus::motor_init_failed;
|
||||
if(monitor_port) monitor_port->println(F("MOT: Init not possible, driver not initialized"));
|
||||
}
|
||||
motor_status = FOCMotorStatus::motor_initializing;
|
||||
if(monitor_port) monitor_port->println(F("MOT: Init"));
|
||||
|
||||
// if set the phase resistance of the motor use current limit to calculate the voltage limit
|
||||
if(_isset(phase_resistance)) {
|
||||
float new_voltage_limit = current_limit * (phase_resistance); // v_lim = current_lim / (3/2 phase resistance) - worst case
|
||||
// use it if it is less then voltage_limit set by the user
|
||||
voltage_limit = new_voltage_limit < voltage_limit ? new_voltage_limit : voltage_limit;
|
||||
}
|
||||
// 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
|
||||
if(_isset(phase_resistance)){
|
||||
// velocity control loop controls current
|
||||
PID_velocity.limit = current_limit;
|
||||
}else{
|
||||
PID_velocity.limit = voltage_limit;
|
||||
}
|
||||
P_angle.limit = velocity_limit;
|
||||
|
||||
_delay(500);
|
||||
// enable motor
|
||||
if(monitor_port) monitor_port->println(F("MOT: Enable driver."));
|
||||
enable();
|
||||
_delay(500);
|
||||
|
||||
motor_status = FOCMotorStatus::motor_uncalibrated;
|
||||
}
|
||||
|
||||
|
||||
// disable motor driver
|
||||
void StepperMotor::disable()
|
||||
{
|
||||
// set zero to PWM
|
||||
driver->setPwm(0, 0);
|
||||
// disable driver
|
||||
driver->disable();
|
||||
// motor status update
|
||||
enabled = 0;
|
||||
}
|
||||
// enable motor driver
|
||||
void StepperMotor::enable()
|
||||
{
|
||||
// disable enable
|
||||
driver->enable();
|
||||
// set zero to PWM
|
||||
driver->setPwm(0, 0);
|
||||
// motor status update
|
||||
enabled = 1;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
FOC functions
|
||||
*/
|
||||
// FOC initialization function
|
||||
int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direction ) {
|
||||
int exit_flag = 1;
|
||||
|
||||
motor_status = FOCMotorStatus::motor_calibrating;
|
||||
|
||||
// align motor if necessary
|
||||
// alignment necessary for encoders!
|
||||
if(_isset(zero_electric_offset)){
|
||||
// abosolute zero offset provided - no need to align
|
||||
zero_electric_angle = zero_electric_offset;
|
||||
// set the sensor direction - default CW
|
||||
sensor_direction = _sensor_direction;
|
||||
}
|
||||
|
||||
// sensor and motor alignment - can be skipped
|
||||
// by setting motor.sensor_direction and motor.zero_electric_angle
|
||||
_delay(500);
|
||||
if(sensor){
|
||||
exit_flag *= alignSensor();
|
||||
// added the shaft_angle update
|
||||
sensor->update();
|
||||
shaft_angle = sensor->getAngle();
|
||||
}else if(monitor_port) monitor_port->println(F("MOT: No sensor."));
|
||||
|
||||
if(exit_flag){
|
||||
if(monitor_port) monitor_port->println(F("MOT: Ready."));
|
||||
motor_status = FOCMotorStatus::motor_ready;
|
||||
}else{
|
||||
if(monitor_port) monitor_port->println(F("MOT: Init FOC failed."));
|
||||
motor_status = FOCMotorStatus::motor_calib_failed;
|
||||
disable();
|
||||
}
|
||||
|
||||
return exit_flag;
|
||||
}
|
||||
|
||||
// Encoder alignment to electrical 0 angle
|
||||
int StepperMotor::alignSensor() {
|
||||
int exit_flag = 1; //success
|
||||
if(monitor_port) monitor_port->println(F("MOT: Align sensor."));
|
||||
|
||||
// if unknown natural direction
|
||||
if(!_isset(sensor_direction)){
|
||||
// check if sensor needs zero search
|
||||
if(sensor->needsSearch()) exit_flag = absoluteZeroSearch();
|
||||
// stop init if not found index
|
||||
if(!exit_flag) return exit_flag;
|
||||
|
||||
// find natural direction
|
||||
// move one electrical revolution forward
|
||||
for (int i = 0; i <=500; i++ ) {
|
||||
float angle = _3PI_2 + _2PI * i / 500.0f;
|
||||
setPhaseVoltage(voltage_sensor_align, 0, angle);
|
||||
_delay(2);
|
||||
}
|
||||
// take and angle in the middle
|
||||
sensor->update();
|
||||
float mid_angle = sensor->getAngle();
|
||||
// move one electrical revolution backwards
|
||||
for (int i = 500; i >=0; i-- ) {
|
||||
float angle = _3PI_2 + _2PI * i / 500.0f ;
|
||||
setPhaseVoltage(voltage_sensor_align, 0, angle);
|
||||
_delay(2);
|
||||
}
|
||||
sensor->update();
|
||||
float end_angle = sensor->getAngle();
|
||||
setPhaseVoltage(0, 0, 0);
|
||||
_delay(200);
|
||||
// determine the direction the sensor moved
|
||||
if (mid_angle == end_angle) {
|
||||
if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement"));
|
||||
return 0; // failed calibration
|
||||
} else if (mid_angle < end_angle) {
|
||||
if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW"));
|
||||
sensor_direction = Direction::CCW;
|
||||
} else{
|
||||
if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW"));
|
||||
sensor_direction = Direction::CW;
|
||||
}
|
||||
// check pole pair number
|
||||
if(monitor_port) monitor_port->print(F("MOT: PP check: "));
|
||||
float moved = fabs(mid_angle - end_angle);
|
||||
if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher!
|
||||
if(monitor_port) monitor_port->print(F("fail - estimated pp:"));
|
||||
if(monitor_port) monitor_port->println(_2PI/moved,4);
|
||||
}else if(monitor_port) monitor_port->println(F("OK!"));
|
||||
|
||||
}else if(monitor_port) monitor_port->println(F("MOT: Skip dir calib."));
|
||||
|
||||
// zero electric angle not known
|
||||
if(!_isset(zero_electric_angle)){
|
||||
// align the electrical phases of the motor and sensor
|
||||
// set angle -90(270 = 3PI/2) degrees
|
||||
setPhaseVoltage(voltage_sensor_align, 0, _3PI_2);
|
||||
_delay(700);
|
||||
// read the sensor
|
||||
sensor->update();
|
||||
// get the current zero electric angle
|
||||
zero_electric_angle = 0;
|
||||
zero_electric_angle = electricalAngle();
|
||||
_delay(20);
|
||||
if(monitor_port){
|
||||
monitor_port->print(F("MOT: Zero elec. angle: "));
|
||||
monitor_port->println(zero_electric_angle);
|
||||
}
|
||||
// stop everything
|
||||
setPhaseVoltage(0, 0, 0);
|
||||
_delay(200);
|
||||
}else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib."));
|
||||
return exit_flag;
|
||||
}
|
||||
|
||||
// Encoder alignment the absolute zero angle
|
||||
// - to the index
|
||||
int StepperMotor::absoluteZeroSearch() {
|
||||
|
||||
if(monitor_port) monitor_port->println(F("MOT: Index search..."));
|
||||
// search the absolute zero with small velocity
|
||||
float limit_vel = velocity_limit;
|
||||
float limit_volt = voltage_limit;
|
||||
velocity_limit = velocity_index_search;
|
||||
voltage_limit = voltage_sensor_align;
|
||||
shaft_angle = 0;
|
||||
while(sensor->needsSearch() && shaft_angle < _2PI){
|
||||
angleOpenloop(1.5f*_2PI);
|
||||
// call important for some sensors not to loose count
|
||||
// not needed for the search
|
||||
sensor->update();
|
||||
}
|
||||
// disable motor
|
||||
setPhaseVoltage(0, 0, 0);
|
||||
// reinit the limits
|
||||
velocity_limit = limit_vel;
|
||||
voltage_limit = limit_volt;
|
||||
// check if the zero found
|
||||
if(monitor_port){
|
||||
if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!"));
|
||||
else monitor_port->println(F("MOT: Success!"));
|
||||
}
|
||||
return !sensor->needsSearch();
|
||||
}
|
||||
|
||||
|
||||
// Iterative function looping FOC algorithm, setting Uq on the Motor
|
||||
// The faster it can be run the better
|
||||
void StepperMotor::loopFOC() {
|
||||
|
||||
// update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track
|
||||
// of full rotations otherwise.
|
||||
if (sensor) sensor->update();
|
||||
|
||||
// if open-loop do nothing
|
||||
if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return;
|
||||
// shaft angle
|
||||
shaft_angle = shaftAngle();
|
||||
|
||||
// if disabled do nothing
|
||||
if(!enabled) return;
|
||||
|
||||
// Needs the update() to be called first
|
||||
// This function will not have numerical issues because it uses Sensor::getMechanicalAngle()
|
||||
// which is in range 0-2PI
|
||||
electrical_angle = electricalAngle();
|
||||
|
||||
// set the phase voltage - FOC heart function :)
|
||||
setPhaseVoltage(voltage.q, voltage.d, electrical_angle);
|
||||
}
|
||||
|
||||
// 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) {
|
||||
|
||||
// downsampling (optional)
|
||||
if(motion_cnt++ < motion_downsample) return;
|
||||
motion_cnt = 0;
|
||||
|
||||
// shaft angle/velocity need the update() to be called first
|
||||
// get shaft angle
|
||||
// TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float
|
||||
// For this reason it is NOT precise when the angles become large.
|
||||
// Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem
|
||||
// when switching to a 2-component representation.
|
||||
if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop )
|
||||
shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode
|
||||
// get angular velocity
|
||||
shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated
|
||||
|
||||
// if disabled do nothing
|
||||
if(!enabled) return;
|
||||
|
||||
// set internal target variable
|
||||
if(_isset(new_target) ) target = new_target;
|
||||
// choose control loop
|
||||
switch (controller) {
|
||||
case MotionControlType::torque:
|
||||
if(!_isset(phase_resistance)) voltage.q = target; // if voltage torque control
|
||||
else voltage.q = target*phase_resistance;
|
||||
voltage.d = 0;
|
||||
break;
|
||||
case MotionControlType::angle:
|
||||
// angle set point
|
||||
shaft_angle_sp = target;
|
||||
// calculate velocity set point
|
||||
shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle );
|
||||
// calculate the torque command
|
||||
current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control
|
||||
// if torque controlled through voltage
|
||||
// use voltage if phase-resistance not provided
|
||||
if(!_isset(phase_resistance)) voltage.q = current_sp;
|
||||
else voltage.q = current_sp*phase_resistance;
|
||||
voltage.d = 0;
|
||||
break;
|
||||
case MotionControlType::velocity:
|
||||
// velocity set point
|
||||
shaft_velocity_sp = target;
|
||||
// calculate the torque command
|
||||
current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control
|
||||
// if torque controlled through voltage control
|
||||
// use voltage if phase-resistance not provided
|
||||
if(!_isset(phase_resistance)) voltage.q = current_sp;
|
||||
else voltage.q = current_sp*phase_resistance;
|
||||
voltage.d = 0;
|
||||
break;
|
||||
case MotionControlType::velocity_openloop:
|
||||
// velocity control in open loop
|
||||
shaft_velocity_sp = target;
|
||||
voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor
|
||||
voltage.d = 0;
|
||||
break;
|
||||
case MotionControlType::angle_openloop:
|
||||
// angle control in open loop
|
||||
shaft_angle_sp = target;
|
||||
voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor
|
||||
voltage.d = 0;
|
||||
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);
|
||||
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
|
||||
float 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-6f;
|
||||
// quick fix for strange cases (micros overflow + timestamp not defined)
|
||||
if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
|
||||
|
||||
// calculate the necessary angle to achieve target velocity
|
||||
shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts);
|
||||
// for display purposes
|
||||
shaft_velocity = target_velocity;
|
||||
|
||||
// use voltage limit or current limit
|
||||
float Uq = voltage_limit;
|
||||
if(_isset(phase_resistance)) Uq = current_limit*phase_resistance;
|
||||
|
||||
// set the maximal allowed voltage (voltage_limit) with the necessary angle
|
||||
setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs));
|
||||
|
||||
// save timestamp for next call
|
||||
open_loop_timestamp = now_us;
|
||||
|
||||
return Uq;
|
||||
}
|
||||
|
||||
// Function (iterative) generating open loop movement towards the target angle
|
||||
// - target_angle - rad
|
||||
// it uses voltage_limit and velocity_limit variables
|
||||
float 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-6f;
|
||||
// quick fix for strange cases (micros overflow + timestamp not defined)
|
||||
if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
|
||||
|
||||
// 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;
|
||||
shaft_velocity = velocity_limit;
|
||||
}else{
|
||||
shaft_angle = target_angle;
|
||||
shaft_velocity = 0;
|
||||
}
|
||||
|
||||
// use voltage limit or current limit
|
||||
float Uq = voltage_limit;
|
||||
if(_isset(phase_resistance)) Uq = current_limit*phase_resistance;
|
||||
// set the maximal allowed voltage (voltage_limit) with the necessary angle
|
||||
setPhaseVoltage(Uq, 0, _electricalAngle((shaft_angle), pole_pairs));
|
||||
|
||||
// save timestamp for next call
|
||||
open_loop_timestamp = now_us;
|
||||
|
||||
return Uq;
|
||||
}
|
||||
@@ -0,0 +1,118 @@
|
||||
/**
|
||||
* @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
|
||||
@param R motor phase resistance
|
||||
*/
|
||||
StepperMotor(int pp, float R = NOT_SET);
|
||||
|
||||
/**
|
||||
* 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 StepperMotor.
|
||||
*
|
||||
* @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 absoluteZeroSearch();
|
||||
|
||||
// Open loop motion control
|
||||
/**
|
||||
* Function (iterative) generating open loop movement for target velocity
|
||||
* it uses voltage_limit variable
|
||||
*
|
||||
* @param target_velocity - rad/s
|
||||
*/
|
||||
float 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
|
||||
*/
|
||||
float angleOpenloop(float target_angle);
|
||||
// open loop variables
|
||||
long open_loop_timestamp;
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,46 @@
|
||||
#ifndef BLDCDRIVER_H
|
||||
#define BLDCDRIVER_H
|
||||
|
||||
#include "Arduino.h"
|
||||
|
||||
class BLDCDriver{
|
||||
public:
|
||||
|
||||
/** Initialise hardware */
|
||||
virtual int init() = 0;
|
||||
/** Enable hardware */
|
||||
virtual void enable() = 0;
|
||||
/** Disable hardware */
|
||||
virtual void disable() = 0;
|
||||
|
||||
long pwm_frequency; //!< pwm frequency value in hertz
|
||||
float voltage_power_supply; //!< power supply voltage
|
||||
float voltage_limit; //!< limiting voltage set to the motor
|
||||
|
||||
|
||||
float dc_a; //!< currently set duty cycle on phaseA
|
||||
float dc_b; //!< currently set duty cycle on phaseB
|
||||
float dc_c; //!< currently set duty cycle on phaseC
|
||||
|
||||
bool initialized = false; // true if driver was successfully initialized
|
||||
|
||||
/**
|
||||
* 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) = 0;
|
||||
|
||||
/**
|
||||
* Set phase state, enable/disable
|
||||
*
|
||||
* @param sc - phase A state : active / disabled ( high impedance )
|
||||
* @param sb - phase B state : active / disabled ( high impedance )
|
||||
* @param sa - phase C state : active / disabled ( high impedance )
|
||||
*/
|
||||
virtual void setPhaseState(int sa, int sb, int sc) = 0;
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,67 @@
|
||||
#include "CurrentSense.h"
|
||||
|
||||
|
||||
// get current magnitude
|
||||
// - absolute - if no electrical_angle provided
|
||||
// - signed - if angle provided
|
||||
float CurrentSense::getDCCurrent(float motor_electrical_angle){
|
||||
// read current phase currents
|
||||
PhaseCurrent_s current = getPhaseCurrents();
|
||||
// currnet sign - if motor angle not provided the magnitude is always positive
|
||||
float sign = 1;
|
||||
|
||||
// calculate clarke transform
|
||||
float i_alpha, i_beta;
|
||||
if(!current.c){
|
||||
// if only two measured currents
|
||||
i_alpha = current.a;
|
||||
i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
|
||||
}else{
|
||||
// signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed.
|
||||
float mid = (1.f/3) * (current.a + current.b + current.c);
|
||||
float a = current.a - mid;
|
||||
float b = current.b - mid;
|
||||
i_alpha = a;
|
||||
i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
|
||||
}
|
||||
|
||||
// if motor angle provided function returns signed value of the current
|
||||
// determine the sign of the current
|
||||
// sign(atan2(current.q, current.d)) is the same as c.q > 0 ? 1 : -1
|
||||
if(motor_electrical_angle)
|
||||
sign = (i_beta * _cos(motor_electrical_angle) - i_alpha*_sin(motor_electrical_angle)) > 0 ? 1 : -1;
|
||||
// return current magnitude
|
||||
return sign*_sqrt(i_alpha*i_alpha + i_beta*i_beta);
|
||||
}
|
||||
|
||||
// function used with the foc algorihtm
|
||||
// calculating DQ currents from phase currents
|
||||
// - function calculating park and clarke transform of the phase currents
|
||||
// - using getPhaseCurrents internally
|
||||
DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){
|
||||
// read current phase currents
|
||||
PhaseCurrent_s current = getPhaseCurrents();
|
||||
|
||||
// calculate clarke transform
|
||||
float i_alpha, i_beta;
|
||||
if(!current.c){
|
||||
// if only two measured currents
|
||||
i_alpha = current.a;
|
||||
i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
|
||||
} else {
|
||||
// signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed.
|
||||
float mid = (1.f/3) * (current.a + current.b + current.c);
|
||||
float a = current.a - mid;
|
||||
float b = current.b - mid;
|
||||
i_alpha = a;
|
||||
i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
|
||||
}
|
||||
|
||||
// calculate park transform
|
||||
float ct = _cos(angle_el);
|
||||
float st = _sin(angle_el);
|
||||
DQCurrent_s return_current;
|
||||
return_current.d = i_alpha * ct + i_beta * st;
|
||||
return_current.q = i_beta * ct - i_alpha * st;
|
||||
return return_current;
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
#ifndef CURRENTSENSE_H
|
||||
#define CURRENTSENSE_H
|
||||
|
||||
#include "BLDCDriver.h"
|
||||
#include "../foc_utils.h"
|
||||
|
||||
/**
|
||||
* Current sensing abstract class defintion
|
||||
* Each current sensoring implementation needs to extend this interface
|
||||
*/
|
||||
class CurrentSense{
|
||||
public:
|
||||
|
||||
/**
|
||||
* Function intialising the CurrentSense class
|
||||
* All the necessary intialisations of adc and sync should be implemented here
|
||||
*/
|
||||
virtual void init() = 0;
|
||||
|
||||
/**
|
||||
* Function intended to implement all that is needed to sync and current sensing with the driver.
|
||||
* If no such thing is needed it can be left empty (return 1)
|
||||
* @returns - 0 - for failure & 1 - for success
|
||||
*/
|
||||
virtual int driverSync(BLDCDriver *driver) = 0;
|
||||
|
||||
// calibration variables
|
||||
bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC()
|
||||
/**
|
||||
* Function intended to verify if:
|
||||
* - phase current are oriented properly
|
||||
* - if their order is the same as driver phases
|
||||
*
|
||||
* This function corrects the alignment errors if possible ans if no such thing is needed it can be left empty (return 1)
|
||||
* @returns - 0 - for failure & positive number (with status) - for success
|
||||
*/
|
||||
virtual int driverAlign(BLDCDriver *driver, float voltage) = 0;
|
||||
|
||||
/**
|
||||
* Function rading the phase currents a, b and c
|
||||
* This function will be used with the foc control throught the function
|
||||
* CurrentSense::getFOCCurrents(electrical_angle)
|
||||
* - it returns current c equal to 0 if only two phase measurements available
|
||||
*
|
||||
* @return PhaseCurrent_s current values
|
||||
*/
|
||||
virtual PhaseCurrent_s getPhaseCurrents() = 0;
|
||||
/**
|
||||
* Function reading the magnitude of the current set to the motor
|
||||
* It returns the abosolute or signed magnitude if possible
|
||||
* It can receive the motor electrical angle to help with calculation
|
||||
* This function is used with the current control (not foc)
|
||||
*
|
||||
* @param angle_el - electrical angle of the motor (optional)
|
||||
*/
|
||||
virtual float getDCCurrent(float angle_el = 0);
|
||||
|
||||
/**
|
||||
* Function used for FOC contorl, it reads the DQ currents of the motor
|
||||
* It uses the function getPhaseCurrents internally
|
||||
*
|
||||
* @param angle_el - motor electrical angle
|
||||
*/
|
||||
DQCurrent_s getFOCCurrents(float angle_el);
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,141 @@
|
||||
#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;
|
||||
// not set on the begining
|
||||
current_limit = DEF_CURRENT_LIM;
|
||||
|
||||
// index search velocity
|
||||
velocity_index_search = DEF_INDEX_SEARCH_TARGET_VELOCITY;
|
||||
// sensor and motor align voltage
|
||||
voltage_sensor_align = DEF_VOLTAGE_SENSOR_ALIGN;
|
||||
|
||||
// default modulation is SinePWM
|
||||
foc_modulation = FOCModulationType::SinePWM;
|
||||
|
||||
// default target value
|
||||
target = 0;
|
||||
voltage.d = 0;
|
||||
voltage.q = 0;
|
||||
// current target values
|
||||
current_sp = 0;
|
||||
current.q = 0;
|
||||
current.d = 0;
|
||||
|
||||
//monitor_port
|
||||
monitor_port = nullptr;
|
||||
//sensor
|
||||
sensor = nullptr;
|
||||
//current sensor
|
||||
current_sense = nullptr;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
Sensor linking method
|
||||
*/
|
||||
void FOCMotor::linkSensor(Sensor* _sensor) {
|
||||
sensor = _sensor;
|
||||
}
|
||||
|
||||
/**
|
||||
CurrentSense linking method
|
||||
*/
|
||||
void FOCMotor::linkCurrentSense(CurrentSense* _current_sense) {
|
||||
current_sense = _current_sense;
|
||||
}
|
||||
|
||||
// shaft angle calculation
|
||||
float FOCMotor::shaftAngle() {
|
||||
// if no sensor linked return previous value ( for open loop )
|
||||
if(!sensor) return shaft_angle;
|
||||
return sensor_direction*LPF_angle(sensor->getAngle()) - sensor_offset;
|
||||
}
|
||||
// shaft velocity calculation
|
||||
float FOCMotor::shaftVelocity() {
|
||||
// if no sensor linked return previous value ( for open loop )
|
||||
if(!sensor) return shaft_velocity;
|
||||
return sensor_direction*LPF_velocity(sensor->getVelocity());
|
||||
}
|
||||
|
||||
float FOCMotor::electricalAngle(){
|
||||
// if no sensor linked return previous value ( for open loop )
|
||||
if(!sensor) return electrical_angle;
|
||||
return _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() - zero_electric_angle );
|
||||
}
|
||||
|
||||
/**
|
||||
* 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(F("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_downsample || monitor_cnt++ < monitor_downsample ) return;
|
||||
monitor_cnt = 0;
|
||||
if(!monitor_port) return;
|
||||
bool printed = 0;
|
||||
|
||||
if(monitor_variables & _MON_TARGET){
|
||||
monitor_port->print(target,4);
|
||||
monitor_port->print("\t");
|
||||
printed= true;
|
||||
}
|
||||
if(monitor_variables & _MON_VOLT_Q) {
|
||||
monitor_port->print(voltage.q,4);
|
||||
monitor_port->print("\t");
|
||||
printed= true;
|
||||
}
|
||||
if(monitor_variables & _MON_VOLT_D) {
|
||||
monitor_port->print(voltage.d,4);
|
||||
monitor_port->print("\t");
|
||||
printed= true;
|
||||
}
|
||||
// read currents if possible - even in voltage mode (if current_sense available)
|
||||
if(monitor_variables & _MON_CURR_Q || monitor_variables & _MON_CURR_D) {
|
||||
DQCurrent_s c{0,0};
|
||||
if(current_sense){
|
||||
if(torque_controller == TorqueControlType::foc_current) c = current;
|
||||
else{
|
||||
c = current_sense->getFOCCurrents(electrical_angle);
|
||||
c.q = LPF_current_q(c.q);
|
||||
c.d = LPF_current_d(c.d);
|
||||
}
|
||||
}
|
||||
if(monitor_variables & _MON_CURR_Q) {
|
||||
monitor_port->print(c.q*1000, 2); // mAmps
|
||||
monitor_port->print("\t");
|
||||
printed= true;
|
||||
}
|
||||
if(monitor_variables & _MON_CURR_D) {
|
||||
monitor_port->print(c.d*1000, 2); // mAmps
|
||||
monitor_port->print("\t");
|
||||
printed= true;
|
||||
}
|
||||
}
|
||||
|
||||
if(monitor_variables & _MON_VEL) {
|
||||
monitor_port->print(shaft_velocity,4);
|
||||
monitor_port->print("\t");
|
||||
printed= true;
|
||||
}
|
||||
if(monitor_variables & _MON_ANGLE) {
|
||||
monitor_port->print(shaft_angle,4);
|
||||
printed= true;
|
||||
}
|
||||
if(printed) monitor_port->println();
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,236 @@
|
||||
#ifndef FOCMOTOR_H
|
||||
#define FOCMOTOR_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "Sensor.h"
|
||||
#include "CurrentSense.h"
|
||||
|
||||
#include "../time_utils.h"
|
||||
#include "../foc_utils.h"
|
||||
#include "../defaults.h"
|
||||
#include "../pid.h"
|
||||
#include "../lowpass_filter.h"
|
||||
|
||||
|
||||
// monitoring bitmap
|
||||
#define _MON_TARGET 0b1000000 // monitor target value
|
||||
#define _MON_VOLT_Q 0b0100000 // monitor voltage q value
|
||||
#define _MON_VOLT_D 0b0010000 // monitor voltage d value
|
||||
#define _MON_CURR_Q 0b0001000 // monitor current q value - if measured
|
||||
#define _MON_CURR_D 0b0000100 // monitor current d value - if measured
|
||||
#define _MON_VEL 0b0000010 // monitor velocity value
|
||||
#define _MON_ANGLE 0b0000001 // monitor angle value
|
||||
|
||||
/**
|
||||
* Motiron control type
|
||||
*/
|
||||
enum MotionControlType : uint8_t {
|
||||
torque = 0x00, //!< Torque control
|
||||
velocity = 0x01, //!< Velocity motion control
|
||||
angle = 0x02, //!< Position/angle motion control
|
||||
velocity_openloop = 0x03,
|
||||
angle_openloop = 0x04
|
||||
};
|
||||
|
||||
/**
|
||||
* Motiron control type
|
||||
*/
|
||||
enum TorqueControlType : uint8_t {
|
||||
voltage = 0x00, //!< Torque control using voltage
|
||||
dc_current = 0x01, //!< Torque control using DC current (one current magnitude)
|
||||
foc_current = 0x02, //!< torque control using dq currents
|
||||
};
|
||||
|
||||
/**
|
||||
* FOC modulation type
|
||||
*/
|
||||
enum FOCModulationType : uint8_t {
|
||||
SinePWM = 0x00, //!< Sinusoidal PWM modulation
|
||||
SpaceVectorPWM = 0x01, //!< Space vector modulation method
|
||||
Trapezoid_120 = 0x02,
|
||||
Trapezoid_150 = 0x03,
|
||||
};
|
||||
|
||||
|
||||
|
||||
enum FOCMotorStatus : uint8_t {
|
||||
motor_uninitialized = 0x00, //!< Motor is not yet initialized
|
||||
motor_initializing = 0x01, //!< Motor intiialization is in progress
|
||||
motor_uncalibrated = 0x02, //!< Motor is initialized, but not calibrated (open loop possible)
|
||||
motor_calibrating = 0x03, //!< Motor calibration in progress
|
||||
motor_ready = 0x04, //!< Motor is initialized and calibrated (closed loop possible)
|
||||
motor_error = 0x08, //!< Motor is in error state (recoverable, e.g. overcurrent protection active)
|
||||
motor_calib_failed = 0x0E, //!< Motor calibration failed (possibly recoverable)
|
||||
motor_init_failed = 0x0F, //!< Motor initialization failed (not recoverable)
|
||||
};
|
||||
|
||||
|
||||
|
||||
/**
|
||||
Generic motor class
|
||||
*/
|
||||
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 linking a motor and current sensing
|
||||
*
|
||||
* @param current_sense CurrentSense class wrapper for the FOC algorihtm to read the motor current measurements
|
||||
*/
|
||||
void linkCurrentSense(CurrentSense* current_sense);
|
||||
|
||||
|
||||
/**
|
||||
* 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();
|
||||
|
||||
|
||||
/**
|
||||
* Electrical angle calculation
|
||||
*/
|
||||
float electricalAngle();
|
||||
|
||||
// state variables
|
||||
float target; //!< current target value - depends of the controller
|
||||
float shaft_angle;//!< current motor angle
|
||||
float electrical_angle;//!< current electrical angle
|
||||
float shaft_velocity;//!< current motor velocity
|
||||
float current_sp;//!< target current ( q current )
|
||||
float shaft_velocity_sp;//!< current target velocity
|
||||
float shaft_angle_sp;//!< current target angle
|
||||
DQVoltage_s voltage;//!< current d and q voltage set to the motor
|
||||
DQCurrent_s current;//!< current d and q current measured
|
||||
|
||||
// motor configuration parameters
|
||||
float voltage_sensor_align;//!< sensor and motor align voltage parameter
|
||||
float velocity_index_search;//!< target velocity for index search
|
||||
|
||||
// motor physical parameters
|
||||
float phase_resistance; //!< motor phase resistance
|
||||
int pole_pairs;//!< motor pole pairs number
|
||||
|
||||
// limiting variables
|
||||
float voltage_limit; //!< Voltage limitting variable - global limit
|
||||
float current_limit; //!< Current limitting variable - global limit
|
||||
float velocity_limit; //!< Velocity limitting variable - global limit
|
||||
|
||||
// motor status vairables
|
||||
int8_t enabled = 0;//!< enabled or disabled motor flag
|
||||
FOCMotorStatus motor_status = FOCMotorStatus::motor_uninitialized; //!< motor status
|
||||
|
||||
// pwm modulation related variables
|
||||
FOCModulationType foc_modulation;//!< parameter derterniming modulation algorithm
|
||||
int8_t modulation_centered = 1;//!< flag (1) centered modulation around driver limit /2 or (0) pulled to 0
|
||||
|
||||
|
||||
// configuration structures
|
||||
TorqueControlType torque_controller; //!< parameter determining the torque control type
|
||||
MotionControlType controller; //!< parameter determining the control loop to be used
|
||||
|
||||
// controllers and low pass filters
|
||||
PIDController PID_current_q{DEF_PID_CURR_P,DEF_PID_CURR_I,DEF_PID_CURR_D,DEF_PID_CURR_RAMP, DEF_POWER_SUPPLY};//!< parameter determining the q current PID config
|
||||
PIDController PID_current_d{DEF_PID_CURR_P,DEF_PID_CURR_I,DEF_PID_CURR_D,DEF_PID_CURR_RAMP, DEF_POWER_SUPPLY};//!< parameter determining the d current PID config
|
||||
LowPassFilter LPF_current_q{DEF_CURR_FILTER_Tf};//!< parameter determining the current Low pass filter configuration
|
||||
LowPassFilter LPF_current_d{DEF_CURR_FILTER_Tf};//!< parameter determining the current Low pass filter configuration
|
||||
PIDController PID_velocity{DEF_PID_VEL_P,DEF_PID_VEL_I,DEF_PID_VEL_D,DEF_PID_VEL_RAMP,DEF_PID_VEL_LIMIT};//!< parameter determining the velocity PID configuration
|
||||
PIDController P_angle{DEF_P_ANGLE_P,0,0,0,DEF_VEL_LIM}; //!< parameter determining the position PID configuration
|
||||
LowPassFilter LPF_velocity{DEF_VEL_FILTER_Tf};//!< parameter determining the velocity Low pass filter configuration
|
||||
LowPassFilter LPF_angle{0.0};//!< parameter determining the angle low pass filter configuration
|
||||
unsigned int motion_downsample = DEF_MOTION_DOWNSMAPLE; //!< parameter defining the ratio of downsampling for move commad
|
||||
unsigned int motion_cnt = 0; //!< counting variable for downsampling for move commad
|
||||
|
||||
// sensor related variabels
|
||||
float sensor_offset; //!< user defined sensor zero offset
|
||||
float zero_electric_angle = NOT_SET;//!< absolute zero electric angle - if available
|
||||
int sensor_direction = NOT_SET; //!< if sensor_direction == Direction::CCW then direction will be flipped to CW
|
||||
|
||||
/**
|
||||
* Function providing BLDCMotor class with the
|
||||
* Serial interface and enabling monitoring mode
|
||||
*
|
||||
* @param serial Monitoring Serial class reference
|
||||
*/
|
||||
void useMonitoring(Print &serial);
|
||||
|
||||
/**
|
||||
* Utility function intended to be used with serial plotter to monitor motor variables
|
||||
* significantly slowing the execution down!!!!
|
||||
*/
|
||||
void monitor();
|
||||
unsigned int monitor_downsample = DEF_MON_DOWNSMAPLE; //!< show monitor outputs each monitor_downsample calls
|
||||
// initial monitoring will display target, voltage, velocity and angle
|
||||
uint8_t monitor_variables = _MON_TARGET | _MON_VOLT_Q | _MON_VEL | _MON_ANGLE; //!< Bit array holding the map of variables the user wants to monitor
|
||||
|
||||
/**
|
||||
* Sensor link:
|
||||
* - Encoder
|
||||
* - MagneticSensor*
|
||||
* - HallSensor
|
||||
*/
|
||||
Sensor* sensor;
|
||||
/**
|
||||
* CurrentSense link
|
||||
*/
|
||||
CurrentSense* current_sense;
|
||||
|
||||
// monitoring functions
|
||||
Print* monitor_port; //!< Serial terminal variable if provided
|
||||
private:
|
||||
// monitor counting variable
|
||||
unsigned int monitor_cnt = 0 ; //!< counting variable
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,72 @@
|
||||
#include "Sensor.h"
|
||||
#include "../foc_utils.h"
|
||||
#include "../time_utils.h"
|
||||
|
||||
// TODO add an init method to make the startup smoother by initializing internal variables to current values rather than 0
|
||||
|
||||
void Sensor::update() {
|
||||
float val = getSensorAngle();
|
||||
angle_prev_ts = _micros();
|
||||
float d_angle = val - angle_prev;
|
||||
// if overflow happened track it as full rotation
|
||||
if(abs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1;
|
||||
angle_prev = val;
|
||||
}
|
||||
|
||||
|
||||
/** get current angular velocity (rad/s) */
|
||||
float Sensor::getVelocity() {
|
||||
// calculate sample time
|
||||
float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6;
|
||||
// quick fix for strange cases (micros overflow)
|
||||
if(Ts <= 0) Ts = 1e-3f;
|
||||
// velocity calculation
|
||||
float vel = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts;
|
||||
// save variables for future pass
|
||||
vel_angle_prev = angle_prev;
|
||||
vel_full_rotations = full_rotations;
|
||||
vel_angle_prev_ts = angle_prev_ts;
|
||||
return vel;
|
||||
}
|
||||
|
||||
void Sensor::init() {
|
||||
// initialize all the internal variables of Sensor to ensure a "smooth" startup (without a 'jump' from zero)
|
||||
getSensorAngle(); // call once
|
||||
delayMicroseconds(1);
|
||||
vel_angle_prev = getSensorAngle(); // call again
|
||||
vel_angle_prev_ts = _micros();
|
||||
delay(1);
|
||||
getSensorAngle(); // call once
|
||||
delayMicroseconds(1);
|
||||
angle_prev = getSensorAngle(); // call again
|
||||
angle_prev_ts = _micros();
|
||||
}
|
||||
|
||||
|
||||
float Sensor::getMechanicalAngle() {
|
||||
return angle_prev;
|
||||
}
|
||||
|
||||
|
||||
|
||||
float Sensor::getAngle(){
|
||||
return (float)full_rotations * _2PI + angle_prev;
|
||||
}
|
||||
|
||||
|
||||
|
||||
double Sensor::getPreciseAngle() {
|
||||
return (double)full_rotations * (double)_2PI + (double)angle_prev;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int32_t Sensor::getFullRotations() {
|
||||
return full_rotations;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int Sensor::needsSearch() {
|
||||
return 0; // default false
|
||||
}
|
||||
@@ -0,0 +1,131 @@
|
||||
#ifndef SENSOR_H
|
||||
#define SENSOR_H
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
/**
|
||||
* Direction structure
|
||||
*/
|
||||
enum Direction : int8_t {
|
||||
CW = 1, // clockwise
|
||||
CCW = -1, // counter clockwise
|
||||
UNKNOWN = 0 // not yet known or invalid state
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* Pullup configuration structure
|
||||
*/
|
||||
enum Pullup : uint8_t {
|
||||
USE_INTERN = 0x00, //!< Use internal pullups
|
||||
USE_EXTERN = 0x01 //!< Use external pullups
|
||||
};
|
||||
|
||||
/**
|
||||
* Sensor abstract class defintion
|
||||
*
|
||||
* This class is purposefully kept simple, as a base for all kinds of sensors. Currently we have
|
||||
* Encoders, Magnetic Encoders and Hall Sensor implementations. This base class extracts the
|
||||
* most basic common features so that a FOC driver can obtain the data it needs for operation.
|
||||
*
|
||||
* To implement your own sensors, create a sub-class of this class, and implement the getSensorAngle()
|
||||
* method. getSensorAngle() returns a float value, in radians, representing the current shaft angle in the
|
||||
* range 0 to 2*PI (one full turn).
|
||||
*
|
||||
* To function correctly, the sensor class update() method has to be called sufficiently quickly. Normally,
|
||||
* the BLDCMotor's loopFOC() function calls it once per iteration, so you must ensure to call loopFOC() quickly
|
||||
* enough, both for correct motor and sensor operation.
|
||||
*
|
||||
* The Sensor base class provides an implementation of getVelocity(), and takes care of counting full
|
||||
* revolutions in a precise way, but if you wish you can additionally override these methods to provide more
|
||||
* optimal implementations for your hardware.
|
||||
*
|
||||
*/
|
||||
class Sensor{
|
||||
public:
|
||||
/**
|
||||
* Get mechanical shaft angle in the range 0 to 2PI. This value will be as precise as possible with
|
||||
* the hardware. Base implementation uses the values returned by update() so that
|
||||
* the same values are returned until update() is called again.
|
||||
*/
|
||||
virtual float getMechanicalAngle();
|
||||
|
||||
/**
|
||||
* Get current position (in rad) including full rotations and shaft angle.
|
||||
* Base implementation uses the values returned by update() so that the same
|
||||
* values are returned until update() is called again.
|
||||
* Note that this value has limited precision as the number of rotations increases,
|
||||
* because the limited precision of float can't capture the large angle of the full
|
||||
* rotations and the small angle of the shaft angle at the same time.
|
||||
*/
|
||||
virtual float getAngle();
|
||||
|
||||
/**
|
||||
* On architectures supporting it, this will return a double precision position value,
|
||||
* which should have improved precision for large position values.
|
||||
* Base implementation uses the values returned by update() so that the same
|
||||
* values are returned until update() is called again.
|
||||
*/
|
||||
virtual double getPreciseAngle();
|
||||
|
||||
/**
|
||||
* Get current angular velocity (rad/s)
|
||||
* Can be overridden in subclasses. Base implementation uses the values
|
||||
* returned by update() so that it only makes sense to call this if update()
|
||||
* has been called in the meantime.
|
||||
*/
|
||||
virtual float getVelocity();
|
||||
|
||||
/**
|
||||
* Get the number of full rotations
|
||||
* Base implementation uses the values returned by update() so that the same
|
||||
* values are returned until update() is called again.
|
||||
*/
|
||||
virtual int32_t getFullRotations();
|
||||
|
||||
/**
|
||||
* Updates the sensor values by reading the hardware sensor.
|
||||
* Some implementations may work with interrupts, and not need this.
|
||||
* The base implementation calls getSensorAngle(), and updates internal
|
||||
* fields for angle, timestamp and full rotations.
|
||||
* This method must be called frequently enough to guarantee that full
|
||||
* rotations are not "missed" due to infrequent polling.
|
||||
* Override in subclasses if alternative behaviours are required for your
|
||||
* sensor hardware.
|
||||
*/
|
||||
virtual void update();
|
||||
|
||||
/**
|
||||
* returns 0 if it does need search for absolute zero
|
||||
* 0 - magnetic sensor (& encoder with index which is found)
|
||||
* 1 - ecoder with index (with index not found yet)
|
||||
*/
|
||||
virtual int needsSearch();
|
||||
protected:
|
||||
/**
|
||||
* Get current shaft angle from the sensor hardware, and
|
||||
* return it as a float in radians, in the range 0 to 2PI.
|
||||
*
|
||||
* This method is pure virtual and must be implemented in subclasses.
|
||||
* Calling this method directly does not update the base-class internal fields.
|
||||
* Use update() when calling from outside code.
|
||||
*/
|
||||
virtual float getSensorAngle()=0;
|
||||
/**
|
||||
* Call Sensor::init() from your sensor subclass's init method if you want smoother startup
|
||||
* The base class init() method calls getSensorAngle() several times to initialize the internal fields
|
||||
* to current values, ensuring there is no discontinuity ("jump from zero") during the first calls
|
||||
* to sensor.getAngle() and sensor.getVelocity()
|
||||
*/
|
||||
virtual void init();
|
||||
|
||||
// velocity calculation variables
|
||||
float angle_prev=0; // result of last call to getSensorAngle(), used for full rotations and velocity
|
||||
long angle_prev_ts=0; // timestamp of last call to getAngle, used for velocity
|
||||
float vel_angle_prev=0; // angle at last call to getVelocity, used for velocity
|
||||
long vel_angle_prev_ts=0; // last velocity calculation timestamp
|
||||
int32_t full_rotations=0; // full rotation tracking
|
||||
int32_t vel_full_rotations=0; // previous full rotation value for velocity calculation
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,29 @@
|
||||
#ifndef STEPPERDRIVER_H
|
||||
#define STEPPERDRIVER_H
|
||||
|
||||
class StepperDriver{
|
||||
public:
|
||||
|
||||
/** Initialise hardware */
|
||||
virtual int init() = 0;
|
||||
/** Enable hardware */
|
||||
virtual void enable() = 0;
|
||||
/** Disable hardware */
|
||||
virtual void disable() = 0;
|
||||
|
||||
long pwm_frequency; //!< pwm frequency value in hertz
|
||||
float voltage_power_supply; //!< power supply voltage
|
||||
float voltage_limit; //!< limiting voltage set to the motor
|
||||
|
||||
bool initialized = false; // true if driver was successfully initialized
|
||||
|
||||
/**
|
||||
* Set phase voltages to the harware
|
||||
*
|
||||
* @param Ua phase A voltage
|
||||
* @param Ub phase B voltage
|
||||
*/
|
||||
virtual void setPwm(float Ua, float Ub) = 0;
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,49 @@
|
||||
// default configuration values
|
||||
// change this file to optimal values for your application
|
||||
|
||||
#define DEF_POWER_SUPPLY 12.0f //!< default power supply voltage
|
||||
// velocity PI controller params
|
||||
#define DEF_PID_VEL_P 0.5f //!< default PID controller P value
|
||||
#define DEF_PID_VEL_I 10.0f //!< default PID controller I value
|
||||
#define DEF_PID_VEL_D 0.0f //!< default PID controller D value
|
||||
#define DEF_PID_VEL_RAMP 1000.0f //!< default PID controller voltage ramp value
|
||||
#define DEF_PID_VEL_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit
|
||||
|
||||
// current sensing PID values
|
||||
#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__)
|
||||
// for 16Mhz controllers like Arduino uno and mega
|
||||
#define DEF_PID_CURR_P 2 //!< default PID controller P value
|
||||
#define DEF_PID_CURR_I 100 //!< default PID controller I value
|
||||
#define DEF_PID_CURR_D 0.0f //!< default PID controller D value
|
||||
#define DEF_PID_CURR_RAMP 1000.0f //!< default PID controller voltage ramp value
|
||||
#define DEF_PID_CURR_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit
|
||||
#define DEF_CURR_FILTER_Tf 0.01f //!< default velocity filter time constant
|
||||
#else
|
||||
// for stm32, due, teensy, esp32 and similar
|
||||
#define DEF_PID_CURR_P 3 //!< default PID controller P value
|
||||
#define DEF_PID_CURR_I 300.0f //!< default PID controller I value
|
||||
#define DEF_PID_CURR_D 0.0f //!< default PID controller D value
|
||||
#define DEF_PID_CURR_RAMP 0 //!< default PID controller voltage ramp value
|
||||
#define DEF_PID_CURR_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit
|
||||
#define DEF_CURR_FILTER_Tf 0.005f //!< default currnet filter time constant
|
||||
#endif
|
||||
// default current limit values
|
||||
#define DEF_CURRENT_LIM 2.0f //!< 2Amps current limit by default
|
||||
|
||||
// default monitor downsample
|
||||
#define DEF_MON_DOWNSMAPLE 100 //!< default monitor downsample
|
||||
#define DEF_MOTION_DOWNSMAPLE 0 //!< default motion downsample - disable
|
||||
|
||||
// angle P params
|
||||
#define DEF_P_ANGLE_P 20.0f //!< default P controller P value
|
||||
#define DEF_VEL_LIM 20.0f //!< angle velocity limit default
|
||||
|
||||
// index search
|
||||
#define DEF_INDEX_SEARCH_TARGET_VELOCITY 1.0f //!< default index search velocity
|
||||
// align voltage
|
||||
#define DEF_VOLTAGE_SENSOR_ALIGN 3.0f //!< default voltage for sensor and motor zero alignemt
|
||||
// low pass filter velocity
|
||||
#define DEF_VEL_FILTER_Tf 0.005 //!< default velocity filter time constant
|
||||
|
||||
// current sense default parameters
|
||||
#define DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf 0.0f //!< default currnet sense per phase low pass filter time constant
|
||||
@@ -0,0 +1,73 @@
|
||||
#include "foc_utils.h"
|
||||
|
||||
// int array instead of float array
|
||||
// 4x200 points per 360 deg
|
||||
// 2x storage save (int 2Byte float 4 Byte )
|
||||
// sin*10000
|
||||
const int sine_array[200] = {0,79,158,237,316,395,473,552,631,710,789,867,946,1024,1103,1181,1260,1338,1416,1494,1572,1650,1728,1806,1883,1961,2038,2115,2192,2269,2346,2423,2499,2575,2652,2728,2804,2879,2955,3030,3105,3180,3255,3329,3404,3478,3552,3625,3699,3772,3845,3918,3990,4063,4135,4206,4278,4349,4420,4491,4561,4631,4701,4770,4840,4909,4977,5046,5113,5181,5249,5316,5382,5449,5515,5580,5646,5711,5775,5839,5903,5967,6030,6093,6155,6217,6279,6340,6401,6461,6521,6581,6640,6699,6758,6815,6873,6930,6987,7043,7099,7154,7209,7264,7318,7371,7424,7477,7529,7581,7632,7683,7733,7783,7832,7881,7930,7977,8025,8072,8118,8164,8209,8254,8298,8342,8385,8428,8470,8512,8553,8594,8634,8673,8712,8751,8789,8826,8863,8899,8935,8970,9005,9039,9072,9105,9138,9169,9201,9231,9261,9291,9320,9348,9376,9403,9429,9455,9481,9506,9530,9554,9577,9599,9621,9642,9663,9683,9702,9721,9739,9757,9774,9790,9806,9821,9836,9850,9863,9876,9888,9899,9910,9920,9930,9939,9947,9955,9962,9969,9975,9980,9985,9989,9992,9995,9997,9999,10000,10000};
|
||||
|
||||
// function approximating the sine calculation by using fixed size array
|
||||
// ~40us (float array)
|
||||
// ~50us (int array)
|
||||
// precision +-0.005
|
||||
// it has to receive an angle in between 0 and 2PI
|
||||
float _sin(float a){
|
||||
if(a < _PI_2){
|
||||
//return sine_array[(int)(199.0f*( a / (_PI/2.0)))];
|
||||
//return sine_array[(int)(126.6873f* a)]; // float array optimized
|
||||
return 0.0001f*sine_array[_round(126.6873f* a)]; // int array optimized
|
||||
}else if(a < _PI){
|
||||
// return sine_array[(int)(199.0f*(1.0f - (a-_PI/2.0) / (_PI/2.0)))];
|
||||
//return sine_array[398 - (int)(126.6873f*a)]; // float array optimized
|
||||
return 0.0001f*sine_array[398 - _round(126.6873f*a)]; // int array optimized
|
||||
}else if(a < _3PI_2){
|
||||
// return -sine_array[(int)(199.0f*((a - _PI) / (_PI/2.0)))];
|
||||
//return -sine_array[-398 + (int)(126.6873f*a)]; // float array optimized
|
||||
return -0.0001f*sine_array[-398 + _round(126.6873f*a)]; // int array optimized
|
||||
} else {
|
||||
// return -sine_array[(int)(199.0f*(1.0f - (a - 3*_PI/2) / (_PI/2.0)))];
|
||||
//return -sine_array[796 - (int)(126.6873f*a)]; // float array optimized
|
||||
return -0.0001f*sine_array[796 - _round(126.6873f*a)]; // int array optimized
|
||||
}
|
||||
}
|
||||
|
||||
// function approximating cosine calculation by using fixed size array
|
||||
// ~55us (float array)
|
||||
// ~56us (int array)
|
||||
// precision +-0.005
|
||||
// it has to receive an angle in between 0 and 2PI
|
||||
float _cos(float a){
|
||||
float a_sin = a + _PI_2;
|
||||
a_sin = a_sin > _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);
|
||||
}
|
||||
|
||||
// square root approximation function using
|
||||
// https://reprap.org/forum/read.php?147,219210
|
||||
// https://en.wikipedia.org/wiki/Fast_inverse_square_root
|
||||
float _sqrtApprox(float number) {//low in fat
|
||||
long i;
|
||||
float y;
|
||||
// float x;
|
||||
// const float f = 1.5F; // better precision
|
||||
|
||||
// x = number * 0.5F;
|
||||
y = number;
|
||||
i = * ( long * ) &y;
|
||||
i = 0x5f375a86 - ( i >> 1 );
|
||||
y = * ( float * ) &i;
|
||||
// y = y * ( f - ( x * y * y ) ); // better precision
|
||||
return number * y;
|
||||
}
|
||||
@@ -0,0 +1,92 @@
|
||||
#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.5f):(long)((x)-0.5f))
|
||||
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
|
||||
#define _sqrt(a) (_sqrtApprox(a))
|
||||
#define _isset(a) ( (a) != (NOT_SET) )
|
||||
#define _UNUSED(v) (void) (v)
|
||||
|
||||
// utility defines
|
||||
#define _2_SQRT3 1.15470053838f
|
||||
#define _SQRT3 1.73205080757f
|
||||
#define _1_SQRT3 0.57735026919f
|
||||
#define _SQRT3_2 0.86602540378f
|
||||
#define _SQRT2 1.41421356237f
|
||||
#define _120_D2R 2.09439510239f
|
||||
#define _PI 3.14159265359f
|
||||
#define _PI_2 1.57079632679f
|
||||
#define _PI_3 1.0471975512f
|
||||
#define _2PI 6.28318530718f
|
||||
#define _3PI_2 4.71238898038f
|
||||
#define _PI_6 0.52359877559f
|
||||
|
||||
#define NOT_SET -12345.0
|
||||
#define _HIGH_IMPEDANCE 0
|
||||
#define _HIGH_Z _HIGH_IMPEDANCE
|
||||
#define _ACTIVE 1
|
||||
|
||||
// dq current structure
|
||||
struct DQCurrent_s
|
||||
{
|
||||
float d;
|
||||
float q;
|
||||
};
|
||||
// phase current structure
|
||||
struct PhaseCurrent_s
|
||||
{
|
||||
float a;
|
||||
float b;
|
||||
float c;
|
||||
};
|
||||
// dq voltage structs
|
||||
struct DQVoltage_s
|
||||
{
|
||||
float d;
|
||||
float q;
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* 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);
|
||||
|
||||
/**
|
||||
* Function approximating square root function
|
||||
* - using fast inverse square root
|
||||
*
|
||||
* @param value - number
|
||||
*/
|
||||
float _sqrtApprox(float value);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,28 @@
|
||||
#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 = 1e-3f;
|
||||
else if(dt > 0.3f) {
|
||||
y_prev = x;
|
||||
timestamp_prev = timestamp;
|
||||
return x;
|
||||
}
|
||||
|
||||
float alpha = Tf/(Tf + dt);
|
||||
float y = alpha*y_prev + (1.0f - alpha)*x;
|
||||
y_prev = y;
|
||||
timestamp_prev = timestamp;
|
||||
return y;
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
#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
|
||||
@@ -0,0 +1,58 @@
|
||||
#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]
|
||||
, error_prev(0.0f)
|
||||
, output_prev(0.0f)
|
||||
, integral_prev(0.0f)
|
||||
{
|
||||
timestamp_prev = _micros();
|
||||
}
|
||||
|
||||
// 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-6f;
|
||||
// quick fix for strange cases (micros overflow)
|
||||
if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
|
||||
|
||||
// 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.5f*(error + error_prev);
|
||||
// antiwindup - limit the output
|
||||
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);
|
||||
|
||||
// if output ramp defined
|
||||
if(output_ramp > 0){
|
||||
// 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;
|
||||
}
|
||||
@@ -0,0 +1,44 @@
|
||||
#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
|
||||
|
||||
float error_prev; //!< last tracking error value
|
||||
|
||||
protected:
|
||||
//float error_prev; //!< last tracking error value
|
||||
float output_prev; //!< last pid output value
|
||||
float integral_prev; //!< last integral component value
|
||||
unsigned long timestamp_prev; //!< Last execution timestamp
|
||||
};
|
||||
|
||||
#endif // PID_H
|
||||
|
||||
//Lyf对此进行了修改，将error_prev从protected移至public，便于清楚积分影响
|
||||
@@ -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_ATmega328PB__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega32U4__)
|
||||
// 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_ATmega328PB__) || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega32U4__)
|
||||
// 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
|
||||
}
|
||||
@@ -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
|
||||
@@ -0,0 +1,614 @@
|
||||
#include "Commander.h"
|
||||
|
||||
Commander::Commander(Stream& serial, char eol, bool echo){
|
||||
com_port = &serial;
|
||||
this->eol = eol;
|
||||
this->echo = echo;
|
||||
}
|
||||
Commander::Commander(char eol, bool echo){
|
||||
this->eol = eol;
|
||||
this->echo = echo;
|
||||
}
|
||||
|
||||
|
||||
void Commander::add(char id, CommandCallback onCommand, char* label ){
|
||||
call_list[call_count] = onCommand;
|
||||
call_ids[call_count] = id;
|
||||
call_label[call_count] = label;
|
||||
call_count++;
|
||||
}
|
||||
|
||||
|
||||
void Commander::run(){
|
||||
if(!com_port) return;
|
||||
run(*com_port, eol);
|
||||
}
|
||||
|
||||
void Commander::run(Stream& serial, char eol){
|
||||
Stream* tmp = com_port; // save the serial instance
|
||||
char eol_tmp = this->eol;
|
||||
this->eol = eol;
|
||||
com_port = &serial;
|
||||
|
||||
// a string to hold incoming data
|
||||
while (serial.available()) {
|
||||
// get the new byte:
|
||||
int ch = serial.read();
|
||||
received_chars[rec_cnt++] = (char)ch;
|
||||
// end of user input
|
||||
if(echo)
|
||||
print((char)ch);
|
||||
if (isSentinel(ch)) {
|
||||
// execute the user command
|
||||
run(received_chars);
|
||||
|
||||
// reset the command buffer
|
||||
received_chars[0] = 0;
|
||||
rec_cnt=0;
|
||||
}
|
||||
if (rec_cnt>=MAX_COMMAND_LENGTH) { // prevent buffer overrun if message is too long
|
||||
received_chars[0] = 0;
|
||||
rec_cnt=0;
|
||||
}
|
||||
}
|
||||
|
||||
com_port = tmp; // reset the instance to the internal value
|
||||
this->eol = eol_tmp;
|
||||
}
|
||||
|
||||
void Commander::run(char* user_input){
|
||||
// execute the user command
|
||||
char id = user_input[0];
|
||||
switch(id){
|
||||
case CMD_SCAN:
|
||||
for(int i=0; i < call_count; i++){
|
||||
print(call_ids[i]);
|
||||
print(":");
|
||||
if(call_label[i]) println(call_label[i]);
|
||||
else println("");
|
||||
}
|
||||
break;
|
||||
case CMD_VERBOSE:
|
||||
if(!isSentinel(user_input[1])) verbose = (VerboseMode)atoi(&user_input[1]);
|
||||
printVerbose(F("Verb:"));
|
||||
switch (verbose){
|
||||
case VerboseMode::nothing:
|
||||
println(F("off!"));
|
||||
break;
|
||||
case VerboseMode::on_request:
|
||||
case VerboseMode::user_friendly:
|
||||
println(F("on!"));
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case CMD_DECIMAL:
|
||||
if(!isSentinel(user_input[1])) decimal_places = atoi(&user_input[1]);
|
||||
printVerbose(F("Decimal:"));
|
||||
println(decimal_places);
|
||||
break;
|
||||
default:
|
||||
for(int i=0; i < call_count; i++){
|
||||
if(id == call_ids[i]){
|
||||
call_list[i](&user_input[1]);
|
||||
break;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Commander::motor(FOCMotor* motor, char* user_command) {
|
||||
|
||||
// if target setting
|
||||
if(isDigit(user_command[0]) || user_command[0] == '-' || user_command[0] == '+'){
|
||||
target(motor, user_command);
|
||||
return;
|
||||
}
|
||||
|
||||
// parse command letter
|
||||
char cmd = user_command[0];
|
||||
char sub_cmd = user_command[1];
|
||||
// check if there is a subcommand or not
|
||||
int value_index = (sub_cmd >= 'A' && sub_cmd <= 'Z') ? 2 : 1;
|
||||
// check if get command
|
||||
bool GET = isSentinel(user_command[value_index]);
|
||||
// parse command values
|
||||
float value = atof(&user_command[value_index]);
|
||||
|
||||
|
||||
// a bit of optimisation of variable memory for Arduino UNO (atmega328)
|
||||
switch(cmd){
|
||||
case CMD_C_Q_PID: //
|
||||
printVerbose(F("PID curr q| "));
|
||||
if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_q, &user_command[1]);
|
||||
else pid(&motor->PID_current_q,&user_command[1]);
|
||||
break;
|
||||
case CMD_C_D_PID: //
|
||||
printVerbose(F("PID curr d| "));
|
||||
if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_d, &user_command[1]);
|
||||
else pid(&motor->PID_current_d, &user_command[1]);
|
||||
break;
|
||||
case CMD_V_PID: //
|
||||
printVerbose(F("PID vel| "));
|
||||
if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_velocity, &user_command[1]);
|
||||
else pid(&motor->PID_velocity, &user_command[1]);
|
||||
break;
|
||||
case CMD_A_PID: //
|
||||
printVerbose(F("PID angle| "));
|
||||
if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_angle, &user_command[1]);
|
||||
else pid(&motor->P_angle, &user_command[1]);
|
||||
break;
|
||||
case CMD_LIMITS: //
|
||||
printVerbose(F("Limits| "));
|
||||
switch (sub_cmd){
|
||||
case SCMD_LIM_VOLT: // voltage limit change
|
||||
printVerbose(F("volt: "));
|
||||
if(!GET) {
|
||||
motor->voltage_limit = value;
|
||||
motor->PID_current_d.limit = value;
|
||||
motor->PID_current_q.limit = value;
|
||||
// change velocity pid limit if in voltage mode and no phase resistance set
|
||||
if( !_isset(motor->phase_resistance) && motor->torque_controller==TorqueControlType::voltage) motor->PID_velocity.limit = value;
|
||||
}
|
||||
println(motor->voltage_limit);
|
||||
break;
|
||||
case SCMD_LIM_CURR: // current limit
|
||||
printVerbose(F("curr: "));
|
||||
if(!GET){
|
||||
motor->current_limit = value;
|
||||
// if phase resistance specified or the current control is on set the current limit to the velocity PID
|
||||
if(_isset(motor->phase_resistance) || motor->torque_controller != TorqueControlType::voltage ) motor->PID_velocity.limit = value;
|
||||
}
|
||||
println(motor->current_limit);
|
||||
break;
|
||||
case SCMD_LIM_VEL: // velocity limit
|
||||
printVerbose(F("vel: "));
|
||||
if(!GET){
|
||||
motor->velocity_limit = value;
|
||||
motor->P_angle.limit = value;
|
||||
}
|
||||
println(motor->velocity_limit);
|
||||
break;
|
||||
default:
|
||||
printError();
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case CMD_MOTION_TYPE:
|
||||
case CMD_TORQUE_TYPE:
|
||||
case CMD_STATUS:
|
||||
motion(motor, &user_command[0]);
|
||||
break;
|
||||
case CMD_PWMMOD:
|
||||
// PWM modulation change
|
||||
printVerbose(F("PWM Mod | "));
|
||||
switch (sub_cmd){
|
||||
case SCMD_PWMMOD_TYPE: // zero offset
|
||||
printVerbose(F("type: "));
|
||||
if(!GET) motor->foc_modulation = (FOCModulationType)value;
|
||||
switch(motor->foc_modulation){
|
||||
case FOCModulationType::SinePWM:
|
||||
println(F("SinePWM"));
|
||||
break;
|
||||
case FOCModulationType::SpaceVectorPWM:
|
||||
println(F("SVPWM"));
|
||||
break;
|
||||
case FOCModulationType::Trapezoid_120:
|
||||
println(F("Trap 120"));
|
||||
break;
|
||||
case FOCModulationType::Trapezoid_150:
|
||||
println(F("Trap 150"));
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case SCMD_PWMMOD_CENTER: // centered modulation
|
||||
printVerbose(F("center: "));
|
||||
if(!GET) motor->modulation_centered = value;
|
||||
println(motor->modulation_centered);
|
||||
break;
|
||||
default:
|
||||
printError();
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case CMD_RESIST:
|
||||
// enable/disable
|
||||
printVerbose(F("R phase: "));
|
||||
if(!GET){
|
||||
motor->phase_resistance = value;
|
||||
if(motor->torque_controller==TorqueControlType::voltage){
|
||||
motor->voltage_limit = motor->current_limit*value;
|
||||
motor->PID_velocity.limit= motor->current_limit;
|
||||
}
|
||||
}
|
||||
if(_isset(motor->phase_resistance)) println(motor->phase_resistance);
|
||||
else println(0);
|
||||
break;
|
||||
case CMD_SENSOR:
|
||||
// Sensor zero offset
|
||||
printVerbose(F("Sensor | "));
|
||||
switch (sub_cmd){
|
||||
case SCMD_SENS_MECH_OFFSET: // zero offset
|
||||
printVerbose(F("offset: "));
|
||||
if(!GET) motor->sensor_offset = value;
|
||||
println(motor->sensor_offset);
|
||||
break;
|
||||
case SCMD_SENS_ELEC_OFFSET: // electrical zero offset - not suggested to touch
|
||||
printVerbose(F("el. offset: "));
|
||||
if(!GET) motor->zero_electric_angle = value;
|
||||
println(motor->zero_electric_angle);
|
||||
break;
|
||||
default:
|
||||
printError();
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case CMD_MONITOR: // get current values of the state variables
|
||||
printVerbose(F("Monitor | "));
|
||||
switch (sub_cmd){
|
||||
case SCMD_GET: // get command
|
||||
switch((uint8_t)value){
|
||||
case 0: // get target
|
||||
printVerbose(F("target: "));
|
||||
println(motor->target);
|
||||
break;
|
||||
case 1: // get voltage q
|
||||
printVerbose(F("Vq: "));
|
||||
println(motor->voltage.q);
|
||||
break;
|
||||
case 2: // get voltage d
|
||||
printVerbose(F("Vd: "));
|
||||
println(motor->voltage.d);
|
||||
break;
|
||||
case 3: // get current q
|
||||
printVerbose(F("Cq: "));
|
||||
println(motor->current.q);
|
||||
break;
|
||||
case 4: // get current d
|
||||
printVerbose(F("Cd: "));
|
||||
println(motor->current.d);
|
||||
break;
|
||||
case 5: // get velocity
|
||||
printVerbose(F("vel: "));
|
||||
println(motor->shaft_velocity);
|
||||
break;
|
||||
case 6: // get angle
|
||||
printVerbose(F("angle: "));
|
||||
println(motor->shaft_angle);
|
||||
break;
|
||||
case 7: // get all states
|
||||
printVerbose(F("all: "));
|
||||
print(motor->target);
|
||||
print(";");
|
||||
print(motor->voltage.q);
|
||||
print(";");
|
||||
print(motor->voltage.d);
|
||||
print(";");
|
||||
print(motor->current.q);
|
||||
print(";");
|
||||
print(motor->current.d);
|
||||
print(";");
|
||||
print(motor->shaft_velocity);
|
||||
print(";");
|
||||
println(motor->shaft_angle);
|
||||
break;
|
||||
default:
|
||||
printError();
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case SCMD_DOWNSAMPLE:
|
||||
printVerbose(F("downsample: "));
|
||||
if(!GET) motor->monitor_downsample = value;
|
||||
println((int)motor->monitor_downsample);
|
||||
break;
|
||||
case SCMD_CLEAR:
|
||||
motor->monitor_variables = (uint8_t) 0;
|
||||
println(F("clear"));
|
||||
break;
|
||||
case SCMD_SET:
|
||||
if(!GET) motor->monitor_variables = (uint8_t) 0;
|
||||
for(int i = 0; i < 7; i++){
|
||||
if(isSentinel(user_command[value_index+i])) break;
|
||||
if(!GET) motor->monitor_variables |= (user_command[value_index+i] - '0') << (6-i);
|
||||
print( (user_command[value_index+i] - '0') );
|
||||
}
|
||||
println("");
|
||||
break;
|
||||
default:
|
||||
printError();
|
||||
break;
|
||||
}
|
||||
break;
|
||||
default: // unknown cmd
|
||||
printVerbose(F("unknown cmd "));
|
||||
printError();
|
||||
}
|
||||
}
|
||||
|
||||
void Commander::motion(FOCMotor* motor, char* user_cmd, char* separator){
|
||||
char cmd = user_cmd[0];
|
||||
char sub_cmd = user_cmd[1];
|
||||
bool GET = isSentinel(user_cmd[1]);
|
||||
float value = atof(&user_cmd[(sub_cmd >= 'A' && sub_cmd <= 'Z') ? 2 : 1]);
|
||||
|
||||
switch(cmd){
|
||||
case CMD_MOTION_TYPE:
|
||||
printVerbose(F("Motion:"));
|
||||
switch(sub_cmd){
|
||||
case SCMD_DOWNSAMPLE:
|
||||
printVerbose(F(" downsample: "));
|
||||
if(!GET) motor->motion_downsample = value;
|
||||
println((int)motor->motion_downsample);
|
||||
break;
|
||||
default:
|
||||
// change control type
|
||||
if(!GET && value >= 0 && (int)value < 5) // if set command
|
||||
motor->controller = (MotionControlType)value;
|
||||
switch(motor->controller){
|
||||
case MotionControlType::torque:
|
||||
println(F("torque"));
|
||||
break;
|
||||
case MotionControlType::velocity:
|
||||
println(F("vel"));
|
||||
break;
|
||||
case MotionControlType::angle:
|
||||
println(F("angle"));
|
||||
break;
|
||||
case MotionControlType::velocity_openloop:
|
||||
println(F("vel open"));
|
||||
break;
|
||||
case MotionControlType::angle_openloop:
|
||||
println(F("angle open"));
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case CMD_TORQUE_TYPE:
|
||||
// change control type
|
||||
printVerbose(F("Torque: "));
|
||||
if(!GET && (int8_t)value >= 0 && (int8_t)value < 3)// if set command
|
||||
motor->torque_controller = (TorqueControlType)value;
|
||||
switch(motor->torque_controller){
|
||||
case TorqueControlType::voltage:
|
||||
println(F("volt"));
|
||||
// change the velocity control limits if necessary
|
||||
if( !_isset(motor->phase_resistance) ) motor->PID_velocity.limit = motor->voltage_limit;
|
||||
break;
|
||||
case TorqueControlType::dc_current:
|
||||
println(F("dc curr"));
|
||||
// change the velocity control limits if necessary
|
||||
motor->PID_velocity.limit = motor->current_limit;
|
||||
break;
|
||||
case TorqueControlType::foc_current:
|
||||
println(F("foc curr"));
|
||||
// change the velocity control limits if necessary
|
||||
motor->PID_velocity.limit = motor->current_limit;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case CMD_STATUS:
|
||||
// enable/disable
|
||||
printVerbose(F("Status: "));
|
||||
if(!GET) (bool)value ? motor->enable() : motor->disable();
|
||||
println(motor->enabled);
|
||||
break;
|
||||
default:
|
||||
target(motor, user_cmd, separator);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Commander::pid(PIDController* pid, char* user_cmd){
|
||||
char cmd = user_cmd[0];
|
||||
bool GET = isSentinel(user_cmd[1]);
|
||||
float value = atof(&user_cmd[1]);
|
||||
|
||||
switch (cmd){
|
||||
case SCMD_PID_P: // P gain change
|
||||
printVerbose("P: ");
|
||||
if(!GET) pid->P = value;
|
||||
println(pid->P);
|
||||
break;
|
||||
case SCMD_PID_I: // I gain change
|
||||
printVerbose("I: ");
|
||||
if(!GET) pid->I = value;
|
||||
println(pid->I);
|
||||
break;
|
||||
case SCMD_PID_D: // D gain change
|
||||
printVerbose("D: ");
|
||||
if(!GET) pid->D = value;
|
||||
println(pid->D);
|
||||
break;
|
||||
case SCMD_PID_RAMP: // ramp change
|
||||
printVerbose("ramp: ");
|
||||
if(!GET) pid->output_ramp = value;
|
||||
println(pid->output_ramp);
|
||||
break;
|
||||
case SCMD_PID_LIM: // limit change
|
||||
printVerbose("limit: ");
|
||||
if(!GET) pid->limit = value;
|
||||
println(pid->limit);
|
||||
break;
|
||||
default:
|
||||
printError();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Commander::lpf(LowPassFilter* lpf, char* user_cmd){
|
||||
char cmd = user_cmd[0];
|
||||
bool GET = isSentinel(user_cmd[1]);
|
||||
float value = atof(&user_cmd[1]);
|
||||
|
||||
switch (cmd){
|
||||
case SCMD_LPF_TF: // Tf value change
|
||||
printVerbose(F("Tf: "));
|
||||
if(!GET) lpf->Tf = value;
|
||||
println(lpf->Tf);
|
||||
break;
|
||||
default:
|
||||
printError();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Commander::scalar(float* value, char* user_cmd){
|
||||
bool GET = isSentinel(user_cmd[0]);
|
||||
if(!GET) *value = atof(user_cmd);
|
||||
println(*value);
|
||||
}
|
||||
|
||||
|
||||
void Commander::target(FOCMotor* motor, char* user_cmd, char* separator){
|
||||
// if no values sent
|
||||
if(isSentinel(user_cmd[0])) return;
|
||||
|
||||
float pos, vel, torque;
|
||||
char* next_value;
|
||||
switch(motor->controller){
|
||||
case MotionControlType::torque: // setting torque target
|
||||
torque = atof(strtok (user_cmd, separator));
|
||||
motor->target = torque;
|
||||
break;
|
||||
case MotionControlType::velocity: // setting velocity target + torque limit
|
||||
// set the target
|
||||
vel= atof(strtok (user_cmd, separator));
|
||||
motor->target = vel;
|
||||
|
||||
// allow for setting only the target velocity without chaning the torque limit
|
||||
next_value = strtok (NULL, separator);
|
||||
if (next_value){
|
||||
torque = atof(next_value);
|
||||
motor->PID_velocity.limit = torque;
|
||||
// torque command can be voltage or current
|
||||
if(!_isset(motor->phase_resistance) && motor->torque_controller == TorqueControlType::voltage) motor->voltage_limit = torque;
|
||||
else motor->current_limit = torque;
|
||||
}
|
||||
break;
|
||||
case MotionControlType::angle: // setting angle target + torque, velocity limit
|
||||
// setting the target position
|
||||
pos= atof(strtok (user_cmd, separator));
|
||||
motor->target = pos;
|
||||
|
||||
// allow for setting only the target position without chaning the velocity/torque limits
|
||||
next_value = strtok (NULL, separator);
|
||||
if( next_value ){
|
||||
vel = atof(next_value);
|
||||
motor->velocity_limit = vel;
|
||||
motor->P_angle.limit = vel;
|
||||
|
||||
// allow for setting only the target position and velocity limit without the torque limit
|
||||
next_value = strtok (NULL, separator);
|
||||
if( next_value ){
|
||||
torque= atof(next_value);
|
||||
motor->PID_velocity.limit = torque;
|
||||
// torque command can be voltage or current
|
||||
if(!_isset(motor->phase_resistance) && motor->torque_controller == TorqueControlType::voltage) motor->voltage_limit = torque;
|
||||
else motor->current_limit = torque;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case MotionControlType::velocity_openloop: // setting velocity target + torque limit
|
||||
// set the target
|
||||
vel= atof(strtok (user_cmd, separator));
|
||||
motor->target = vel;
|
||||
// allow for setting only the target velocity without chaning the torque limit
|
||||
next_value = strtok (NULL, separator);
|
||||
if (next_value ){
|
||||
torque = atof(next_value);
|
||||
// torque command can be voltage or current
|
||||
if(!_isset(motor->phase_resistance)) motor->voltage_limit = torque;
|
||||
else motor->current_limit = torque;
|
||||
}
|
||||
break;
|
||||
case MotionControlType::angle_openloop: // setting angle target + torque, velocity limit
|
||||
// set the target
|
||||
pos= atof(strtok (user_cmd, separator));
|
||||
motor->target = pos;
|
||||
|
||||
// allow for setting only the target position without chaning the velocity/torque limits
|
||||
next_value = strtok (NULL, separator);
|
||||
if( next_value ){
|
||||
vel = atof(next_value);
|
||||
motor->velocity_limit = vel;
|
||||
// allow for setting only the target velocity without chaning the torque limit
|
||||
next_value = strtok (NULL, separator);
|
||||
if (next_value ){
|
||||
torque = atof(next_value);
|
||||
// torque command can be voltage or current
|
||||
if(!_isset(motor->phase_resistance)) motor->voltage_limit = torque;
|
||||
else motor->current_limit = torque;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
printVerbose(F("Target: "));
|
||||
println(motor->target);
|
||||
}
|
||||
|
||||
|
||||
bool Commander::isSentinel(char ch)
|
||||
{
|
||||
if(ch == eol)
|
||||
return true;
|
||||
else if (ch == '\r')
|
||||
{
|
||||
printVerbose(F("Warn: \\r detected! \n"));
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void Commander::print(const int number){
|
||||
if( !com_port || verbose == VerboseMode::nothing ) return;
|
||||
com_port->print(number);
|
||||
}
|
||||
void Commander::print(const float number){
|
||||
if(!com_port || verbose == VerboseMode::nothing ) return;
|
||||
com_port->print((float)number,(int)decimal_places);
|
||||
}
|
||||
void Commander::print(const char* message){
|
||||
if(!com_port || verbose == VerboseMode::nothing ) return;
|
||||
com_port->print(message);
|
||||
}
|
||||
void Commander::print(const __FlashStringHelper *message){
|
||||
if(!com_port || verbose == VerboseMode::nothing ) return;
|
||||
com_port->print(message);
|
||||
}
|
||||
void Commander::print(const char message){
|
||||
if(!com_port || verbose == VerboseMode::nothing ) return;
|
||||
com_port->print(message);
|
||||
}
|
||||
|
||||
void Commander::println(const int number){
|
||||
if(!com_port || verbose == VerboseMode::nothing ) return;
|
||||
com_port->println(number);
|
||||
}
|
||||
void Commander::println(const float number){
|
||||
if(!com_port || verbose == VerboseMode::nothing ) return;
|
||||
com_port->println((float)number, (int)decimal_places);
|
||||
}
|
||||
void Commander::println(const char* message){
|
||||
if(!com_port || verbose == VerboseMode::nothing ) return;
|
||||
com_port->println(message);
|
||||
}
|
||||
void Commander::println(const __FlashStringHelper *message){
|
||||
if(!com_port || verbose == VerboseMode::nothing ) return;
|
||||
com_port->println(message);
|
||||
}
|
||||
void Commander::println(const char message){
|
||||
if(!com_port || verbose == VerboseMode::nothing ) return;
|
||||
com_port->println(message);
|
||||
}
|
||||
|
||||
|
||||
void Commander::printVerbose(const char* message){
|
||||
if(verbose == VerboseMode::user_friendly) print(message);
|
||||
}
|
||||
void Commander::printVerbose(const __FlashStringHelper *message){
|
||||
if(verbose == VerboseMode::user_friendly) print(message);
|
||||
}
|
||||
void Commander::printError(){
|
||||
println(F("err"));
|
||||
}
|
||||
Loaded 100 of 1854 files, more files were not shown because too many files have changed in this diff.
Show more
Reference in new issue
Block a user