summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--digital/asserv/src/asserv/eeprom.avr.c17
-rw-r--r--digital/asserv/src/asserv/eeprom.h4
-rw-r--r--digital/asserv/src/asserv/main.c94
-rw-r--r--digital/asserv/src/asserv/pos.c75
-rw-r--r--digital/asserv/src/asserv/pos.h1
-rw-r--r--digital/asserv/src/asserv/speed.c30
-rw-r--r--digital/asserv/src/asserv/speed.h1
7 files changed, 173 insertions, 49 deletions
diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c
index dbbdbdec..96611d3b 100644
--- a/digital/asserv/src/asserv/eeprom.avr.c
+++ b/digital/asserv/src/asserv/eeprom.avr.c
@@ -34,6 +34,11 @@
#define EEPROM_START 0
+/* WARNING:
+ * If you change EEPROM format, be sure to change the EEPROM_KEY in header if
+ * your new format is not compatible with the old one or you will load
+ * garbages in parameters. */
+
/* Read parameters from eeprom. */
void
eeprom_read_params (void)
@@ -44,19 +49,25 @@ eeprom_read_params (void)
return;
speed_theta.max = eeprom_read_byte (p8++);
speed_alpha.max = eeprom_read_byte (p8++);
+ speed_aux0.max = eeprom_read_byte (p8++);
speed_theta.slow = eeprom_read_byte (p8++);
speed_alpha.slow = eeprom_read_byte (p8++);
+ speed_aux0.slow = eeprom_read_byte (p8++);
pwm_set_reverse (eeprom_read_byte (p8++));
p16 = (uint16_t *) p8;
postrack_set_footing (eeprom_read_word (p16++));
speed_theta.acc = eeprom_read_word (p16++);
speed_alpha.acc = eeprom_read_word (p16++);
+ speed_aux0.acc = eeprom_read_word (p16++);
pos_theta.kp = eeprom_read_word (p16++);
pos_alpha.kp = eeprom_read_word (p16++);
+ pos_aux0.kp = eeprom_read_word (p16++);
pos_theta.ki = eeprom_read_word (p16++);
pos_alpha.ki = eeprom_read_word (p16++);
+ pos_aux0.ki = eeprom_read_word (p16++);
pos_theta.kd = eeprom_read_word (p16++);
pos_alpha.kd = eeprom_read_word (p16++);
+ pos_aux0.kd = eeprom_read_word (p16++);
pos_e_sat = eeprom_read_word (p16++);
pos_int_sat = eeprom_read_word (p16++);
pos_blocked = eeprom_read_word (p16++);
@@ -71,19 +82,25 @@ eeprom_write_params (void)
eeprom_write_byte (p8++, EEPROM_KEY);
eeprom_write_byte (p8++, speed_theta.max);
eeprom_write_byte (p8++, speed_alpha.max);
+ eeprom_write_byte (p8++, speed_aux0.max);
eeprom_write_byte (p8++, speed_theta.slow);
eeprom_write_byte (p8++, speed_alpha.slow);
+ eeprom_write_byte (p8++, speed_aux0.slow);
eeprom_write_byte (p8++, pwm_reverse);
p16 = (uint16_t *) p8;
eeprom_write_word (p16++, postrack_footing);
eeprom_write_word (p16++, speed_theta.acc);
eeprom_write_word (p16++, speed_alpha.acc);
+ eeprom_write_word (p16++, speed_aux0.acc);
eeprom_write_word (p16++, pos_theta.kp);
eeprom_write_word (p16++, pos_alpha.kp);
+ eeprom_write_word (p16++, pos_aux0.kp);
eeprom_write_word (p16++, pos_theta.ki);
eeprom_write_word (p16++, pos_alpha.ki);
+ eeprom_write_word (p16++, pos_aux0.ki);
eeprom_write_word (p16++, pos_theta.kd);
eeprom_write_word (p16++, pos_alpha.kd);
+ eeprom_write_word (p16++, pos_aux0.kd);
eeprom_write_word (p16++, pos_e_sat);
eeprom_write_word (p16++, pos_int_sat);
eeprom_write_word (p16++, pos_blocked);
diff --git a/digital/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h
index 929d0da6..95bb4e64 100644
--- a/digital/asserv/src/asserv/eeprom.h
+++ b/digital/asserv/src/asserv/eeprom.h
@@ -25,8 +25,8 @@
*
* }}} */
-/* Change the eeprom key each time you change eeprom format. */
-#define EEPROM_KEY 0x45
+/** Change the eeprom key each time you change eeprom format. */
+#define EEPROM_KEY 0x46
void
eeprom_read_params (void);
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 6d761315..38a2e9f8 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -59,6 +59,9 @@ uint8_t main_stat_counter, main_stat_counter_cpt;
/** Report of position. */
uint8_t main_stat_postrack, main_stat_postrack_cpt;
+/** Report of auxiliary position. */
+uint8_t main_stat_aux_pos, main_stat_aux_pos_cpt;
+
/** Statistics about speed control. */
uint8_t main_stat_speed, main_stat_speed_cpt;
@@ -122,8 +125,7 @@ main_loop (void)
counter_update ();
main_timer[0] = timer_read ();
/* Position control. */
- if (state_main.mode >= MODE_POS)
- pos_update ();
+ pos_update ();
main_timer[1] = timer_read ();
/* Pwm setup. */
pwm_update ();
@@ -134,8 +136,7 @@ main_loop (void)
if (state_main.mode >= MODE_TRAJ)
traj_update ();
/* Speed control. */
- if (state_main.mode >= MODE_SPEED)
- speed_update ();
+ speed_update ();
main_timer[3] = timer_read ();
/* Stats. */
if (main_sequence_ack
@@ -156,15 +157,22 @@ main_loop (void)
proto_send3d ('X', postrack_x, postrack_y, postrack_a);
main_stat_postrack_cpt = main_stat_postrack;
}
+ if (main_stat_aux_pos && !--main_stat_aux_pos_cpt)
+ {
+ proto_send1w ('Z', pos_aux0.cur);
+ main_stat_aux_pos_cpt = main_stat_aux_pos;
+ }
if (main_stat_speed && !--main_stat_speed_cpt)
{
- proto_send2b ('S', speed_theta.cur >> 8, speed_alpha.cur >> 8);
+ proto_send3b ('S', speed_theta.cur >> 8, speed_alpha.cur >> 8,
+ speed_aux0.cur >> 8);
main_stat_speed_cpt = main_stat_speed;
}
if (main_stat_pos && !--main_stat_pos_cpt)
{
- proto_send4w ('P', pos_theta.e_old, pos_theta.i,
- pos_alpha.e_old, pos_alpha.i);
+ proto_send6w ('P', pos_theta.e_old, pos_theta.i,
+ pos_alpha.e_old, pos_alpha.i,
+ pos_aux0.e_old, pos_aux0.i);
main_stat_pos_cpt = main_stat_pos;
}
#ifdef HOST
@@ -235,7 +243,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('w', 2):
/* Set auxiliary pwm.
* - w: pwm. */
- //pos_reset (); // TODO
+ pos_reset (&pos_aux0);
state_aux0.mode = MODE_PWM;
pwm_aux0 = v8_to_v16 (args[0], args[1]);
UTILS_BOUND (pwm_aux0, -PWM_MAX, PWM_MAX);
@@ -248,6 +256,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
pos_theta.cons += v8_to_v16 (args[0], args[1]);
pos_alpha.cons += v8_to_v16 (args[2], args[3]);
break;
+ case c ('c', 2):
+ /* Add to auxiliary position consign.
+ * - w: consign offset. */
+ state_aux0.mode = MODE_POS;
+ pos_aux0.cons += v8_to_v16 (args[0], args[1]);
+ break;
case c ('s', 0):
/* Stop (set zero speed). */
state_main.mode = MODE_SPEED;
@@ -264,6 +278,13 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
speed_theta.cons = args[0] << 8;
speed_alpha.cons = args[1] << 8;
break;
+ case c ('s', 1):
+ /* Set auxiliary speed.
+ * - b: speed. */
+ state_aux0.mode = MODE_SPEED;
+ speed_aux0.use_pos = 0;
+ speed_aux0.cons = args[0] << 8;
+ break;
case c ('s', 9):
/* Set speed controlled position consign.
* - d: theta consign offset.
@@ -279,6 +300,18 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
speed_alpha.pos_cons += v8_to_v32 (args[4], args[5], args[6], args[7]);
state_start (&state_main, args[8]);
break;
+ case c ('s', 5):
+ /* Set auxiliary speed controlled position consign.
+ * - d: consign offset.
+ * - b: sequence number. */
+ if (args[4] == state_aux0.sequence)
+ break;
+ state_aux0.mode = MODE_SPEED;
+ speed_aux0.use_pos = 1;
+ speed_aux0.pos_cons = pos_aux0.cons;
+ speed_aux0.pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]);
+ state_start (&state_aux0, args[4]);
+ break;
case c ('f', 1):
/* Go to the wall.
* - b: sequence number. */
@@ -289,10 +322,18 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
traj_mode = 10;
state_start (&state_main, args[0]);
break;
+ case c ('a', 2):
+ /* Set both acknoledge.
+ * - b: main ack sequence number.
+ * - b: auxiliary ack sequence number. */
+ if (state_aux0.blocked && args[1] == state_aux0.finished)
+ pos_reset (&pos_aux0);
+ state_acknowledge (&state_aux0, args[1]);
+ /* no break; */
case c ('a', 1):
- /* Set acknoledge.
- * - b: ack sequence number. */
- if (state_main.blocked)
+ /* Set main acknoledge.
+ * - b: main ack sequence number. */
+ if (state_main.blocked && args[0] == state_main.finished)
{
pos_reset (&pos_theta);
pos_reset (&pos_alpha);
@@ -313,6 +354,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Position stats. */
main_stat_postrack_cpt = main_stat_postrack = args[0];
break;
+ case c ('Z', 1):
+ /* Auxiliary position stats. */
+ main_stat_aux_pos_cpt = main_stat_aux_pos = args[0];
+ break;
case c ('S', 1):
/* Motor speed control stats. */
main_stat_speed_cpt = main_stat_speed = args[0];
@@ -372,14 +417,19 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
postrack_set_footing (v8_to_v16 (args[1], args[2]));
break;
case c ('a', 5):
- /* Set acceleration.
+ /* Set main acceleration.
* - w: theta.
* - w: alpha. */
speed_theta.acc = v8_to_v16 (args[1], args[2]);
speed_alpha.acc = v8_to_v16 (args[3], args[4]);
break;
+ case c ('a', 3):
+ /* Set auxiliary acceleration.
+ * - w: acceleration. */
+ speed_aux0.acc = v8_to_v16 (args[1], args[2]);
+ break;
case c ('s', 5):
- /* Set maximum and slow speed.
+ /* Set main maximum and slow speed.
* - b: theta max.
* - b: alpha max.
* - b: theta slow.
@@ -389,22 +439,29 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
speed_theta.slow = args[3];
speed_alpha.slow = args[4];
break;
+ case c ('s', 3):
+ /* Set auxiliary maximum and slow speed.
+ * - b: speed max.
+ * - b: speed slow. */
+ speed_aux0.max = args[1];
+ speed_aux0.slow = args[2];
+ break;
case c ('p', 3):
- pos_theta.kp = pos_alpha.kp = v8_to_v16 (args[1], args[2]);
+ pos_aux0.kp = v8_to_v16 (args[1], args[2]);
break;
case c ('p', 5):
pos_theta.kp = v8_to_v16 (args[1], args[2]);
pos_alpha.kp = v8_to_v16 (args[3], args[4]);
break;
case c ('i', 3):
- pos_theta.ki = pos_alpha.ki = v8_to_v16 (args[1], args[2]);
+ pos_aux0.ki = v8_to_v16 (args[1], args[2]);
break;
case c ('i', 5):
pos_theta.ki = v8_to_v16 (args[1], args[2]);
pos_alpha.ki = v8_to_v16 (args[3], args[4]);
break;
case c ('d', 3):
- pos_theta.kd = pos_alpha.kd = v8_to_v16 (args[1], args[2]);
+ pos_aux0.kd = v8_to_v16 (args[1], args[2]);
break;
case c ('d', 5):
pos_theta.kd = v8_to_v16 (args[1], args[2]);
@@ -442,6 +499,11 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
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_send1w ('a', speed_aux0.acc);
+ proto_send2b ('s', speed_aux0.max, speed_aux0.slow);
+ proto_send1w ('p', pos_aux0.kp);
+ proto_send1w ('i', pos_aux0.ki);
+ proto_send1w ('d', pos_aux0.kd);
proto_send1w ('E', pos_e_sat);
proto_send1w ('I', pos_int_sat);
proto_send1w ('b', pos_blocked);
diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c
index e241b0fb..091335e2 100644
--- a/digital/asserv/src/asserv/pos.c
+++ b/digital/asserv/src/asserv/pos.c
@@ -43,6 +43,9 @@
/** Theta/alpha control states. */
struct pos_t pos_theta, pos_alpha;
+/** Auxiliaries control states. */
+struct pos_t pos_aux0;
+
/** Error saturation. */
int32_t pos_e_sat = 1023;
/** Integral saturation. */
@@ -88,32 +91,58 @@ pos_compute_pid (int32_t e, struct pos_t *pos)
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. */
- diff_theta = pos_theta.cons - pos_theta.cur;
- diff_alpha = pos_alpha.cons - pos_alpha.cur;
- if (state_main.blocked
- || diff_theta < -pos_blocked || pos_blocked < diff_theta
- || diff_alpha < -pos_blocked || pos_blocked < diff_alpha)
+ if (state_main.mode >= MODE_POS)
{
- /* Blocked. */
- pwm_left = 0;
- pwm_right = 0;
- state_blocked (&state_main);
+ 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. */
+ diff_theta = pos_theta.cons - pos_theta.cur;
+ diff_alpha = pos_alpha.cons - pos_alpha.cur;
+ if (state_main.blocked
+ || diff_theta < -pos_blocked || pos_blocked < diff_theta
+ || diff_alpha < -pos_blocked || pos_blocked < diff_alpha)
+ {
+ /* Blocked. */
+ pwm_left = 0;
+ pwm_right = 0;
+ state_blocked (&state_main);
+ }
+ else
+ {
+ pid_theta = pos_compute_pid (diff_theta, &pos_theta);
+ pid_alpha = pos_compute_pid (diff_alpha, &pos_alpha);
+ /* 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);
+ }
}
- else
+ if (state_aux0.mode >= MODE_POS)
{
- pid_theta = pos_compute_pid (diff_theta, &pos_theta);
- pid_alpha = pos_compute_pid (diff_alpha, &pos_alpha);
- /* 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);
+ int16_t pid;
+ int32_t diff;
+ /* Update current shaft position. */
+ pos_aux0.cur += counter_aux0_diff;
+ /* Compute PID. */
+ diff = pos_aux0.cons - pos_aux0.cur;
+ if (state_aux0.blocked
+ || diff < -pos_blocked || pos_blocked < diff)
+ {
+ /* Blocked. */
+ pwm_aux0 = 0;
+ state_blocked (&state_aux0);
+ }
+ else
+ {
+ pid = pos_compute_pid (diff, &pos_aux0);
+ /* Update PWM. */
+ pwm_aux0 = pid;
+ UTILS_BOUND (pwm_aux0, -PWM_MAX, PWM_MAX);
+ }
}
}
diff --git a/digital/asserv/src/asserv/pos.h b/digital/asserv/src/asserv/pos.h
index 1859d178..d28db334 100644
--- a/digital/asserv/src/asserv/pos.h
+++ b/digital/asserv/src/asserv/pos.h
@@ -41,6 +41,7 @@ struct pos_t
};
extern struct pos_t pos_theta, pos_alpha;
+extern struct pos_t pos_aux0;
extern int32_t pos_e_sat;
extern int32_t pos_int_sat;
diff --git a/digital/asserv/src/asserv/speed.c b/digital/asserv/src/asserv/speed.c
index b50a6b52..679ce416 100644
--- a/digital/asserv/src/asserv/speed.c
+++ b/digital/asserv/src/asserv/speed.c
@@ -40,6 +40,9 @@
/** Theta/alpha speed control states. */
struct speed_t speed_theta, speed_alpha;
+/** Auxiliaries speed control states. */
+struct speed_t speed_aux0;
+
/** Update shaft position consign according to a speed consign. */
static void
speed_update_by_speed (struct speed_t *speed)
@@ -101,15 +104,26 @@ speed_update_by (struct speed_t *speed, struct pos_t *pos)
void
speed_update (void)
{
- /* Update speed. */
- speed_update_by (&speed_theta, &pos_theta);
- speed_update_by (&speed_alpha, &pos_alpha);
- /* Check for completion. */
- if ((speed_theta.use_pos || speed_alpha.use_pos)
- && (!speed_theta.use_pos || speed_theta.cur == 0)
- && (!speed_alpha.use_pos || speed_alpha.cur == 0))
+ if (state_main.mode >= MODE_SPEED)
+ {
+ /* Update speed. */
+ speed_update_by (&speed_theta, &pos_theta);
+ speed_update_by (&speed_alpha, &pos_alpha);
+ /* Check for completion. */
+ if ((speed_theta.use_pos || speed_alpha.use_pos)
+ && (!speed_theta.use_pos || speed_theta.cur == 0)
+ && (!speed_alpha.use_pos || speed_alpha.cur == 0))
+ {
+ state_finish (&state_main);
+ }
+ }
+ if (state_aux0.mode >= MODE_SPEED)
{
- state_finish (&state_main);
+ /* Update speed. */
+ speed_update_by (&speed_aux0, &pos_aux0);
+ /* Check for completion. */
+ if (speed_aux0.use_pos && speed_aux0.cur == 0)
+ state_finish (&state_aux0);
}
}
diff --git a/digital/asserv/src/asserv/speed.h b/digital/asserv/src/asserv/speed.h
index f62f267f..d5df91a2 100644
--- a/digital/asserv/src/asserv/speed.h
+++ b/digital/asserv/src/asserv/speed.h
@@ -45,6 +45,7 @@ struct speed_t
};
extern struct speed_t speed_theta, speed_alpha;
+extern struct speed_t speed_aux0;
void
speed_update (void);