Skip to content

Commit fe5bd2a

Browse files
askuricaskuric
committed
custom added
1 parent b42db27 commit fe5bd2a

4 files changed

Lines changed: 289 additions & 1 deletion

File tree

‎docs/simplefoc_library/code/motion_control/custom_control.md‎

Lines changed: 282 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,4 +11,285 @@ grand_parent: Writing the Code
1111
grand_grand_parent: Arduino <span class="simple">Simple<span class="foc">FOC</span>library</span>
1212
---
1313

14-
# Custom Motion Control
14+
# Custom Motion Control
15+
16+
<blockquote class="info"><p class="heading">New experimental feature (not recommended for beginners)</p>
17+
This feature is still in its early stages and we are not 100% happy with the API of the implementation. We are open to suggestions and contributions, so if you have any ideas on how to improve it, please let us know.
18+
19+
</blockquote>
20+
21+
This section is for advanced users that want to implement their own custom motion control algorithms.
22+
23+
<a href ="javascript:show('b','type');" class="btn btn-type btn-b btn-primary">BLDC motors</a>
24+
<a href ="javascript:show('s','type');" class="btn btn-type btn-s"> Stepper motors</a>
25+
26+
<img class="type type-b width60" src="extras/Images/cus_b.png"/>
27+
<img class="type type-s width60 hide" src="extras/Images/cus_s.png"/>
28+
29+
As all the other standard motion control approaches, the custom motion control approach is be executed in the motion control loop (`motor.move()`). This loop will calculate the target torque (current or voltage) to be applied to the motor using the underlying torque/FOC control loop (`motor.loopFOC()`) in order to achieve the desired motion.
30+
31+
To use the custom motion control loop, you need to set the `motor.controller` variable to `MotionControlType::custom`.
32+
```cpp
33+
// set FOC loop to be used
34+
motor.controller = MotionControlType::custom;
35+
```
36+
37+
You can find some examples in `examples/motion_control/custom_motion_control/` folder.
38+
39+
This `custom` motion control requires the user to provide their own implentation of the motion control:
40+
41+
```cpp
42+
// custom motion control function
43+
float custom_motion_control(FOCMotor& motor) {
44+
// your code here
45+
46+
// ex. simple proportional position control
47+
float error = motor->target - motor->shaft_angle;
48+
float target_torque = 10*error;
49+
50+
// return the target torque to be applied to the motor
51+
return target_torque;
52+
}
53+
```
54+
55+
And then link this callback function to the motor instance:
56+
```cpp
57+
motor.linkCustomMotionControl(custom_motion_control);
58+
```
59+
60+
## Target and limits
61+
62+
The custom motion control uses the `motor.target` variable as the desired user defied target entry, and the user can choose to respect or ignore the velocity limits `motor.velocity_limit`. The torque limits are enforeced by the underlying torque control loop, so they will be respected regardless if the
63+
user chooses to use them in their custom control or not. (If the user code returns a target torque that is higher than the limits set by the user, the library is going to limit it to the set limits before applying it to the motor).
64+
65+
The units of the target variable is completely user defined, so it can represent any physical quantity that the user wants to control.
66+
67+
For example, if the user wants to implement a torque control in Newton-meters (Nm), the user can set the `motor.target` variable to represent the target torque in Nm.
68+
69+
```cpp
70+
...
71+
float Kt = 0.1; // torque constant in Nm/A
72+
// custom motion control function
73+
float customTorqueControl(FOCMotor& motor) {
74+
// convert the target from Nm to Amps using the torque constant
75+
float target_in_Amps = motor->target / Kt;
76+
// return the target torque to be applied to the motor
77+
return target_in_Amps;
78+
}
79+
80+
void setup() {
81+
...
82+
// link the custom control method to the motor
83+
motor.linkCustomMotionControl(customTorqueControl);
84+
// set the control loop type to be used
85+
motor.controller = MotionControlType::custom;
86+
...
87+
}
88+
```
89+
90+
## What can I do with the custom motion control?
91+
92+
- Implement any motion control algorithm you want, for example:
93+
- Position control with custom trajectory generation
94+
- Velocity control with custom acceleration profiles
95+
- Advanced control algorithms like model predictive control, sliding mode control, etc.
96+
- Implement control schemes with multiple sensors,
97+
- using different sensors for position and velocity feedback,
98+
- using external sensors like IMUs
99+
- using secondary sensors after gearing
100+
etc.
101+
102+
## Custom control code example
103+
104+
<a href ="javascript:show('b','type');" class="btn btn-type btn-b btn-primary">BLDC motors</a>
105+
<a href ="javascript:show('s','type');" class="btn btn-type btn-s"> Stepper motors</a>
106+
107+
108+
Here is one basic example implementing custom motion control with the voltage mode torque control. The custom motion control is a simple proportional position control, where the target torque is proportional to the error between the target angle and the current angle. The user can change the PID parameters in runtime using the commander interface. As well as the target angle or change the motion control strategy to the standard ones.
109+
110+
<div class="type type-b" markdown="1">
111+
112+
```cpp
113+
#include <Arduino.h>
114+
#include <SimpleFOC.h>
115+
116+
// BLDC motor & driver instance
117+
BLDCMotor motor = BLDCMotor(7);
118+
BLDCDriver3PWM driver = BLDCDriver3PWM(D6, D10, D5, D8);
119+
120+
// encoder instance
121+
MagneticSensorSPI sensor = MagneticSensorSPI(AS5048_SPI, D4);
122+
123+
// commander communication instance
124+
Commander command = Commander(Serial);
125+
// void doMotion(char* cmd){ command.motion(&motor, cmd); }
126+
void doMotor(char* cmd){ command.motor(&motor, cmd); }
127+
128+
129+
// custom PID controller instance for the custom control method
130+
// P controller with gain of 1.0f, no integral or derivative gain
131+
PIDController custom_PID = PIDController(1.0f, 0, 0);
132+
// custom motion control method
133+
float positionPControl(FOCMotor* motor){
134+
// simple proportional position control
135+
float error = motor->target - motor->shaft_angle;
136+
// set the PID output limit to the motor voltage limit
137+
custom_PID.limit = motor->voltage_limit;
138+
return custom_PID(error); // return current command based on the error
139+
}
140+
141+
// optional add the PID to command to be able to tune it in runtime
142+
void doPID(char* cmd){ command.pid(&custom_PID, cmd); }
143+
144+
void setup() {
145+
// use monitoring with serial
146+
Serial.begin(115200);
147+
// enable more verbose output for debugging
148+
// comment out if not needed
149+
SimpleFOCDebug::enable(&Serial);
150+
151+
// initialize sensor hardware
152+
sensor.init();
153+
motor.linkSensor(&sensor);
154+
155+
// driver config
156+
// power supply voltage [V]
157+
driver.voltage_power_supply = 20;
158+
driver.init();
159+
// link driver
160+
motor.linkDriver(&driver);
161+
162+
// set the custom control method
163+
motor.linkCustomMotionControl(positionPControl);
164+
// set control loop type to be used
165+
motor.controller = MotionControlType::custom;
166+
// set the torque control type to voltage control
167+
motor.torque_controller = TorqueControlType::voltage;
168+
169+
// comment out if not needed
170+
motor.useMonitoring(Serial);
171+
motor.monitor_downsample = 0; // disable intially
172+
motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle
173+
174+
// subscribe motor to the commander
175+
//command.add('T', doMotion, "motion control"); // a bit less resouce intensive
176+
command.add('M', doMotor, "motor");
177+
command.add('C', doPID, "custom PID");
178+
179+
// initialise motor
180+
motor.init();
181+
// align encoder and start FOC
182+
motor.initFOC();
183+
184+
_delay(1000);
185+
}
186+
187+
void loop() {
188+
// iterative setting FOC phase voltage
189+
motor.loopFOC();
190+
191+
// iterative function setting the outter loop target
192+
motor.move();
193+
194+
// // motor monitoring
195+
motor.monitor();
196+
197+
// user communication
198+
command.run();
199+
}
200+
```
201+
202+
</div>
203+
204+
<div class="type type-s hide" markdown="1">
205+
206+
```cpp
207+
#include <Arduino.h>
208+
#include <SimpleFOC.h>
209+
210+
// Stepper motor & driver instance
211+
HybridStepperMotor motor = HybridStepperMotor(50);
212+
BLDCDriver3PWM driver = BLDCDriver3PWM(D6, D10, D5, D8);
213+
214+
// encoder instance
215+
MagneticSensorSPI sensor = MagneticSensorSPI(AS5048_SPI, D4);
216+
217+
// commander communication instance
218+
Commander command = Commander(Serial);
219+
// void doMotion(char* cmd){ command.motion(&motor, cmd); }
220+
void doMotor(char* cmd){ command.motor(&motor, cmd); }
221+
222+
223+
// custom PID controller instance for the custom control method
224+
// P controller with gain of 1.0f, no integral or derivative gain
225+
PIDController custom_PID = PIDController(1.0f, 0, 0);
226+
// custom motion control method
227+
float positionPControl(FOCMotor* motor){
228+
// simple proportional position control
229+
float error = motor->target - motor->shaft_angle;
230+
// set the PID output limit to the motor voltage limit
231+
custom_PID.limit = motor->voltage_limit;
232+
return custom_PID(error); // return current command based on the error
233+
}
234+
235+
// optional add the PID to command to be able to tune it in runtime
236+
void doPID(char* cmd){ command.pid(&custom_PID, cmd); }
237+
238+
void setup() {
239+
// use monitoring with serial
240+
Serial.begin(115200);
241+
// enable more verbose output for debugging
242+
// comment out if not needed
243+
SimpleFOCDebug::enable(&Serial);
244+
245+
// initialize sensor hardware
246+
sensor.init();
247+
motor.linkSensor(&sensor);
248+
249+
// driver config
250+
// power supply voltage [V]
251+
driver.voltage_power_supply = 20;
252+
driver.init();
253+
// link driver
254+
motor.linkDriver(&driver);
255+
256+
// set the custom control method
257+
motor.linkCustomMotionControl(positionPControl);
258+
// set control loop type to be used
259+
motor.controller = MotionControlType::custom;
260+
// set the torque control type to voltage control
261+
motor.torque_controller = TorqueControlType::voltage;
262+
263+
// comment out if not needed
264+
motor.useMonitoring(Serial);
265+
motor.monitor_downsample = 0; // disable intially
266+
motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle
267+
268+
// subscribe motor to the commander
269+
//command.add('T', doMotion, "motion control"); // a bit less resouce intensive
270+
command.add('M', doMotor, "motor");
271+
command.add('C', doPID, "custom PID");
272+
273+
// initialise motor
274+
motor.init();
275+
// align encoder and start FOC
276+
motor.initFOC();
277+
278+
_delay(1000);
279+
}
280+
281+
void loop() {
282+
// iterative setting FOC phase voltage
283+
motor.loopFOC();
284+
285+
// iterative function setting the outter loop target
286+
motor.move();
287+
288+
// // motor monitoring
289+
motor.monitor();
290+
291+
// user communication
292+
command.run();
293+
}
294+
```
295+
</div>

‎docs/simplefoc_library/code/motion_control/index.md‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -147,6 +147,13 @@ motor.controller = MotionControlType::angle_openloop;
147147

148148
## Custom control mode
149149

150+
151+
<a href ="javascript:show('b','type');" class="btn btn-type btn-b btn-primary">BLDC motors</a>
152+
<a href ="javascript:show('s','type');" class="btn btn-type btn-s"> Stepper motors</a>
153+
154+
<img class="type type-b width60" src="extras/Images/cus_b.png"/>
155+
<img class="type type-s width60 hide" src="extras/Images/cus_s.png"/>
156+
150157
The <span class="simple">Simple<span class="foc">FOC</span>library</span> also gives you the possibility to implement your own custom control loop. This is an experimental feature and we are not yet 100% happy with the implementation, but we wanted to give you the possibility to try it out and give us feedback. The custom control mode allows you to implement any control loop you want, and link it to the library. This way you can use the library's functions for reading sensors, controlling the motor and so on, while implementing your own control strategy.
151158

152159
```cpp

‎extras/Images/cus_b.png‎

38.3 KB
Loading

‎extras/Images/cus_s.png‎

37.1 KB
Loading

0 commit comments

Comments
 (0)