summaryrefslogtreecommitdiff
path: root/digital/ai/src/twi_master
diff options
context:
space:
mode:
Diffstat (limited to 'digital/ai/src/twi_master')
-rw-r--r--digital/ai/src/twi_master/asserv.c28
-rw-r--r--digital/ai/src/twi_master/asserv.h14
-rw-r--r--digital/ai/src/twi_master/mimot.c82
-rw-r--r--digital/ai/src/twi_master/mimot.h30
4 files changed, 141 insertions, 13 deletions
diff --git a/digital/ai/src/twi_master/asserv.c b/digital/ai/src/twi_master/asserv.c
index d066737b..839b1d23 100644
--- a/digital/ai/src/twi_master/asserv.c
+++ b/digital/ai/src/twi_master/asserv.c
@@ -202,12 +202,12 @@ asserv_get_moving_direction (void)
{
/* Foward move? */
if (asserv_status.status & _BV (asserv_status_flag_move_forward))
- return 1;
+ return DIRECTION_FORWARD;
/* Backward move? */
if (asserv_status.status & _BV (asserv_status_flag_move_backward))
- return 2;
+ return DIRECTION_BACKWARD;
/* Not moving */
- return 0;
+ return DIRECTION_NONE;
}
uint8_t
@@ -301,6 +301,28 @@ asserv_go_to_the_wall (uint8_t backward)
}
void
+asserv_push_the_wall (uint8_t backward, uint32_t init_x, uint32_t init_y,
+ uint16_t init_a)
+{
+ if (init_x != (uint32_t) -1)
+ init_x = fixed_mul_f824 (init_x, asserv_scale_inv);
+ if (init_y != (uint32_t) -1)
+ init_y = fixed_mul_f824 (init_y, asserv_scale_inv);
+ uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
+ buffer[0] = 'G';
+ buffer[1] = backward;
+ buffer[2] = v32_to_v8 (init_x, 2);
+ buffer[3] = v32_to_v8 (init_x, 1);
+ buffer[4] = v32_to_v8 (init_x, 0);
+ buffer[5] = v32_to_v8 (init_y, 2);
+ buffer[6] = v32_to_v8 (init_y, 1);
+ buffer[7] = v32_to_v8 (init_y, 0);
+ buffer[8] = v16_to_v8 (init_a, 1);
+ buffer[9] = v16_to_v8 (init_a, 0);
+ twi_master_send_buffer (10);
+}
+
+void
asserv_move_motor0_absolute (uint16_t position, uint8_t speed)
{
uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
diff --git a/digital/ai/src/twi_master/asserv.h b/digital/ai/src/twi_master/asserv.h
index 0d29ba96..c9a52a11 100644
--- a/digital/ai/src/twi_master/asserv.h
+++ b/digital/ai/src/twi_master/asserv.h
@@ -121,16 +121,16 @@ asserv_get_motor1_position (void);
/**
* Are we moving forward/backward?
* @return
- * - 0 we are not moving;
- * - 1 we are moving forward;
- * - 2 we are moving backward.
+ * - DIRECTION_NONE we are not moving;
+ * - DIRECTION_FORWARD we are moving forward;
+ * - DIRECTION_BACKWARD we are moving backward.
*/
uint8_t
asserv_get_moving_direction (void);
/**
* Get the last moving direction of the bot.
- * @return 1 is forward, 2 is backward.
+ * @return DIRECTION_FORWARD or DIRECTION_BACKWARD.
*/
uint8_t
asserv_get_last_moving_direction (void);
@@ -198,6 +198,12 @@ asserv_goto_xya (uint32_t x, uint32_t y, int16_t a, uint8_t backward);
void
asserv_go_to_the_wall (uint8_t backward);
+/** Push the wall and initialise position. Use -1 for coordinates to keep
+ * unchanged. */
+void
+asserv_push_the_wall (uint8_t backward, uint32_t init_x, uint32_t init_y,
+ uint16_t init_a);
+
/**
* Move the motor0.
* Motor0 class command.
diff --git a/digital/ai/src/twi_master/mimot.c b/digital/ai/src/twi_master/mimot.c
index af6ff5f9..8df84538 100644
--- a/digital/ai/src/twi_master/mimot.c
+++ b/digital/ai/src/twi_master/mimot.c
@@ -97,6 +97,12 @@ mimot_motor1_cmd_status (void)
return none;
}
+uint8_t
+mimot_get_input (void)
+{
+ return mimot_status.input_port;
+}
+
uint16_t
mimot_get_motor0_position (void)
{
@@ -118,6 +124,30 @@ mimot_reset (void)
}
void
+mimot_set_motor0_position (uint16_t position)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'p';
+ buffer[1] = 'Y';
+ buffer[2] = 0;
+ buffer[3] = v16_to_v8 (position, 1);
+ buffer[4] = v16_to_v8 (position, 0);
+ twi_master_send_buffer (5);
+}
+
+void
+mimot_set_motor1_position (uint16_t position)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'p';
+ buffer[1] = 'Y';
+ buffer[2] = 1;
+ buffer[3] = v16_to_v8 (position, 1);
+ buffer[4] = v16_to_v8 (position, 0);
+ twi_master_send_buffer (5);
+}
+
+void
mimot_move_motor0_absolute (uint16_t position, uint8_t speed)
{
uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
@@ -142,19 +172,41 @@ mimot_move_motor1_absolute (uint16_t position, uint8_t speed)
void
mimot_motor0_zero_position (int8_t speed)
{
+ mimot_motor0_find_zero (speed, 0, 0);
+}
+
+void
+mimot_motor1_zero_position (int8_t speed)
+{
+ mimot_motor1_find_zero (speed, 0, 0);
+}
+
+void
+mimot_motor0_find_zero (int8_t speed, uint8_t use_switch,
+ uint16_t reset_position)
+{
uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
buffer[0] = 'B';
- buffer[1] = speed;
- twi_master_send_buffer (2);
+ buffer[1] = 0;
+ buffer[2] = speed;
+ buffer[3] = use_switch;
+ buffer[4] = v16_to_v8 (reset_position, 1);
+ buffer[5] = v16_to_v8 (reset_position, 0);
+ twi_master_send_buffer (6);
}
void
-mimot_motor1_zero_position (int8_t speed)
+mimot_motor1_find_zero (int8_t speed, uint8_t use_switch,
+ uint16_t reset_position)
{
uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
- buffer[0] = 'C';
- buffer[1] = speed;
- twi_master_send_buffer (2);
+ buffer[0] = 'B';
+ buffer[1] = 1;
+ buffer[2] = speed;
+ buffer[3] = use_switch;
+ buffer[4] = v16_to_v8 (reset_position, 1);
+ buffer[5] = v16_to_v8 (reset_position, 0);
+ twi_master_send_buffer (6);
}
void
@@ -181,3 +233,21 @@ mimot_motor1_clamp (int8_t speed, int16_t pwm)
twi_master_send_buffer (5);
}
+void
+mimot_motor0_free (void)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'w';
+ buffer[1] = 0;
+ twi_master_send_buffer (2);
+}
+
+void
+mimot_motor1_free (void)
+{
+ uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
+ buffer[0] = 'w';
+ buffer[1] = 1;
+ twi_master_send_buffer (2);
+}
+
diff --git a/digital/ai/src/twi_master/mimot.h b/digital/ai/src/twi_master/mimot.h
index 60811c10..c9de7090 100644
--- a/digital/ai/src/twi_master/mimot.h
+++ b/digital/ai/src/twi_master/mimot.h
@@ -56,6 +56,10 @@ mimot_motor0_cmd_status (void);
asserv_status_e
mimot_motor1_cmd_status (void);
+/** Return input port state. */
+uint8_t
+mimot_get_input (void);
+
/** Get motor0 position in steps. */
uint16_t
mimot_get_motor0_position (void);
@@ -68,6 +72,14 @@ mimot_get_motor1_position (void);
void
mimot_reset (void);
+/** Set motor0 position in steps. */
+void
+mimot_set_motor0_position (uint16_t position);
+
+/** Set motor1 position in steps. */
+void
+mimot_set_motor1_position (uint16_t position);
+
/** Move motor0 to absolute position in steps. */
void
mimot_move_motor0_absolute (uint16_t position, uint8_t speed);
@@ -84,6 +96,16 @@ mimot_motor0_zero_position (int8_t speed);
void
mimot_motor1_zero_position (int8_t speed);
+/** Find zero position. */
+void
+mimot_motor0_find_zero (int8_t speed, uint8_t use_switch,
+ uint16_t reset_position);
+
+/** Find zero position. */
+void
+mimot_motor1_find_zero (int8_t speed, uint8_t use_switch,
+ uint16_t reset_position);
+
/** Clamp motor0. */
void
mimot_motor0_clamp (int8_t speed, int16_t pwm);
@@ -92,4 +114,12 @@ mimot_motor0_clamp (int8_t speed, int16_t pwm);
void
mimot_motor1_clamp (int8_t speed, int16_t pwm);
+/** Free motor0. */
+void
+mimot_motor0_free (void);
+
+/** Free motor1. */
+void
+mimot_motor1_free (void);
+
#endif /* mimot_h */