summaryrefslogtreecommitdiff
path: root/n/asserv/src/asserv/pos.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src/asserv/pos.c')
-rw-r--r--n/asserv/src/asserv/pos.c44
1 files changed, 33 insertions, 11 deletions
diff --git a/n/asserv/src/asserv/pos.c b/n/asserv/src/asserv/pos.c
index 1beffc7..eecd71f 100644
--- a/n/asserv/src/asserv/pos.c
+++ b/n/asserv/src/asserv/pos.c
@@ -46,11 +46,16 @@ uint16_t pos_theta_kp, pos_alpha_kp;
uint16_t pos_theta_ki, pos_alpha_ki;
/** D coefficients. */
uint16_t pos_theta_kd, pos_alpha_kd;
+/** Blocked value. If error is greater than this value, stop the robot and
+ * report blocked state. */
+int32_t pos_blocked = 15000L;
/** Current integral values. */
int32_t pos_theta_int, pos_alpha_int;
/** Last error values. */
int32_t pos_theta_e_old, pos_alpha_e_old;
+/** One if blocked. */
+uint8_t pos_blocked_state;
/* +AutoDec */
/* -AutoDec */
@@ -94,21 +99,37 @@ static void
pos_update (void)
{
int16_t pid_theta, pid_alpha;
+ int32_t diff_theta, diff_alpha;
/* Update current shaft positions. */
pos_theta_cur += counter_left_diff + counter_right_diff;
pos_alpha_cur += counter_right_diff - counter_left_diff;
/* Compute PID. */
- pid_theta = pos_compute_pid (pos_theta_cons - pos_theta_cur,
- &pos_theta_int, &pos_theta_e_old,
- pos_theta_kp, pos_theta_ki, pos_theta_kd);
- pid_alpha = pos_compute_pid (pos_alpha_cons - pos_alpha_cur,
- &pos_alpha_int, &pos_alpha_e_old,
- pos_alpha_kp, pos_alpha_ki, pos_alpha_kd);
- /* Update PWM. */
- pwm_left = pid_theta - pid_alpha;
- UTILS_BOUND (pwm_left, -PWM_MAX, PWM_MAX);
- pwm_right = pid_theta + pid_alpha;
- UTILS_BOUND (pwm_right, -PWM_MAX, PWM_MAX);
+ diff_theta = pos_theta_cons - pos_theta_cur;
+ diff_alpha = pos_alpha_cons - pos_alpha_cur;
+ if (pos_blocked_state
+ || diff_theta < -pos_blocked || pos_blocked < diff_theta
+ || diff_alpha < -pos_blocked || pos_blocked < diff_alpha)
+ {
+ /* Blocked. */
+ pwm_left = 0;
+ pwm_right = 0;
+ pos_blocked_state = 1;
+ main_sequence_finish = main_sequence | 0x80;
+ }
+ else
+ {
+ pid_theta = pos_compute_pid (diff_theta, &pos_theta_int,
+ &pos_theta_e_old, pos_theta_kp,
+ pos_theta_ki, pos_theta_kd);
+ pid_alpha = pos_compute_pid (diff_alpha, &pos_alpha_int,
+ &pos_alpha_e_old, pos_alpha_kp,
+ pos_alpha_ki, pos_alpha_kd);
+ /* Update PWM. */
+ pwm_left = pid_theta - pid_alpha;
+ UTILS_BOUND (pwm_left, -PWM_MAX, PWM_MAX);
+ pwm_right = pid_theta + pid_alpha;
+ UTILS_BOUND (pwm_right, -PWM_MAX, PWM_MAX);
+ }
}
/** Reset position control internal state. */
@@ -119,4 +140,5 @@ pos_reset (void)
pos_theta_cur = pos_alpha_cur = 0;
pos_theta_cons = pos_alpha_cons = 0;
pos_theta_e_old = pos_alpha_e_old = 0;
+ pos_blocked_state = 0;
}