Skip to content

Commit 4fefc94

Browse files
at-watat-wat
authored
Clear internal control state on error (#92)
1 parent db0be91 commit 4fefc94

File tree

1 file changed

+80
-79
lines changed

1 file changed

+80
-79
lines changed

‎tfrog-motordriver/controlVelocity.c

Lines changed: 80 additions & 79 deletions
Original file line numberDiff line numberDiff line change
@@ -67,102 +67,103 @@ void ISR_VelocityControl()
6767
int64_t out_pwm[2];
6868
int64_t acc[2];
6969

70-
for (i = 0; i < 2; i++)
70+
YPSpur_servo_level servo_level[2] = {
71+
motor[0].servo_level, motor[1].servo_level,
72+
};
73+
if (motor[0].error_state || motor[1].error_state)
7174
{
72-
if (motor[i].servo_level == SERVO_LEVEL_STOP ||
73-
motor[0].error_state || motor[1].error_state)
74-
soft_start[i] = 0;
75-
else if (soft_start[i] < 128)
76-
soft_start[i]++;
77-
78-
if (motor[i].servo_level >= SERVO_LEVEL_TORQUE)
79-
{
80-
// servo_level 2(toque enable)
75+
// Don't output PWM if error is detected
76+
servo_level[0] = SERVO_LEVEL_OPENFREE;
77+
servo_level[1] = SERVO_LEVEL_OPENFREE;
78+
}
8179

82-
if (motor[i].servo_level >= SERVO_LEVEL_VELOCITY &&
83-
motor[i].servo_level != SERVO_LEVEL_OPENFREE)
84-
{
85-
int64_t acc_pi;
86-
87-
// 積分
88-
if (motor[i].control_init)
89-
{
90-
motor[i].ref.vel_diff = 0;
91-
motor[i].error = 0;
92-
motor[i].error_integ = 0;
93-
motor[i].control_init = 0;
94-
}
95-
96-
motor[i].ref.vel_interval++;
97-
if (motor[i].ref.vel_changed)
98-
{
99-
motor[i].ref.vel_buf = motor[i].ref.vel;
100-
motor[i].ref.vel_diff = (motor[i].ref.vel_buf - motor[i].ref.vel_buf_prev) / motor[i].ref.vel_interval;
101-
// [cnt/msms]
102-
103-
motor[i].ref.vel_buf_prev = motor[i].ref.vel_buf;
104-
motor[i].ref.vel_interval = 0;
105-
106-
motor[i].ref.vel_changed = 0;
107-
}
108-
else if (motor[i].ref.vel_interval > 128)
109-
{
110-
motor[i].ref.vel_diff = 0;
111-
}
112-
113-
motor[i].error = motor[i].ref.vel - motor[i].vel;
114-
motor[i].error_integ += motor[i].error;
115-
if (motor[i].error_integ > motor_param[i].integ_max)
116-
{
117-
motor[i].error_integ = motor_param[i].integ_max;
118-
}
119-
else if (motor[i].error_integ < motor_param[i].integ_min)
120-
{
121-
motor[i].error_integ = motor_param[i].integ_min;
122-
}
123-
124-
// PI制御分 単位：加速度[cnt/ss]
125-
acc_pi = ((int64_t)motor[i].error * motor_param[i].Kp) * driver_param.control_s;
126-
// [cnt/ms] * 1000[ms/s] * Kp[1/s] = [cnt/ss]
127-
acc_pi += motor[i].error_integ * motor_param[i].Ki;
128-
// [cnt] * Ki[1/ss] = [cnt/ss]
129-
130-
acc[i] = (acc_pi +
131-
(int64_t)Filter1st_Filter(&accelf[i], motor[i].ref.vel_diff) *
132-
driver_param.control_s * driver_param.control_s) /
133-
16;
134-
// [cnt/msms] * 1000[ms/s] * 1000[ms/s] = [cnt/ss]
135-
}
136-
else
137-
{
138-
motor[i].ref.vel_buf_prev = motor[i].vel;
139-
motor[i].ref.vel_buf = motor[i].vel;
140-
motor[i].ref.vel = motor[i].vel;
141-
motor[i].error_integ = 0;
142-
motor[i].ref.vel_diff = 0;
143-
Filter1st_Filter(&accelf[i], 0);
144-
acc[i] = 0;
145-
}
146-
}
147-
else
80+
// Calculate acceleration from reference velocity by PI control
81+
for (i = 0; i < 2; i++)
82+
{
83+
if (servo_level[i] != SERVO_LEVEL_VELOCITY &&
84+
servo_level[i] != SERVO_LEVEL_POSITION)
14885
{
14986
motor[i].ref.rate = 0;
15087
motor[i].ref.vel_buf_prev = motor[i].vel;
15188
motor[i].ref.vel_buf = motor[i].vel;
15289
motor[i].ref.vel = motor[i].vel;
90+
motor[i].control_init = 1;
15391
Filter1st_Filter(&accelf[i], 0);
15492
acc[i] = 0;
93+
continue;
94+
}
95+
96+
// Clear integral term
97+
if (motor[i].control_init)
98+
{
99+
motor[i].ref.vel_diff = 0;
100+
motor[i].error = 0;
101+
motor[i].error_integ = 0;
102+
motor[i].control_init = 0;
155103
}
104+
105+
motor[i].ref.vel_interval++;
106+
if (motor[i].ref.vel_changed)
107+
{
108+
motor[i].ref.vel_buf = motor[i].ref.vel;
109+
motor[i].ref.vel_diff = (motor[i].ref.vel_buf - motor[i].ref.vel_buf_prev) / motor[i].ref.vel_interval;
110+
// [cnt/msms]
111+
112+
motor[i].ref.vel_buf_prev = motor[i].ref.vel_buf;
113+
motor[i].ref.vel_interval = 0;
114+
115+
motor[i].ref.vel_changed = 0;
116+
}
117+
else if (motor[i].ref.vel_interval > 128)
118+
{
119+
motor[i].ref.vel_diff = 0;
120+
}
121+
122+
motor[i].error = motor[i].ref.vel - motor[i].vel;
123+
motor[i].error_integ += motor[i].error;
124+
if (motor[i].error_integ > motor_param[i].integ_max)
125+
{
126+
motor[i].error_integ = motor_param[i].integ_max;
127+
}
128+
else if (motor[i].error_integ < motor_param[i].integ_min)
129+
{
130+
motor[i].error_integ = motor_param[i].integ_min;
131+
}
132+
133+
int64_t acc_pi;
134+
// PI制御分 単位：加速度[cnt/ss]
135+
acc_pi = ((int64_t)motor[i].error * motor_param[i].Kp) * driver_param.control_s;
136+
// [cnt/ms] * 1000[ms/s] * Kp[1/s] = [cnt/ss]
137+
acc_pi += motor[i].error_integ * motor_param[i].Ki;
138+
// [cnt] * Ki[1/ss] = [cnt/ss]
139+
140+
acc[i] = (acc_pi +
141+
(int64_t)Filter1st_Filter(&accelf[i], motor[i].ref.vel_diff) *
142+
driver_param.control_s * driver_param.control_s) /
143+
16;
144+
// [cnt/msms] * 1000[ms/s] * 1000[ms/s] = [cnt/ss]
156145
}
157146

147+
// Calculate PWM duty ratio from acceleration and velocity
158148
for (i = 0; i < 2; i++)
159149
{
160-
if (motor[i].servo_level >= SERVO_LEVEL_TORQUE)
150+
if (servo_level[i] == SERVO_LEVEL_STOP ||
151+
servo_level[i] == SERVO_LEVEL_OPENFREE)
152+
{
153+
// Soft-start if it is uncontrolled state (STOP, OPENFREE)
154+
soft_start[i] = 0;
155+
}
156+
else if (soft_start[i] < 128)
157+
{
158+
soft_start[i]++;
159+
}
160+
161+
if (servo_level[i] >= SERVO_LEVEL_TORQUE)
161162
{
162163
// servo_level 2(toque enable)
163164

164-
if (motor[i].servo_level >= SERVO_LEVEL_VELOCITY &&
165-
motor[i].servo_level != SERVO_LEVEL_OPENFREE)
165+
if (servo_level[i] >= SERVO_LEVEL_VELOCITY &&
166+
servo_level[i] != SERVO_LEVEL_OPENFREE)
166167
{
167168
int32_t ipair;
168169
if (i == 0)

0 commit comments

Comments
 (0)