summaryrefslogtreecommitdiff
path: root/digital/mimot/src/dirty/main.c
diff options
context:
space:
mode:
authorNicolas Schodet2011-04-19 21:26:15 +0200
committerNicolas Schodet2011-04-19 21:26:15 +0200
commit4cd9c4458eaa49c269171145f4bc2a184948eae1 (patch)
tree780cdc9493bfcfd2907a61cce62ff8a6f64a0c45 /digital/mimot/src/dirty/main.c
parente2594ccdadae65d7e7473e51fb8bfbbd49c04a24 (diff)
digital/mimot: switch to differential drive
Diffstat (limited to 'digital/mimot/src/dirty/main.c')
-rw-r--r--digital/mimot/src/dirty/main.c358
1 files changed, 231 insertions, 127 deletions
diff --git a/digital/mimot/src/dirty/main.c b/digital/mimot/src/dirty/main.c
index 5dcfd8c7..a04c53b6 100644
--- a/digital/mimot/src/dirty/main.c
+++ b/digital/mimot/src/dirty/main.c
@@ -37,7 +37,8 @@
#include "pwm.h"
#include "pos.h"
#include "speed.h"
-#include "aux.h"
+#include "postrack.h"
+#include "traj.h"
#include "twi_proto.h"
#include "eeprom.h"
@@ -54,8 +55,8 @@ uint8_t main_sequence_ack = 4, main_sequence_ack_cpt;
/** Report of counters. */
uint8_t main_stat_counter, main_stat_counter_cpt;
-/** Report of auxiliary position. */
-uint8_t main_stat_aux_pos, main_stat_aux_pos_cpt;
+/** Report of position. */
+uint8_t main_stat_postrack, main_stat_postrack_cpt;
/** Statistics about speed control. */
uint8_t main_stat_speed, main_stat_speed_cpt;
@@ -72,6 +73,11 @@ uint8_t main_stat_timer, main_stat_timer_cpt;
/** Print input port. */
uint8_t main_print_pin, main_print_pin_cpt;
+#ifdef HOST
+/** Simulation data. */
+uint8_t main_simu, main_simu_cpt;
+#endif /* HOST */
+
/** Record timer value at different stage of computing. Used for performance
* analisys. */
uint8_t main_timer[6];
@@ -93,8 +99,9 @@ main (int argc, char **argv)
counter_init ();
uart0_init ();
twi_proto_init ();
+ postrack_init ();
speed_init ();
- aux_init ();
+ traj_init ();
eeprom_read_params ();
proto_send0 ('z');
sei ();
@@ -119,46 +126,54 @@ main_loop (void)
pwm_update ();
main_timer[2] = timer_read ();
/* Compute absolute position. */
- aux_pos_update ();
+ postrack_update ();
/* Compute trajectory. */
- aux_traj_update ();
+ if (state_main.mode >= MODE_TRAJ)
+ traj_update ();
/* Speed control. */
speed_update ();
main_timer[3] = timer_read ();
/* Stats. */
if (main_sequence_ack
- && (state_aux[0].sequence_ack != state_aux[0].sequence_finish
- || state_aux[1].sequence_ack != state_aux[1].sequence_finish)
+ && state_main.sequence_ack != state_main.sequence_finish
&& !--main_sequence_ack_cpt)
{
- proto_send2b ('A', state_aux[0].sequence_finish,
- state_aux[1].sequence_finish);
+ proto_send1b ('A', state_main.sequence_finish);
main_sequence_ack_cpt = main_sequence_ack;
}
if (main_stat_counter && !--main_stat_counter_cpt)
{
- proto_send2w ('C', counter_aux[0], counter_aux[1]);
+ proto_send2w ('C', counter_left, counter_right);
main_stat_counter_cpt = main_stat_counter;
}
- if (main_stat_aux_pos && !--main_stat_aux_pos_cpt)
+ if (main_stat_postrack && !--main_stat_postrack_cpt)
{
- proto_send2w ('Y', aux[0].pos, aux[1].pos);
- main_stat_aux_pos_cpt = main_stat_aux_pos;
+ proto_send3d ('X', postrack_x, postrack_y, postrack_a);
+ main_stat_postrack_cpt = main_stat_postrack;
}
if (main_stat_speed && !--main_stat_speed_cpt)
{
- proto_send2b ('S', speed_aux[0].cur >> 8, speed_aux[1].cur >> 8);
+ proto_send2b ('S', speed_theta.cur >> 8, speed_alpha.cur >> 8);
main_stat_speed_cpt = main_stat_speed;
}
if (main_stat_pos && !--main_stat_pos_cpt)
{
- proto_send4w ('P', pos_aux[0].e_old, pos_aux[0].i,
- pos_aux[1].e_old, pos_aux[1].i);
+ proto_send4w ('P', pos_theta.e_old, pos_theta.i,
+ pos_alpha.e_old, pos_alpha.i);
main_stat_pos_cpt = main_stat_pos;
}
+#ifdef HOST
+ if (main_simu && !--main_simu_cpt)
+ {
+ proto_send3w ('H', (uint16_t) simu_pos_x,
+ (uint16_t) simu_pos_y,
+ (uint16_t) (simu_pos_a * 1024));
+ main_simu_cpt = main_simu;
+ }
+#endif /* HOST */
if (main_stat_pwm && !--main_stat_pwm_cpt)
{
- proto_send2w ('W', pwm_aux[0].cur, pwm_aux[1].cur);
+ proto_send2w ('W', pwm_left.cur, pwm_right.cur);
main_stat_pwm_cpt = main_stat_pwm;
}
if (main_stat_timer && !--main_stat_timer_cpt)
@@ -184,19 +199,8 @@ void
proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
{
/* Many commands use the first argument as a selector. */
- struct aux_t *auxp = 0;
- struct pwm_t *pwm = 0;
struct pos_t *pos = 0;
struct speed_t *speed = 0;
- struct state_t *state = 0;
- if (args[0] < AC_ASSERV_AUX_NB)
- {
- auxp = &aux[args[0]];
- pwm = &pwm_aux[args[0]];
- pos = &pos_aux[args[0]];
- speed = &speed_aux[args[0]];
- state = &state_aux[args[0]];
- }
/* Decode command. */
#define c(cmd, size) (cmd << 8 | size)
switch (c (cmd, size))
@@ -207,102 +211,132 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
/* Commands. */
case c ('w', 0):
- /* Set zero auxiliary pwm. */
- pos_reset (&pos_aux[0]);
- pos_reset (&pos_aux[1]);
- state_aux[0].mode = MODE_PWM;
- state_aux[1].mode = MODE_PWM;
- pwm_set (&pwm_aux[0], 0);
- pwm_set (&pwm_aux[1], 0);
+ /* Set zero pwm. */
+ pos_reset (&pos_theta);
+ pos_reset (&pos_alpha);
+ state_main.mode = MODE_PWM;
+ pwm_set (&pwm_left, 0);
+ pwm_set (&pwm_right, 0);
break;
- case c ('w', 3):
- /* Set auxiliary pwm.
- * - b: aux index.
- * - w: pwm. */
- if (!auxp) { proto_send0 ('?'); return; }
- pos_reset (pos);
- state->mode = MODE_PWM;
- pwm_set (pwm, v8_to_v16 (args[1], args[2]));
+ case c ('w', 4):
+ /* Set pwm.
+ * - w: left pwm.
+ * - w: right pwm. */
+ pos_reset (&pos_theta);
+ pos_reset (&pos_alpha);
+ state_main.mode = MODE_PWM;
+ pwm_set (&pwm_left, v8_to_v16 (args[0], args[1]));
+ pwm_set (&pwm_right, v8_to_v16 (args[2], args[3]));
break;
- case c ('c', 3):
- /* Add to auxiliary position consign.
- * - b: aux index.
- * - w: consign offset. */
- if (!auxp) { proto_send0 ('?'); return; }
- state->mode = MODE_POS;
- state->variant = 0;
- pos->cons += v8_to_v16 (args[1], args[2]);
+ case c ('c', 4):
+ /* Add to position consign.
+ * - w: theta consign offset.
+ * - w: alpha consign offset. */
+ state_main.mode = MODE_POS;
+ state_main.variant = 0;
+ pos_theta.cons += v8_to_v16 (args[0], args[1]);
+ pos_alpha.cons += v8_to_v16 (args[2], args[3]);
break;
- case c ('s', 1):
- /* Set auxiliary zero speed.
- * - b: aux index. */
- if (!auxp) { proto_send0 ('?'); return; }
- state->mode = MODE_SPEED;
- state->variant = 0;
- speed->use_pos = 0;
- speed->cons = 0;
+ case c ('s', 0):
+ /* Stop (set zero speed). */
+ state_main.mode = MODE_SPEED;
+ state_main.variant = 0;
+ speed_theta.use_pos = speed_alpha.use_pos = 0;
+ speed_theta.cons = 0;
+ speed_alpha.cons = 0;
break;
case c ('s', 2):
- /* Set auxiliary speed.
- * - b: aux index.
- * - b: speed. */
- if (!auxp) { proto_send0 ('?'); return; }
- state->mode = MODE_SPEED;
- state->variant = 0;
- speed->use_pos = 0;
- speed->cons = args[1] << 8;
+ /* Set speed.
+ * - b: theta speed.
+ * - b: alpha speed. */
+ state_main.mode = MODE_SPEED;
+ state_main.variant = 0;
+ speed_theta.use_pos = speed_alpha.use_pos = 0;
+ speed_theta.cons = args[0] << 8;
+ speed_alpha.cons = args[1] << 8;
+ break;
+ case c ('s', 9):
+ /* Set speed controlled position consign.
+ * - d: theta consign offset.
+ * - d: alpha consign offset.
+ * - b: sequence number. */
+ if (args[8] == state_main.sequence)
+ break;
+ speed_theta.use_pos = speed_alpha.use_pos = 1;
+ speed_theta.pos_cons = pos_theta.cons;
+ speed_theta.pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]);
+ speed_alpha.pos_cons = pos_alpha.cons;
+ speed_alpha.pos_cons += v8_to_v32 (args[4], args[5], args[6], args[7]);
+ state_start (&state_main, MODE_SPEED, args[8]);
break;
- case c ('s', 6):
- /* Set auxiliary speed controlled position consign.
- * - b: aux index.
+ case c ('l', 5):
+ /* Set linear speed controlled position consign.
* - d: consign offset.
* - b: sequence number. */
- if (!auxp) { proto_send0 ('?'); return; }
- if (args[5] == state->sequence)
+ if (args[4] == state_main.sequence)
break;
- speed->use_pos = 1;
- speed->pos_cons = pos->cons;
- speed->pos_cons += v8_to_v32 (args[1], args[2], args[3], args[4]);
- state_start (state, MODE_SPEED, args[5]);
+ speed_theta.use_pos = speed_alpha.use_pos = 1;
+ speed_theta.pos_cons = pos_theta.cons;
+ speed_theta.pos_cons += v8_to_v32 (args[0], args[1], args[2], args[3]);
+ speed_alpha.pos_cons = pos_alpha.cons;
+ state_start (&state_main, MODE_SPEED, args[4]);
break;
- case c ('y', 4):
- /* Auxiliary go to position.
- * - b: aux index.
- * - w: pos, i16.
+ case c ('a', 5):
+ /* Set angular speed controlled position consign.
+ * - d: angle offset.
* - b: sequence number. */
- if (!auxp) { proto_send0 ('?'); return; }
- if (args[3] == state->sequence)
+ if (args[4] == state_main.sequence)
break;
- aux_traj_goto_start (auxp, v8_to_v16 (args[1], args[2]), args[3]);
+ traj_angle_offset_start (v8_to_v32 (args[0], args[1], args[2],
+ args[3]), args[4]);
break;
- case c ('y', 5):
- /* Auxiliary clamp.
- * - b: aux index.
- * - b: speed.
- * - w: clamping PWM.
+ case c ('f', 2):
+ /* Go to the wall.
+ * - b: 0: forward, 1: backward.
* - b: sequence number. */
- if (!auxp) { proto_send0 ('?'); return; }
- if (args[4] == state->sequence)
+ if (args[1] == state_main.sequence)
break;
- aux_traj_clamp_start (auxp, args[1], v8_to_v16 (args[2], args[3]),
- args[4]);
+ traj_ftw_start (args[0], args[1]);
break;
- case c ('y', 3):
- /* Auxiliary find zero.
- * - b: aux index.
- * - b: speed.
+ case c ('x', 10):
+ /* Go to position.
+ * - d: x, f24.8.
+ * - d: y, f24.8.
+ * - b: backward (see traj.h).
* - b: sequence number. */
- if (!auxp) { proto_send0 ('?'); return; }
- if (args[2] == state->sequence)
+ if (args[9] == state_main.sequence)
break;
- aux_traj_find_limit_start (auxp, args[1], args[2]);
+ traj_goto_start (v8_to_v32 (args[0], args[1], args[2], args[3]),
+ v8_to_v32 (args[4], args[5], args[6], args[7]),
+ args[8], args[9]);
break;
- case c ('a', 2):
- /* Set all acknoledge.
- * - b: first auxiliary ack sequence number.
- * - b: second auxiliary ack sequence number. */
- state_acknowledge (&state_aux[0], args[0]);
- state_acknowledge (&state_aux[1], args[1]);
+ case c ('x', 3):
+ /* Go to angle.
+ * - d: a, f0.16.
+ * - b: sequence number. */
+ if (args[2] == state_main.sequence)
+ break;
+ traj_goto_angle_start (v8_to_v32 (0, args[0], args[1], 0),
+ args[2]);
+ break;
+ case c ('x', 12):
+ /* Go to position, then angle.
+ * - d: x, f24.8.
+ * - d: y, f24.8.
+ * - w: a, f0.16.
+ * - b: backward (see traj.h).
+ * - b: sequence number. */
+ if (args[11] == state_main.sequence)
+ break;
+ traj_goto_xya_start (v8_to_v32 (args[0], args[1], args[2], args[3]),
+ v8_to_v32 (args[4], args[5], args[6], args[7]),
+ v8_to_v32 (0, args[8], args[9], 0),
+ args[10], args[11]);
+ break;
+ case c ('a', 1):
+ /* Set main acknoledge.
+ * - b: main ack sequence number. */
+ state_acknowledge (&state_main, args[0]);
break;
/* Stats.
* - b: interval between stats. */
@@ -314,16 +348,16 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Counter stats. */
main_stat_counter_cpt = main_stat_counter = args[0];
break;
- case c ('Y', 1):
- /* Auxiliary position stats. */
- main_stat_aux_pos_cpt = main_stat_aux_pos = args[0];
+ case c ('X', 1):
+ /* Position stats. */
+ main_stat_postrack_cpt = main_stat_postrack = args[0];
break;
case c ('S', 1):
/* Motor speed control stats. */
main_stat_speed_cpt = main_stat_speed = args[0];
break;
case c ('P', 1):
- /* Auxiliary motor position control stats. */
+ /* Motor position control stats. */
main_stat_pos_cpt = main_stat_pos = args[0];
break;
case c ('W', 1):
@@ -338,6 +372,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Input port stats. */
main_print_pin_cpt = main_print_pin = args[0];
break;
+#ifdef HOST
+ case c ('H', 1):
+ /* Simulation data. */
+ main_simu_cpt = main_simu = args[0];
+ break;
+#endif /* HOST */
default:
/* Params. */
if (cmd == 'p')
@@ -345,10 +385,13 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Many commands use the first argument as a selector. */
switch (args[1])
{
- case 0:
- case 1:
- pos = &pos_aux[args[1]];
- speed = &speed_aux[args[1]];
+ case 't':
+ pos = &pos_theta;
+ speed = &speed_theta;
+ break;
+ case 'a':
+ pos = &pos_alpha;
+ speed = &speed_alpha;
break;
default:
pos = 0;
@@ -357,6 +400,38 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
}
switch (c (args[0], size))
{
+ case c ('X', 1):
+ /* Reset position. */
+ postrack_x = 0;
+ postrack_y = 0;
+ postrack_a = 0;
+ break;
+ case c ('X', 5):
+ /* Set current x position.
+ * - d: x position. */
+ postrack_x = v8_to_v32 (args[1], args[2], args[3], args[4]);
+ break;
+ case c ('Y', 5):
+ /* Set current y position.
+ * - d: y position. */
+ postrack_y = v8_to_v32 (args[1], args[2], args[3], args[4]);
+ break;
+ case c ('A', 5):
+ /* Set current angle.
+ * - d: angle. */
+ postrack_a = v8_to_v32 (args[1], args[2], args[3], args[4]);
+ break;
+ case c ('c', 5):
+ /* Set right counter correction factor.
+ * - d: factor (f8.24). */
+ counter_right_correction = v8_to_v32 (args[1], args[2],
+ args[3], args[4]);
+ break;
+ case c ('f', 3):
+ /* Set footing.
+ * - w: footing. */
+ postrack_set_footing (v8_to_v16 (args[1], args[2]));
+ break;
case c ('a', 4):
/* Set acceleration.
* - b: index.
@@ -414,9 +489,16 @@ 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 ('e', 5):
+ traj_eps = v8_to_v16 (args[1], args[2]);
+ traj_aeps = v8_to_v16 (args[3], args[4]);
+ break;
+ case c ('l', 3):
+ traj_set_angle_limit (v8_to_v16 (args[1], args[2]));
+ break;
case c ('w', 2):
/* Set PWM direction.
- * - b: bits: 0000[aux1][aux0][right][left]. */
+ * - b: bits: 000000[right][left]. */
pwm_set_reverse (args[1]);
break;
case c ('E', 2):
@@ -430,21 +512,25 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('P', 1):
/* Print current settings. */
proto_send1b ('E', EEPROM_KEY);
- 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_send1d ('c', counter_right_correction);
+ proto_send1w ('f', postrack_footing);
+ 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_send1w ('E', pos_e_sat);
proto_send1w ('I', pos_i_sat);
proto_send1w ('D', pos_d_sat);
+ proto_send2w ('e', traj_eps, traj_aeps);
+ proto_send1w ('l', traj_angle_limit);
proto_send1b ('w', pwm_reverse);
break;
default:
@@ -452,6 +538,24 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
return;
}
}
+#ifdef HOST
+ else if (cmd == 'h')
+ {
+ switch (c (args[0], size))
+ {
+ case c ('X', 7):
+ /* Set simulated position.
+ * - w: x position.
+ * - w: y position.
+ * - w: angle (rad) * 1024. */
+ simu_pos_x = (double) v8_to_v16 (args[1], args[2]);
+ simu_pos_y = (double) v8_to_v16 (args[3], args[4]);
+ simu_pos_a = (double) (int16_t) v8_to_v16 (args[5], args[6])
+ / 1024;
+ break;
+ }
+ }
+#endif /* HOST */
else
{
proto_send0 ('?');