AN2350 Datasheet, PDF (34/41 Pages) STMicroelectronics – Guidelines for the control of a multiaxial planar robot with ST10F276

Control algorithm
Figure 26. PID position control ISR
Reload of Compare unit CC2
and Reset errors
AN2350
T2
sh_pulses_desired
T4
elb_pulses_desired
Encoder feedback for
shoulder and error
computation
error_shoulder
PW0
PID_Position_Control
(max 90% - min 10%)
for the shoulder
or
P_sh , I_sh , D_sh
(max 65% - min 35%)
Encoder feedback for
elbow and error
computation
error_elbow
PID_Position_Control
for the elbow
P_elb , I_elb , D_elb
PW1
(max 90% - min 10%)
or
(max 65% - min 35%)
Compute total error as
abs(error shoulder)
+
abs(error_elbow)
total_error
N
>
MAX_ERR_POS
flag_error = 0
Y
flag_error = 1
34/41