summaryrefslogtreecommitdiffhomepage
path: root/digital/asserv
diff options
context:
space:
mode:
Diffstat (limited to 'digital/asserv')
-rw-r--r--digital/asserv/src/asserv/aux.c49
-rw-r--r--digital/asserv/src/asserv/aux.h2
-rw-r--r--digital/asserv/src/asserv/main.c9
-rw-r--r--digital/asserv/src/asserv/twi_proto.c6
4 files changed, 19 insertions, 47 deletions
diff --git a/digital/asserv/src/asserv/aux.c b/digital/asserv/src/asserv/aux.c
index f7c48b7a..787a33c5 100644
--- a/digital/asserv/src/asserv/aux.c
+++ b/digital/asserv/src/asserv/aux.c
@@ -50,12 +50,8 @@ enum
AUX_TRAJ_GOTO,
/* Goto position, try to unblock. */
AUX_TRAJ_GOTO_UNBLOCK,
- /* 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,
+ /* Find zero mode, turn until zero is seen. */
+ AUX_TRAJ_FIND_ZERO,
/* Everything done. */
AUX_TRAJ_DONE,
};
@@ -131,43 +127,22 @@ void
aux_traj_find_zero (struct aux_t *aux)
{
uint8_t zero = *aux->zero_pin & aux->zero_bv;
- switch (aux->traj_mode)
+ if (!zero)
{
- case AUX_TRAJ_FIND_ZERO_START:
- if (!zero)
- {
- aux->speed->cons = aux->speed->max << 8;
- aux->traj_mode = AUX_TRAJ_FIND_ZERO_SLOW;
- }
- break;
- case AUX_TRAJ_FIND_ZERO_SLOW:
- if (zero)
- {
- aux->speed->cons = -aux->speed->slow << 8;
- aux->traj_mode = AUX_TRAJ_FIND_ZERO_BACK;
- }
- break;
- case AUX_TRAJ_FIND_ZERO_BACK:
- if (!zero)
- {
- aux->speed->cons = 0;
- state_finish (aux->state);
- aux->pos = 0;
- aux->traj_mode = AUX_TRAJ_DONE;
- }
- break;
- default:
- assert (0);
+ aux->speed->cons = 0;
+ state_finish (aux->state);
+ aux->pos = 0;
+ aux->traj_mode = AUX_TRAJ_DONE;
}
}
/** Start find zero mode. */
void
-aux_traj_find_zero_start (struct aux_t *aux, uint8_t seq)
+aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq)
{
- aux->traj_mode = AUX_TRAJ_FIND_ZERO_START;
+ aux->traj_mode = AUX_TRAJ_FIND_ZERO;
aux->speed->use_pos = 0;
- aux->speed->cons = aux->speed->max << 8;
+ aux->speed->cons = speed << 8;
state_start (aux->state, MODE_TRAJ, seq);
}
@@ -183,9 +158,7 @@ aux_traj_update_single (struct aux_t *aux)
case AUX_TRAJ_GOTO_UNBLOCK:
aux_traj_goto (aux);
break;
- case AUX_TRAJ_FIND_ZERO_START:
- case AUX_TRAJ_FIND_ZERO_SLOW:
- case AUX_TRAJ_FIND_ZERO_BACK:
+ case AUX_TRAJ_FIND_ZERO:
aux_traj_find_zero (aux);
break;
case AUX_TRAJ_DONE:
diff --git a/digital/asserv/src/asserv/aux.h b/digital/asserv/src/asserv/aux.h
index 5a95bdaf..8102d78a 100644
--- a/digital/asserv/src/asserv/aux.h
+++ b/digital/asserv/src/asserv/aux.h
@@ -58,7 +58,7 @@ void
aux_traj_goto_start (struct aux_t *aux, uint16_t pos, uint8_t seq);
void
-aux_traj_find_zero_start (struct aux_t *aux, uint8_t seq);
+aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq);
void
aux_traj_update (void);
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 983995f5..f0b7e5b3 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -455,7 +455,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
v8_to_v32 (0, args[8], args[9], 0),
1, args[10]);
break;
- case c ('y', 3):
+ case c ('y', 4):
/* Auxiliary go to position.
* - b: aux index.
* - w: pos, i16.
@@ -465,14 +465,15 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
aux_traj_goto_start (auxp, v8_to_v16 (args[1], args[2]), args[3]);
break;
- case c ('y', 1):
+ case c ('y', 3):
/* Auxiliary find zero.
* - b: aux index.
+ * - b: speed.
* - b: sequence number. */
if (!auxp) { proto_send0 ('?'); return; }
- if (args[1] == state->sequence)
+ if (args[2] == state->sequence)
break;
- aux_traj_find_zero_start (auxp, args[1]);
+ aux_traj_find_zero_start (auxp, args[1], args[2]);
break;
case c ('a', 3):
/* Set all acknoledge.
diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c
index 10c49eeb..735430a2 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -202,8 +202,7 @@ twi_proto_callback (u8 *buf, u8 size)
case c ('B', 1):
/* Find the zero position of the arm.
* - b: speed. */
- speed_aux[0].max = buf[2];
- aux_traj_find_zero_start (&aux[0], 0);
+ aux_traj_find_zero_start (&aux[0], buf[2], 0);
break;
case c ('c', 3):
/* Move the aux1.
@@ -215,8 +214,7 @@ twi_proto_callback (u8 *buf, u8 size)
case c ('C', 1):
/* Find the zero position of the aux1.
* - b: speed. */
- speed_aux[1].max = buf[2];
- aux_traj_find_zero_start (&aux[1], 0);
+ aux_traj_find_zero_start (&aux[1], buf[2], 0);
break;
case c ('p', x):
/* Set parameters. */