summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2008-04-06 23:21:52 +0200
committerNicolas Schodet2008-04-06 23:21:52 +0200
commitfcee99a4ff34939a6dbde8238e660f5e70f8d8cd (patch)
treecd9cbc2af97d0a648f6ff9bb029c7badb9848bf5
parent32b0ab53c2c79d0a07bc094cec1ce06c0b4b3d6b (diff)
* digital/asserv/src/asserv:
- added go to the wall.
-rw-r--r--digital/asserv/src/asserv/main.c5
-rw-r--r--digital/asserv/src/asserv/traj.c30
-rw-r--r--digital/asserv/src/asserv/traj.h3
-rw-r--r--digital/asserv/src/asserv/twi_proto.c1
4 files changed, 29 insertions, 10 deletions
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 45ae321e..25b0f024 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -326,10 +326,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - b: sequence number. */
if (args[0] == state_main.sequence)
break;
- state_main.mode = MODE_TRAJ;
- speed_theta.use_pos = speed_alpha.use_pos = 0;
- traj_mode = 10;
- state_start (&state_main, args[0]);
+ traj_ftw_start (args[0]);
break;
case c ('a', 2):
/* Set both acknoledge.
diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c
index 3b8bc7d3..d33c76c9 100644
--- a/digital/asserv/src/asserv/traj.c
+++ b/digital/asserv/src/asserv/traj.c
@@ -41,9 +41,16 @@
/* This file provides high level trajectories commands. */
-/** Traj mode:
- * 10, 11: go to the wall.
- */
+/** Traj mode enum. */
+enum
+{
+ /* Go to the wall. */
+ TRAJ_FTW,
+ /* Everything done. */
+ TRAJ_DONE,
+};
+
+/** Traj mode. */
uint8_t traj_mode;
/** Angle offset. Directly handled to speed layer. */
@@ -68,6 +75,7 @@ traj_ftw (void)
int16_t speed;
speed = speed_theta.slow;
speed *= 256;
+ speed_theta.use_pos = speed_alpha.use_pos = 0;
if (PINC & _BV (0) && PINC & _BV (1))
{
speed_theta.cons = -speed;
@@ -90,19 +98,29 @@ traj_ftw (void)
speed_theta.cur = 0;
speed_alpha.cur = 0;
state_finish (&state_main);
- traj_mode = 11;
+ traj_mode = TRAJ_DONE;
}
}
+/** Start go to the wall mode. */
+void
+traj_ftw_start (uint8_t seq)
+{
+ state_main.mode = MODE_TRAJ;
+ traj_mode = TRAJ_FTW;
+ state_start (&state_main, seq);
+}
+
/* Compute new speed according the defined trajectory. */
void
traj_update (void)
{
switch (traj_mode)
{
- case 10:
+ case TRAJ_FTW:
traj_ftw ();
- case 11:
+ break;
+ case TRAJ_DONE:
break;
}
}
diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h
index 2eaf9a0e..283f2c49 100644
--- a/digital/asserv/src/asserv/traj.h
+++ b/digital/asserv/src/asserv/traj.h
@@ -33,4 +33,7 @@ traj_update (void);
void
traj_angle_offset_start (int32_t angle, uint8_t seq);
+void
+traj_ftw_start (uint8_t seq);
+
#endif /* traj_h */
diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c
index 9b3d8f36..b7cfd22f 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -133,6 +133,7 @@ twi_proto_callback (u8 *buf, u8 size)
break;
case c ('f', 0):
/* Go to the wall. */
+ traj_ftw_start (0);
break;
case c ('F', 0):
/* Go to the dispenser. */