summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/asserv/src/asserv/eeprom.avr.c30
-rw-r--r--digital/asserv/src/asserv/eeprom.h2
-rw-r--r--digital/asserv/src/asserv/main.c31
-rw-r--r--digital/asserv/src/asserv/pos.c62
-rw-r--r--digital/asserv/src/asserv/pos.h8
-rw-r--r--digital/asserv/tools/asserv/asserv.py8
6 files changed, 91 insertions, 50 deletions
diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c
index cbb92b33..ddc10f3c 100644
--- a/digital/asserv/src/asserv/eeprom.avr.c
+++ b/digital/asserv/src/asserv/eeprom.avr.c
@@ -99,12 +99,21 @@ eeprom_read_params (void)
pos_alpha.kd = eeprom_read_word (p16++);
pos_aux[0].kd = eeprom_read_word (p16++);
pos_aux[1].kd = eeprom_read_word (p16++);
+ pos_theta.blocked_error_limit = eeprom_read_word (p16++);
+ pos_theta.blocked_speed_limit = eeprom_read_word (p16++);
+ pos_theta.blocked_counter_limit = eeprom_read_word (p16++);
+ pos_alpha.blocked_error_limit = eeprom_read_word (p16++);
+ pos_alpha.blocked_speed_limit = eeprom_read_word (p16++);
+ pos_alpha.blocked_counter_limit = eeprom_read_word (p16++);
+ pos_aux[0].blocked_error_limit = eeprom_read_word (p16++);
+ pos_aux[0].blocked_speed_limit = eeprom_read_word (p16++);
+ pos_aux[0].blocked_counter_limit = eeprom_read_word (p16++);
+ pos_aux[1].blocked_error_limit = eeprom_read_word (p16++);
+ pos_aux[1].blocked_speed_limit = eeprom_read_word (p16++);
+ pos_aux[1].blocked_counter_limit = eeprom_read_word (p16++);
pos_e_sat = eeprom_read_word (p16++);
pos_i_sat = eeprom_read_word (p16++);
pos_d_sat = eeprom_read_word (p16++);
- pos_blocked_error_limit = eeprom_read_word (p16++);
- pos_blocked_speed_limit = eeprom_read_word (p16++);
- pos_blocked_counter_limit = eeprom_read_word (p16++);
traj_eps = eeprom_read_word (p16++);
traj_aeps = eeprom_read_word (p16++);
traj_set_angle_limit (eeprom_read_word (p16++));
@@ -145,12 +154,21 @@ eeprom_write_params (void)
eeprom_write_word (p16++, pos_alpha.kd);
eeprom_write_word (p16++, pos_aux[0].kd);
eeprom_write_word (p16++, pos_aux[1].kd);
+ eeprom_write_word (p16++, pos_theta.blocked_error_limit);
+ eeprom_write_word (p16++, pos_theta.blocked_speed_limit);
+ eeprom_write_word (p16++, pos_theta.blocked_counter_limit);
+ eeprom_write_word (p16++, pos_alpha.blocked_error_limit);
+ eeprom_write_word (p16++, pos_alpha.blocked_speed_limit);
+ eeprom_write_word (p16++, pos_alpha.blocked_counter_limit);
+ eeprom_write_word (p16++, pos_aux[0].blocked_error_limit);
+ eeprom_write_word (p16++, pos_aux[0].blocked_speed_limit);
+ eeprom_write_word (p16++, pos_aux[0].blocked_counter_limit);
+ eeprom_write_word (p16++, pos_aux[1].blocked_error_limit);
+ eeprom_write_word (p16++, pos_aux[1].blocked_speed_limit);
+ eeprom_write_word (p16++, pos_aux[1].blocked_counter_limit);
eeprom_write_word (p16++, pos_e_sat);
eeprom_write_word (p16++, pos_i_sat);
eeprom_write_word (p16++, pos_d_sat);
- eeprom_write_word (p16++, pos_blocked_error_limit);
- eeprom_write_word (p16++, pos_blocked_speed_limit);
- eeprom_write_word (p16++, pos_blocked_counter_limit);
eeprom_write_word (p16++, traj_eps);
eeprom_write_word (p16++, traj_aeps);
eeprom_write_word (p16++, traj_angle_limit);
diff --git a/digital/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h
index 052f37de..000a0e14 100644
--- a/digital/asserv/src/asserv/eeprom.h
+++ b/digital/asserv/src/asserv/eeprom.h
@@ -26,7 +26,7 @@
* }}} */
/** Change the eeprom key each time you change eeprom format. */
-#define EEPROM_KEY 0x4d
+#define EEPROM_KEY 0x4e
void
eeprom_read_params (void);
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 04352f3b..527b1940 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -630,6 +630,17 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
if (!pos) { proto_send0 ('?'); return; }
pos->kd = v8_to_v16 (args[2], args[3]);
break;
+ case c ('b', 7):
+ /* Set blocking detection parameters.
+ * - b: index.
+ * - w: error limit.
+ * - w: speed limit.
+ * - b: counter limit. */
+ if (!pos) { proto_send0 ('?'); return; }
+ pos->blocked_error_limit = v8_to_v16 (args[2], args[3]);
+ pos->blocked_speed_limit = v8_to_v16 (args[4], args[5]);
+ pos->blocked_counter_limit = args[6];
+ break;
case c ('E', 3):
pos_e_sat = v8_to_v16 (args[1], args[2]);
break;
@@ -639,11 +650,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('D', 3):
pos_d_sat = v8_to_v16 (args[1], args[2]);
break;
- case c ('b', 7):
- pos_blocked_error_limit = v8_to_v16 (args[1], args[2]);
- pos_blocked_speed_limit = v8_to_v16 (args[3], args[4]);
- pos_blocked_counter_limit = v8_to_v16 (args[5], args[6]);
- break;
case c ('e', 5):
traj_eps = v8_to_v16 (args[1], args[2]);
traj_aeps = v8_to_v16 (args[3], args[4]);
@@ -672,21 +678,30 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
proto_send2w ('a', speed_theta.acc, speed_alpha.acc);
proto_send2b ('s', speed_theta.max, speed_theta.slow);
proto_send2b ('s', speed_alpha.max, speed_alpha.slow);
+ proto_send3w ('b', pos_theta.blocked_error_limit,
+ pos_theta.blocked_speed_limit,
+ pos_theta.blocked_counter_limit);
+ proto_send3w ('b', pos_alpha.blocked_error_limit,
+ pos_alpha.blocked_speed_limit,
+ pos_alpha.blocked_counter_limit);
proto_send2w ('p', pos_theta.kp, pos_alpha.kp);
proto_send2w ('i', pos_theta.ki, pos_alpha.ki);
proto_send2w ('d', pos_theta.kd, pos_alpha.kd);
proto_send2w ('a', speed_aux[0].acc, speed_aux[1].acc);
proto_send2b ('s', speed_aux[0].max, speed_aux[0].slow);
proto_send2b ('s', speed_aux[1].max, speed_aux[1].slow);
+ proto_send3w ('b', pos_aux[0].blocked_error_limit,
+ pos_aux[0].blocked_speed_limit,
+ pos_aux[0].blocked_counter_limit);
+ proto_send3w ('b', pos_aux[1].blocked_error_limit,
+ pos_aux[1].blocked_speed_limit,
+ pos_aux[1].blocked_counter_limit);
proto_send2w ('p', pos_aux[0].kp, pos_aux[1].kp);
proto_send2w ('i', pos_aux[0].ki, pos_aux[1].ki);
proto_send2w ('d', pos_aux[0].kd, pos_aux[1].kd);
proto_send1w ('E', pos_e_sat);
proto_send1w ('I', pos_i_sat);
proto_send1w ('D', pos_d_sat);
- proto_send3w ('b', pos_blocked_error_limit,
- pos_blocked_speed_limit,
- pos_blocked_counter_limit);
proto_send2w ('e', traj_eps, traj_aeps);
proto_send1w ('l', traj_angle_limit);
proto_send1b ('w', pwm_reverse);
diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c
index 0679de40..dc995a16 100644
--- a/digital/asserv/src/asserv/pos.c
+++ b/digital/asserv/src/asserv/pos.c
@@ -52,12 +52,6 @@ int32_t pos_e_sat = 1023;
int32_t pos_i_sat = 1023;
/** Differential saturation. */
int32_t pos_d_sat = 1023;
-/** Blocking detection: error limit. */
-int32_t pos_blocked_error_limit = 2048;
-/** Blocking detection: speed limit. */
-int32_t pos_blocked_speed_limit = 0x10;
-/** Blocking detection: counter limit. */
-int32_t pos_blocked_counter_limit = 20;
/** Compute a PID.
* How to compute maximum numbers size:
@@ -103,34 +97,33 @@ pos_update_polar (struct state_t *state,
if (state->mode >= MODE_POS)
{
int16_t pid_theta, pid_alpha;
- int32_t diff_theta, diff_alpha;
+ int32_t error_theta, error_alpha;
+ int16_t cur_speed_theta, cur_speed_alpha;
/* Update current shaft positions. */
- pos_theta->cur += counter_left_diff + counter_right_diff;
- pos_alpha->cur += counter_right_diff - counter_left_diff;
+ cur_speed_theta = counter_left_diff + counter_right_diff;
+ cur_speed_alpha = counter_right_diff - counter_left_diff;
+ pos_theta->cur += cur_speed_theta;
+ pos_alpha->cur += cur_speed_alpha;
if (state->variant & 1)
pos_reset (pos_theta);
if (state->variant & 2)
pos_reset (pos_alpha);
- /* Compute PID. */
- diff_theta = pos_theta->cons - pos_theta->cur;
- diff_alpha = pos_alpha->cons - pos_alpha->cur;
- /* Compute actual speed and test for blocking. */
- int32_t cur_speed_theta = pos_theta->cur - pos_theta->cur_old;
- pos_theta->cur_old = pos_theta->cur;
- int32_t cur_speed_alpha = pos_alpha->cur - pos_alpha->cur_old;
- pos_alpha->cur_old = pos_alpha->cur;
- if ((UTILS_ABS (diff_theta) > pos_blocked_error_limit
- && UTILS_ABS (cur_speed_theta) < pos_blocked_speed_limit))
+ /* Compute error. */
+ error_theta = pos_theta->cons - pos_theta->cur;
+ error_alpha = pos_alpha->cons - pos_alpha->cur;
+ /* Test for blocking. */
+ if (UTILS_ABS (error_theta) > pos_theta->blocked_error_limit
+ && UTILS_ABS (cur_speed_theta) < pos_theta->blocked_speed_limit)
pos_theta->blocked_counter++;
else
pos_theta->blocked_counter = 0;
- if ((UTILS_ABS (diff_alpha) > pos_blocked_error_limit
- && UTILS_ABS (cur_speed_alpha) < pos_blocked_speed_limit))
+ if (UTILS_ABS (error_alpha) > pos_alpha->blocked_error_limit
+ && UTILS_ABS (cur_speed_alpha) < pos_alpha->blocked_speed_limit)
pos_alpha->blocked_counter++;
else
pos_alpha->blocked_counter = 0;
- if (pos_theta->blocked_counter > pos_blocked_counter_limit
- || pos_alpha->blocked_counter > pos_blocked_counter_limit)
+ if (pos_theta->blocked_counter > pos_theta->blocked_counter_limit
+ || pos_alpha->blocked_counter > pos_alpha->blocked_counter_limit)
{
/* Blocked. */
pos_reset (pos_theta);
@@ -141,8 +134,9 @@ pos_update_polar (struct state_t *state,
}
else
{
- pid_theta = pos_compute_pid (diff_theta, pos_theta);
- pid_alpha = pos_compute_pid (diff_alpha, pos_alpha);
+ /* Compute PID. */
+ pid_theta = pos_compute_pid (error_theta, pos_theta);
+ pid_alpha = pos_compute_pid (error_alpha, pos_alpha);
/* Update PWM. */
pwm_set (pwm_left, pid_theta - pid_alpha);
pwm_set (pwm_right, pid_theta + pid_alpha);
@@ -158,12 +152,18 @@ pos_update_single (struct state_t *state, struct pos_t *pos,
if (state->mode >= MODE_POS)
{
int16_t pid;
- int32_t diff;
+ int32_t error;
/* Update current shaft position. */
pos->cur += counter_diff;
- /* Compute PID. */
- diff = pos->cons - pos->cur;
- if (UTILS_ABS (diff) > 5000)
+ /* Compute error. */
+ error = pos->cons - pos->cur;
+ /* Test or blocking. */
+ if (UTILS_ABS (error) > pos->blocked_error_limit
+ && UTILS_ABS (counter_diff) < pos->blocked_speed_limit)
+ pos->blocked_counter++;
+ else
+ pos->blocked_counter = 0;
+ if (pos->blocked_counter > pos->blocked_counter_limit)
{
/* Blocked. */
pos_reset (pos);
@@ -172,7 +172,8 @@ pos_update_single (struct state_t *state, struct pos_t *pos,
}
else
{
- pid = pos_compute_pid (diff, pos);
+ /* Compute PID. */
+ pid = pos_compute_pid (error, pos);
/* Update PWM. */
pwm_set (pwm, pid);
}
@@ -201,7 +202,6 @@ pos_reset (struct pos_t *pos)
pos->cur = 0;
pos->cons = 0;
pos->e_old = 0;
- pos->cur_old = 0;
pos->blocked_counter = 0;
}
diff --git a/digital/asserv/src/asserv/pos.h b/digital/asserv/src/asserv/pos.h
index 4ced49b8..5c586026 100644
--- a/digital/asserv/src/asserv/pos.h
+++ b/digital/asserv/src/asserv/pos.h
@@ -38,8 +38,12 @@ struct pos_t
int32_t i;
/** Last error value. */
int32_t e_old;
- /** Old current position. */
- uint32_t cur_old;
+ /** Blocking detection: error limit. */
+ int32_t blocked_error_limit;
+ /** Blocking detection: speed limit. */
+ int16_t blocked_speed_limit;
+ /** Blocking detection: counter limit. */
+ uint8_t blocked_counter_limit;
/** Count the number of blocked detection. */
uint8_t blocked_counter;
};
diff --git a/digital/asserv/tools/asserv/asserv.py b/digital/asserv/tools/asserv/asserv.py
index 323d74c6..5c7830c6 100644
--- a/digital/asserv/tools/asserv/asserv.py
+++ b/digital/asserv/tools/asserv/asserv.py
@@ -106,14 +106,17 @@ class Proto:
scale = 1,
tkp = 0, tki = 0, tkd = 0,
ta = 1, tsm = 0, tss = 0,
+ tbe = 2048, tbs = 0x10, tbc = 20,
akp = 0, aki = 0, akd = 0,
aa = 1, asm = 0, ass = 0,
+ abe = 2048, abs = 0x10, abc = 20,
a0kp = 0, a0ki = 0, a0kd = 0,
a0a = 1, a0sm = 0, a0ss = 0,
+ a0be = 2048, a0bs = 0x10, a0bc = 20,
a1kp = 0, a1ki = 0, a1kd = 0,
a1a = 1, a1sm = 0, a1ss = 0,
+ a1be = 2048, a1bs = 0x10, a1bc = 20,
E = 1023, I = 1023, D = 1023,
- be = 2048, bs = 0x10, bc = 20,
c = 1, f = 0x1000,
l = 0x2000,
w = 0x00,
@@ -273,10 +276,11 @@ class Proto:
self.proto.send ('p', 'cBH', 'd', index, f88 (p[m + 'kd']))
self.proto.send ('p', 'cBH', 'a', index, f88 (p[m + 'a']))
self.proto.send ('p', 'cBBB', 's', index, p[m + 'sm'], p[m + 'ss'])
+ self.proto.send ('p', 'cBHHB', 'b', index, p[m + 'be'],
+ p[m + 'bs'], p[m + 'bc'])
self.proto.send ('p', 'cH', 'E', p['E'])
self.proto.send ('p', 'cH', 'I', p['I'])
self.proto.send ('p', 'cH', 'D', p['D'])
- self.proto.send ('p', 'cHHH', 'b', p['be'], p['bs'], p['bc'])
self.proto.send ('p', 'cL', 'c', f824 (p['c']))
self.proto.send ('p', 'cH', 'f', p['f'])
self.proto.send ('p', 'cH', 'l', p['l'])