summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2008-04-05 01:28:09 +0200
committerNicolas Schodet2008-04-05 01:28:09 +0200
commit172edd2d760c92c6f12d284586de9d3a2fac62b6 (patch)
tree4a2ec97e29e370e482142c41ca4d980de36daf75 /digital
parentcb572654a76e0c83697ce0a838ddc1d3f536325e (diff)
* digital/asserv/src/asserv:
- implemented TWI proto commands and report. - refuse command if using the same sequence number.
Diffstat (limited to 'digital')
-rw-r--r--digital/asserv/src/asserv/twi_proto.c105
1 files changed, 69 insertions, 36 deletions
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: