From 172edd2d760c92c6f12d284586de9d3a2fac62b6 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 5 Apr 2008 01:28:09 +0200 Subject: * digital/asserv/src/asserv: - implemented TWI proto commands and report. - refuse command if using the same sequence number. --- digital/asserv/src/asserv/twi_proto.c | 105 ++++++++++++++++++++++------------ 1 file changed, 69 insertions(+), 36 deletions(-) (limited to 'digital') diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index 04ef34ae..5b398996 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -28,18 +28,23 @@ #include "modules/utils/utils.h" #include "modules/utils/byte.h" #include "modules/twi/twi.h" +#include "modules/math/fixed/fixed.h" #include "io.h" #include "misc.h" +#include "state.h" + +#include "pwm.h" +#include "pos.h" +#include "speed.h" +#include "postrack.h" +#include "traj.h" + +#define M_PI 3.14159265358979323846 /* pi */ struct twi_proto_t { u8 seq; - /* Temporary test values. */ - u32 tmp_l; - u32 tmp_x; - u8 tmp_l_active; - u8 tmp_f; }; struct twi_proto_t twi_proto; @@ -66,34 +71,23 @@ twi_proto_update (void) /* Handle incoming command. */ while (twi_sl_poll (buf, sizeof (buf))) twi_proto_callback (buf, sizeof (buf)); - /* Temporary test code. */ - if (twi_proto.tmp_l_active) - { - if (twi_proto.tmp_l) - { - twi_proto.tmp_x += 256; - twi_proto.tmp_l -= 256; - } - else - { - LED1 (0); - twi_proto.tmp_f = 1; - } - } /* Update status. */ u8 status[12]; - status[0] = twi_proto.tmp_f; + status[0] = (state_aux0.blocked << 3) + | (state_aux0.finished << 2) + | (state_main.blocked << 1) + | (state_main.finished << 0); status[1] = twi_proto.seq; - status[2] = v32_to_v8 (twi_proto.tmp_x, 3); - status[3] = v32_to_v8 (twi_proto.tmp_x, 2); - status[4] = v32_to_v8 (twi_proto.tmp_x, 1); - status[5] = v32_to_v8 (0, 3); - status[6] = v32_to_v8 (0, 2); - status[7] = v32_to_v8 (0, 1); - status[8] = v32_to_v8 (0, 2); - status[9] = v32_to_v8 (0, 1); - status[10] = v16_to_v8 (0, 1); - status[11] = v16_to_v8 (0, 0); + status[2] = v32_to_v8 (postrack_x, 3); + status[3] = v32_to_v8 (postrack_x, 2); + status[4] = v32_to_v8 (postrack_x, 1); + status[5] = v32_to_v8 (postrack_y, 3); + status[6] = v32_to_v8 (postrack_y, 2); + status[7] = v32_to_v8 (postrack_y, 1); + status[8] = v32_to_v8 (postrack_a, 2); + status[9] = v32_to_v8 (postrack_a, 1); + status[10] = v16_to_v8 (pos_aux0.cons, 1); + status[11] = v16_to_v8 (pos_aux0.cons, 0); twi_sl_update (status, sizeof (status)); } @@ -101,6 +95,8 @@ twi_proto_update (void) static void twi_proto_callback (u8 *buf, u8 size) { + if (buf[0] == twi_proto.seq) + return; #define c(cmd, size) (cmd) switch (c (buf[1], 0)) { @@ -110,21 +106,44 @@ twi_proto_callback (u8 *buf, u8 size) break; case c ('w', 0): /* Set zero pwm. */ + pos_reset (&pos_theta); + pos_reset (&pos_alpha); + state_main.mode = MODE_PWM; + pwm_left = 0; + pwm_right = 0; break; case c ('s', 0): /* Stop (set zero speed). */ + state_main.mode = MODE_SPEED; + speed_theta.use_pos = speed_alpha.use_pos = 0; + speed_theta.cons = 0; + speed_alpha.cons = 0; break; case c ('l', 3): /* Set linear speed controlled position consign. * - 3b: theta consign offset. */ - twi_proto.tmp_l = v8_to_v32 (buf[2], buf[3], buf[4], 0); - twi_proto.tmp_l_active = 1; - LED1 (1); - twi_proto.tmp_f = 0; + state_main.mode = MODE_SPEED; + speed_theta.use_pos = speed_alpha.use_pos = 1; + speed_theta.pos_cons = pos_theta.cons; + speed_theta.pos_cons += v8_to_v32 (0, buf[2], buf[3], buf[4]); + speed_alpha.pos_cons = pos_alpha.cons; + state_start (&state_main, 0); break; case c ('a', 2): - /* Set linear speed controlled position consign. - * - w: theta consign offset. */ + /* Set angular speed controlled position consign. + * - w: angle offset. */ + { + int32_t a = ((int32_t) (int16_t) v8_to_v16 (buf[2], buf[3])) << 8; + a = fixed_mul_f824 (a, 2 * M_PI * (1L << 24)); + uint32_t f = postrack_footing; + int32_t arc = fixed_mul_f824 (f, a); + state_main.mode = MODE_SPEED; + speed_theta.use_pos = speed_alpha.use_pos = 1; + speed_theta.pos_cons = pos_theta.cons; + speed_alpha.pos_cons = pos_alpha.cons; + speed_alpha.pos_cons += arc; + state_start (&state_main, 0); + } break; case c ('f', 0): /* Go to the wall. */ @@ -136,6 +155,13 @@ twi_proto_callback (u8 *buf, u8 size) /* Move the arm. * - w: new position. * - b: speed. */ + state_aux0.mode = MODE_SPEED; + speed_aux0.use_pos = 1; + speed_aux0.pos_cons = pos_aux0.cons; + speed_aux0.pos_cons += v8_to_v32 (0, 0, buf[2], buf[3]); + speed_aux0.max = buf[4]; + speed_aux0.slow = buf[4]; + state_start (&state_aux0, 0); break; case c ('p', x): /* Set parameters. */ @@ -165,6 +191,7 @@ twi_proto_params (u8 *buf, u8 size) * - 3b: X position. */ if (size < 3) return 1; + postrack_x = v8_to_v32 (buf[0], buf[1], buf[2], 0); eat = 3; break; case 'Y': @@ -172,6 +199,7 @@ twi_proto_params (u8 *buf, u8 size) * - 3b: Y position. */ if (size < 3) return 1; + postrack_y = v8_to_v32 (buf[0], buf[1], buf[2], 0); eat = 3; break; case 'A': @@ -179,6 +207,7 @@ twi_proto_params (u8 *buf, u8 size) * - w: angle. */ if (size < 2) return 1; + postrack_a = v8_to_v32 (0, buf[0], buf[1], 0); eat = 2; break; case 's': @@ -189,6 +218,10 @@ twi_proto_params (u8 *buf, u8 size) * - b: alpha slow. */ if (size < 4) return 1; + speed_theta.max = buf[0]; + speed_alpha.max = buf[1]; + speed_theta.slow = buf[2]; + speed_alpha.slow = buf[3]; eat = 4; break; default: -- cgit v1.2.3