summaryrefslogtreecommitdiff
path: root/n/asserv/src
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src')
-rw-r--r--n/asserv/src/asserv/main.c4
-rw-r--r--n/asserv/src/asserv/pos.c44
2 files changed, 36 insertions, 12 deletions
diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c
index 8382b39..63fd580 100644
--- a/n/asserv/src/asserv/main.c
+++ b/n/asserv/src/asserv/main.c
@@ -32,7 +32,7 @@
#include "misc.h"
-/** Motor command sequence. */
+/** Motor command sequence, do not use values above 127, do not use zero. */
uint8_t main_sequence, main_sequence_ack, main_sequence_finish;
/* This is implementation include. */
@@ -289,6 +289,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Set acknoledge.
* - b: ack sequence number. */
main_sequence_ack = args[0];
+ if (pos_blocked_state)
+ pos_reset ();
break;
/* Stats.
* - b: interval between stats. */
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;
}