summaryrefslogtreecommitdiff
path: root/digital/mimot/src/dirty/twi_proto.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/mimot/src/dirty/twi_proto.c')
-rw-r--r--digital/mimot/src/dirty/twi_proto.c151
1 files changed, 111 insertions, 40 deletions
diff --git a/digital/mimot/src/dirty/twi_proto.c b/digital/mimot/src/dirty/twi_proto.c
index 3048c594..1de7d30d 100644
--- a/digital/mimot/src/dirty/twi_proto.c
+++ b/digital/mimot/src/dirty/twi_proto.c
@@ -36,7 +36,8 @@
#include "pwm.h"
#include "pos.h"
#include "speed.h"
-#include "aux.h"
+#include "postrack.h"
+#include "traj.h"
#ifdef HOST
# include "simu.host.h"
@@ -73,19 +74,23 @@ twi_proto_update (void)
while ((read_data = twi_slave_poll (buf, sizeof (buf))))
twi_proto_callback (buf, read_data);
/* Update status. */
- u8 status_with_crc[8];
+ u8 status_with_crc[12];
u8 *status = &status_with_crc[1];
status[0] = 0
- | (state_aux[1].blocked << 3)
- | (state_aux[1].finished << 2)
- | (state_aux[0].blocked << 1)
- | (state_aux[0].finished << 0);
+ | (speed_theta.cur < 0 ? (1 << 3) : 0)
+ | (speed_theta.cur > 0 ? (1 << 2) : 0)
+ | (state_main.blocked << 1)
+ | (state_main.finished << 0);
status[1] = PINC;
status[2] = twi_proto.seq;
- status[3] = v16_to_v8 (aux[0].pos, 1);
- status[4] = v16_to_v8 (aux[0].pos, 0);
- status[5] = v16_to_v8 (aux[1].pos, 1);
- status[6] = v16_to_v8 (aux[1].pos, 0);
+ status[3] = v32_to_v8 (postrack_x, 3);
+ status[4] = v32_to_v8 (postrack_x, 2);
+ status[5] = v32_to_v8 (postrack_x, 1);
+ status[6] = v32_to_v8 (postrack_y, 3);
+ status[7] = v32_to_v8 (postrack_y, 2);
+ status[8] = v32_to_v8 (postrack_y, 1);
+ status[9] = v32_to_v8 (postrack_a, 2);
+ status[10] = v32_to_v8 (postrack_a, 1);
/* Compute CRC. */
status_with_crc[0] = crc_compute (&status_with_crc[1],
sizeof (status_with_crc) - 1);
@@ -115,40 +120,68 @@ twi_proto_callback (u8 *buf, u8 size)
/* Reset. */
utils_reset ();
break;
- case c ('b', 3):
- /* Move the aux0.
- * - w: new position.
- * - b: speed. */
- speed_aux[0].max = buf[4];
- aux_traj_goto_start (&aux[0], v8_to_v16 (buf[2], buf[3]), 0);
+ case c ('w', 0):
+ /* Set zero pwm. */
+ pos_reset (&pos_theta);
+ pos_reset (&pos_alpha);
+ state_main.mode = MODE_PWM;
+ pwm_set (&pwm_left, 0);
+ pwm_set (&pwm_right, 0);
break;
- case c ('B', 1):
- /* Find the zero position of the aux0.
- * - b: speed. */
- aux_traj_find_limit_start (&aux[0], buf[2], 0);
+ case c ('s', 0):
+ /* Stop (set zero speed). */
+ state_main.mode = MODE_SPEED;
+ state_main.variant = 0;
+ speed_theta.use_pos = speed_alpha.use_pos = 0;
+ speed_theta.cons = 0;
+ speed_alpha.cons = 0;
break;
- case c ('c', 3):
- /* Move the aux1.
- * - w: new position.
- * - b: speed. */
- speed_aux[1].max = buf[4];
- aux_traj_goto_start (&aux[1], v8_to_v16 (buf[2], buf[3]), 0);
+ case c ('l', 3):
+ /* Set linear speed controlled position consign.
+ * - 3b: theta consign offset. */
+ speed_theta.use_pos = speed_alpha.use_pos = 1;
+ speed_theta.pos_cons = pos_theta.cons;
+ if (buf[2] & 0x80)
+ speed_theta.pos_cons += v8_to_v32 (0xff, buf[2], buf[3], buf[4]);
+ else
+ 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, MODE_SPEED, 0);
break;
- case c ('C', 1):
- /* Find the zero position of the aux1.
- * - b: speed. */
- aux_traj_find_limit_start (&aux[1], buf[2], 0);
+ case c ('a', 2):
+ /* Set angular speed controlled position consign.
+ * - w: angle offset. */
+ traj_angle_offset_start (((int32_t) (int16_t) v8_to_v16 (buf[2], buf[3])) << 8, 0);
break;
- case c ('l', 4):
- /* Clamp.
- * - b: aux index.
- * - b: speed.
- * - w: claming PWM. */
- if (buf[2] < AC_ASSERV_AUX_NB)
- aux_traj_clamp_start (&aux[buf[2]], buf[3],
- v8_to_v16 (buf[4], buf[5]), 0);
- else
- buf[0] = 0;
+ case c ('f', 1):
+ /* Go to the wall.
+ * - b: 0: forward, 1: backward. */
+ traj_ftw_start (buf[2], 0);
+ break;
+ case c ('x', 7):
+ /* Go to position.
+ * - 3b: x position.
+ * - 3b: y position.
+ * - b: backward (see traj.h). */
+ traj_goto_start (v8_to_v32 (buf[2], buf[3], buf[4], 0),
+ v8_to_v32 (buf[5], buf[6], buf[7], 0),
+ buf[8], 0);
+ break;
+ case c ('y', 2):
+ /* Go to angle.
+ * - w: angle. */
+ traj_goto_angle_start (v8_to_v32 (0, buf[2], buf[3], 0), 0);
+ break;
+ case c ('X', 9):
+ /* Go to position, then angle.
+ * - 3b: x position.
+ * - 3b: y position.
+ * - w: angle.
+ * - b: backward (see traj.h). */
+ traj_goto_xya_start (v8_to_v32 (buf[2], buf[3], buf[4], 0),
+ v8_to_v32 (buf[5], buf[6], buf[7], 0),
+ v8_to_v32 (0, buf[8], buf[9], 0),
+ buf[10], 0);
break;
case c ('p', x):
/* Set parameters. */
@@ -173,6 +206,44 @@ twi_proto_params (u8 *buf, u8 size)
size--;
switch (*buf++)
{
+ case 'X':
+ /* Set current X position.
+ * - 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':
+ /* Set current Y position.
+ * - 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':
+ /* Set current angle.
+ * - w: angle. */
+ if (size < 2)
+ return 1;
+ postrack_a = v8_to_v32 (0, buf[0], buf[1], 0);
+ eat = 2;
+ break;
+ case 's':
+ /* Set maximum and slow speed.
+ * - b: theta max.
+ * - b: alpha max.
+ * - b: theta slow.
+ * - 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:
return 1;
}