summaryrefslogtreecommitdiffhomepage
path: root/digital/asserv/src/asserv/aux.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/asserv/src/asserv/aux.c')
-rw-r--r--digital/asserv/src/asserv/aux.c78
1 files changed, 78 insertions, 0 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;
+ }
+}