summaryrefslogtreecommitdiff
path: root/digital/asserv/src
diff options
context:
space:
mode:
authorNicolas Schodet2008-04-21 00:30:41 +0200
committerNicolas Schodet2008-04-21 00:30:41 +0200
commit19e614348483e09ddb2acca5fcf905ec0c582625 (patch)
tree5cf25e494f2b0bf5cf3c693b1904b3d8f5651ead /digital/asserv/src
parent557fdd8e9bea25a1f5d81d93466dac2abfef6cd3 (diff)
* digital/asserv/src/asserv:
- added find zero auxiliary command.
Diffstat (limited to 'digital/asserv/src')
-rw-r--r--digital/asserv/src/asserv/aux.c78
-rw-r--r--digital/asserv/src/asserv/aux.h8
-rw-r--r--digital/asserv/src/asserv/init4
-rw-r--r--digital/asserv/src/asserv/main.c9
-rw-r--r--digital/asserv/src/asserv/simu.host.c6
-rw-r--r--digital/asserv/src/asserv/twi_proto.c6
6 files changed, 109 insertions, 2 deletions
diff --git a/digital/asserv/src/asserv/aux.c b/digital/asserv/src/asserv/aux.c
index 880496b4..562bcffd 100644
--- a/digital/asserv/src/asserv/aux.c
+++ b/digital/asserv/src/asserv/aux.c
@@ -25,15 +25,34 @@
#include "common.h"
#include "aux.h"
+#include "io.h"
+
#include "state.h"
#include "counter.h"
#include "pos.h"
#include "speed.h"
+#ifdef HOST
+# include "simu.host.h"
+#endif
+
/** Motor state. */
struct aux_t aux0;
+/** Trajectory modes. */
+enum
+{
+ /* Find zero mode, first start at full speed to detect a arm... */
+ AUX_TRAJ_FIND_ZERO_START,
+ /* ...then go on until it is not seen any more... */
+ AUX_TRAJ_FIND_ZERO_SLOW,
+ /* ...finally, go backward until it is seen. */
+ AUX_TRAJ_FIND_ZERO_BACK,
+ /* Everything done. */
+ AUX_TRAJ_DONE,
+};
+
/** Update positions. */
void
aux_pos_update (void)
@@ -52,3 +71,62 @@ aux_traj_goto_start (uint16_t pos, uint8_t seq)
state_start (&state_aux0, MODE_SPEED, seq);
}
+/** Find zero mode. */
+void
+aux_traj_find_zero (void)
+{
+ switch (aux0.traj_mode)
+ {
+ case AUX_TRAJ_FIND_ZERO_START:
+ if (!(PINC & _BV (3)))
+ {
+ speed_aux0.cons = speed_aux0.max << 8;
+ aux0.traj_mode = AUX_TRAJ_FIND_ZERO_SLOW;
+ }
+ break;
+ case AUX_TRAJ_FIND_ZERO_SLOW:
+ if (PINC & _BV (3))
+ {
+ speed_aux0.cons = -speed_aux0.slow << 8;
+ aux0.traj_mode = AUX_TRAJ_FIND_ZERO_BACK;
+ }
+ break;
+ case AUX_TRAJ_FIND_ZERO_BACK:
+ if (!(PINC & _BV (3)))
+ {
+ speed_aux0.cons = 0;
+ state_finish (&state_aux0);
+ aux0.pos = 0;
+ aux0.traj_mode = AUX_TRAJ_DONE;
+ }
+ break;
+ default:
+ assert (0);
+ }
+}
+
+/** Start find zero mode. */
+void
+aux_traj_find_zero_start (uint8_t seq)
+{
+ aux0.traj_mode = AUX_TRAJ_FIND_ZERO_START;
+ speed_aux0.use_pos = 0;
+ speed_aux0.cons = speed_aux0.max << 8;
+ state_start (&state_aux0, MODE_TRAJ, seq);
+}
+
+/** Update trajectories. */
+void
+aux_traj_update (void)
+{
+ switch (aux0.traj_mode)
+ {
+ case AUX_TRAJ_FIND_ZERO_START:
+ case AUX_TRAJ_FIND_ZERO_SLOW:
+ case AUX_TRAJ_FIND_ZERO_BACK:
+ aux_traj_find_zero ();
+ break;
+ case AUX_TRAJ_DONE:
+ break;
+ }
+}
diff --git a/digital/asserv/src/asserv/aux.h b/digital/asserv/src/asserv/aux.h
index 175933cc..3dc3c6ee 100644
--- a/digital/asserv/src/asserv/aux.h
+++ b/digital/asserv/src/asserv/aux.h
@@ -30,6 +30,8 @@ struct aux_t
{
/** Absolute position. */
int16_t pos;
+ /** Trajectory mode. */
+ uint8_t traj_mode;
};
extern struct aux_t aux0;
@@ -40,4 +42,10 @@ aux_pos_update (void);
void
aux_traj_goto_start (uint16_t pos, uint8_t seq);
+void
+aux_traj_find_zero_start (uint8_t seq);
+
+void
+aux_traj_update (void);
+
#endif /* aux_h */
diff --git a/digital/asserv/src/asserv/init b/digital/asserv/src/asserv/init
index 7937e666..8bfa283e 100644
--- a/digital/asserv/src/asserv/init
+++ b/digital/asserv/src/asserv/init
@@ -5,10 +5,10 @@
!p'E03ff
!p'D01ff
!p'a00800080
-!p's60406040
+!p's60602020
!p'p0100
!p'i0000
!p'd1000
!p'a0100
-!p's1010
+!p's1002
!Aff
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 8be5373f..47b2af2f 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -137,6 +137,8 @@ main_loop (void)
/* Compute trajectory. */
if (state_main.mode >= MODE_TRAJ)
traj_update ();
+ if (state_aux0.mode >= MODE_TRAJ)
+ aux_traj_update ();
/* Speed control. */
speed_update ();
main_timer[3] = timer_read ();
@@ -393,6 +395,13 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
aux_traj_goto_start (v8_to_v16 (args[0], args[1]), args[2]);
break;
+ case c ('y', 1):
+ /* Auxiliary find zero.
+ * - b: sequence number. */
+ if (args[0] == state_aux0.sequence)
+ break;
+ aux_traj_find_zero_start (args[0]);
+ break;
case c ('a', 2):
/* Set both acknoledge.
* - b: main ack sequence number.
diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c
index 8b499123..64fc38db 100644
--- a/digital/asserv/src/asserv/simu.host.c
+++ b/digital/asserv/src/asserv/simu.host.c
@@ -131,6 +131,7 @@ simu_pos_update (double dl, double dr, double footing)
static void
simu_sensor_update (void)
{
+ /** Micro-switch sensors. */
static const double sensors[][2] =
{
{ -70.0, 200.0 },
@@ -153,6 +154,11 @@ simu_sensor_update (void)
PINC |= bit;
bit <<= 1;
}
+ /** Top zero sensor. */
+ double aa = simu_aux0_model.th / simu_aux0_model.m.i_G * 3;
+ if (!(cos (aa) > 0 && fabs (sin (aa)) * 80.0 < 7.5))
+ PINC |= bit;
+ bit <<= 1;
}
/** Do a simulation step. */
diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c
index ffc55061..809f5112 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -170,6 +170,12 @@ twi_proto_callback (u8 *buf, u8 size)
speed_aux0.max = buf[4];
aux_traj_goto_start (v8_to_v16 (buf[2], buf[3]), 0);
break;
+ case c ('B', 1):
+ /* Find the zero position of the arm.
+ * - b: speed. */
+ speed_aux0.max = buf[2];
+ aux_traj_find_zero_start (0);
+ break;
case c ('p', x):
/* Set parameters. */
if (twi_proto_params (&buf[2], size - 2) != 0)