summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-28 00:31:13 +0200
committerNicolas Schodet2011-05-29 09:57:32 +0200
commit4882fbefaeca9cc5292cec9a2f93d34f04e67b5c (patch)
tree30c592351bcccc7b3e1f95c8b1af4419be5fec46 /digital
parentfd8ce8373abf81f5d6781b577a0f50906f2f4ff8 (diff)
digital/io-hub: add clamp initialisation
Diffstat (limited to 'digital')
-rw-r--r--digital/ai/src/twi_master/mimot.c42
-rw-r--r--digital/ai/src/twi_master/mimot.h16
-rw-r--r--digital/io-hub/src/robospierre/bot.h14
-rw-r--r--digital/io-hub/src/robospierre/clamp.c92
-rw-r--r--digital/io-hub/src/robospierre/main.c4
-rw-r--r--digital/mimot/src/dirty/twi_proto.c21
6 files changed, 186 insertions, 3 deletions
diff --git a/digital/ai/src/twi_master/mimot.c b/digital/ai/src/twi_master/mimot.c
index af6ff5f9..c3b82823 100644
--- a/digital/ai/src/twi_master/mimot.c
+++ b/digital/ai/src/twi_master/mimot.c
@@ -118,6 +118,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);
@@ -181,3 +205,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..97575b0d 100644
--- a/digital/ai/src/twi_master/mimot.h
+++ b/digital/ai/src/twi_master/mimot.h
@@ -68,6 +68,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);
@@ -92,4 +100,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 */
diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h
index 02b74da9..7a8065dd 100644
--- a/digital/io-hub/src/robospierre/bot.h
+++ b/digital/io-hub/src/robospierre/bot.h
@@ -63,6 +63,8 @@
# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP (0x3b0b / 2 + 1000)
# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP (0x3b0b / 2 + 1000)
# define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP (0x3b0b / 2)
+# define BOT_CLAMP_INIT_FRONT_SEEN_ELEVATION_STEP 0x18ca
+# define BOT_CLAMP_INIT_BACK_SEEN_ELEVATION_STEP 0x18ca
# define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0
# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0
@@ -93,6 +95,9 @@
# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP 0x1da7
# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP 0x1f03
# define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP ((0x1da7 + 0x1f03) / 2)
+// TODO: to be measured.
+# define BOT_CLAMP_INIT_FRONT_SEEN_ELEVATION_STEP (0x1da7 / 2)
+# define BOT_CLAMP_INIT_BACK_SEEN_ELEVATION_STEP (0x1f03 / 2)
# define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0
# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0
@@ -112,12 +117,17 @@
#endif /* !HOST */
+#define BOT_CLAMP_INIT_ELEVATION_STEP \
+ BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP
+
#define BOT_CLAMP_CLOSED_ROTATION_OFFSET(pos) \
(CLAMP_IS_SLOT_IN_FRONT_BAY (pos) \
? BOT_CLAMP_CLOSED_FRONT_ROTATION_OFFSET \
: (CLAMP_IS_SLOT_IN_BACK_BAY (pos) \
? BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET : 0))
+#define BOT_CLAMP_INIT_ELEVATION_SPEED 0x08
+#define BOT_CLAMP_INIT_ROTATION_SPEED -0x04
#define BOT_CLAMP_ELEVATION_SPEED 0x60
#define BOT_CLAMP_ROTATION_SPEED 0x60
#define BOT_CLAMP_ROTATION_OFFSET_SPEED 1
@@ -140,4 +150,8 @@
-0x3ff, 50, ((slot == CLAMP_SLOT_FRONT_BOTTOM \
|| slot == CLAMP_SLOT_BACK_BOTTOM) ? -0x100 : -0x180)
+#define BOT_PWM_CLAMP_INIT 0x1ff, 125, 0
+#define BOT_PWM_DOOR_INIT 0x1ff, 74, 0x55
+#define BOT_PWM_CLAMP_DOOR_INIT 125
+
#endif /* bot_h */
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
index 71cb2dfb..65a90a7a 100644
--- a/digital/io-hub/src/robospierre/clamp.c
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -35,6 +35,8 @@
#include "fsm.h"
#include "fsm_queue.h"
+#include "modules/proto/proto.h"
+
#include "logistic.h"
/*
@@ -50,8 +52,18 @@
FSM_INIT
FSM_STATES (
- /* Initial state, to be complete with full initialisation. */
+ /* Initial state. */
CLAMP_START,
+ /* Initialisation sequence: opening everything. */
+ CLAMP_INIT_OPENING,
+ /* Initialisation sequence: going up until a middle pawn sensor
+ * see the clamp. */
+ CLAMP_INIT_UPING_UNTIL_SEEN,
+ /* Initialisation sequence: going to the middle level. */
+ CLAMP_INIT_GOING_MIDDLE,
+ /* Initialisation sequence: finding front right edge. */
+ CLAMP_INIT_FINDING_EDGE,
+
/* Returning to idle position. */
CLAMP_GOING_IDLE,
/* Waiting external events, clamp at middle level. */
@@ -85,6 +97,8 @@ FSM_STATES (
CLAMP_MOVE_DST_CLAMP_OPENING)
FSM_EVENTS (
+ /* During initialisation sequence, clamp seen by a sensor. */
+ clamp_init_seen,
/* New element inside bottom slot. */
clamp_new_element,
/* Order to drop elements. */
@@ -100,8 +114,12 @@ FSM_EVENTS (
clamp_move_success,
/* Elevation and elevation motor success. */
clamp_elevation_rotation_success,
+ /* Elevation motor success. */
+ clamp_elevation_success,
/* Elevation motor failure. */
clamp_elevation_failure,
+ /* Rotation motor success. */
+ clamp_rotation_success,
/* Rotation motor failure. */
clamp_rotation_failure)
@@ -127,6 +145,10 @@ struct clamp_t
uint8_t open;
/** True if clamp position is controled. */
uint8_t controled;
+ /** Position of clamp when seen by sensor. */
+ uint16_t init_seen_step;
+ /** Position to initialise at seen position. */
+ uint16_t init_seen_init_step;
};
/** Global context. */
@@ -280,6 +302,24 @@ clamp_handle_event (void)
/* Go directly to next point. */
clamp_route ();
}
+ /* Handle initialisation. */
+ if (FSM_CAN_HANDLE (AI, clamp_init_seen))
+ {
+ if (!IO_GET (CONTACT_FRONT_MIDDLE))
+ {
+ ctx.init_seen_step = mimot_get_motor0_position ();
+ ctx.init_seen_init_step = BOT_CLAMP_INIT_FRONT_SEEN_ELEVATION_STEP;
+ FSM_HANDLE (AI, clamp_init_seen);
+ return 1;
+ }
+ if (!IO_GET (CONTACT_BACK_MIDDLE))
+ {
+ ctx.init_seen_step = mimot_get_motor0_position ();
+ ctx.init_seen_init_step = BOT_CLAMP_INIT_BACK_SEEN_ELEVATION_STEP;
+ FSM_HANDLE (AI, clamp_init_seen);
+ return 1;
+ }
+ }
return 0;
}
@@ -374,12 +414,58 @@ clamp_route (void)
/* CLAMP FSM */
-FSM_TRANS (CLAMP_START, init_actuators, CLAMP_GOING_IDLE)
+FSM_TRANS (CLAMP_START, init_actuators, CLAMP_INIT_OPENING)
{
- clamp_move (CLAMP_SLOT_FRONT_MIDDLE);
+ pwm_set_timed (BOT_PWM_DOOR_FRONT_BOTTOM, BOT_PWM_DOOR_INIT);
+ pwm_set_timed (BOT_PWM_DOOR_FRONT_TOP, BOT_PWM_DOOR_INIT);
+ pwm_set_timed (BOT_PWM_DOOR_BACK_BOTTOM, BOT_PWM_DOOR_INIT);
+ pwm_set_timed (BOT_PWM_DOOR_BACK_TOP, BOT_PWM_DOOR_INIT);
+ pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_INIT);
return FSM_NEXT (CLAMP_START, init_actuators);
}
+FSM_TRANS_TIMEOUT (CLAMP_INIT_OPENING, BOT_PWM_CLAMP_DOOR_INIT,
+ CLAMP_INIT_UPING_UNTIL_SEEN)
+{
+ mimot_move_motor0_absolute (mimot_get_motor0_position () +
+ BOT_CLAMP_INIT_ELEVATION_STEP,
+ BOT_CLAMP_INIT_ELEVATION_SPEED);
+ return FSM_NEXT_TIMEOUT (CLAMP_INIT_OPENING);
+}
+
+FSM_TRANS (CLAMP_INIT_UPING_UNTIL_SEEN, clamp_elevation_success, CLAMP_START)
+{
+ /* Dead end. */
+ mimot_motor0_free ();
+ return FSM_NEXT (CLAMP_INIT_UPING_UNTIL_SEEN, clamp_elevation_success);
+}
+
+FSM_TRANS (CLAMP_INIT_UPING_UNTIL_SEEN, clamp_init_seen,
+ CLAMP_INIT_GOING_MIDDLE)
+{
+ mimot_motor0_free ();
+ mimot_set_motor0_position (ctx.init_seen_init_step
+ + mimot_get_motor0_position ()
+ - ctx.init_seen_step);
+ proto_send1w ('C', ctx.init_seen_step);
+ mimot_move_motor0_absolute (BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP,
+ BOT_CLAMP_ELEVATION_SPEED);
+ return FSM_NEXT (CLAMP_INIT_UPING_UNTIL_SEEN, clamp_init_seen);
+}
+
+FSM_TRANS (CLAMP_INIT_GOING_MIDDLE, clamp_elevation_success,
+ CLAMP_INIT_FINDING_EDGE)
+{
+ mimot_motor1_zero_position (BOT_CLAMP_INIT_ROTATION_SPEED);
+ return FSM_NEXT (CLAMP_INIT_GOING_MIDDLE, clamp_elevation_success);
+}
+
+FSM_TRANS (CLAMP_INIT_FINDING_EDGE, clamp_rotation_success, CLAMP_GOING_IDLE)
+{
+ clamp_move (CLAMP_SLOT_FRONT_MIDDLE);
+ return FSM_NEXT (CLAMP_INIT_FINDING_EDGE, clamp_rotation_success);
+}
+
FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE)
{
return FSM_NEXT (CLAMP_GOING_IDLE, clamp_move_success);
diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c
index b1e85b72..7c773761 100644
--- a/digital/io-hub/src/robospierre/main.c
+++ b/digital/io-hub/src/robospierre/main.c
@@ -139,8 +139,12 @@ main_event_to_fsm (void)
if (mimot_motor0_status == success
&& mimot_motor1_status == success)
FSM_HANDLE_E (AI, clamp_elevation_rotation_success);
+ if (mimot_motor0_status == success)
+ FSM_HANDLE_E (AI, clamp_elevation_success);
else if (mimot_motor0_status == failure)
FSM_HANDLE_E (AI, clamp_elevation_failure);
+ if (mimot_motor1_status == success)
+ FSM_HANDLE_E (AI, clamp_rotation_success);
else if (mimot_motor1_status == failure)
FSM_HANDLE_E (AI, clamp_rotation_failure);
/* Clamp specific events. */
diff --git a/digital/mimot/src/dirty/twi_proto.c b/digital/mimot/src/dirty/twi_proto.c
index 3048c594..bda255ef 100644
--- a/digital/mimot/src/dirty/twi_proto.c
+++ b/digital/mimot/src/dirty/twi_proto.c
@@ -150,6 +150,18 @@ twi_proto_callback (u8 *buf, u8 size)
else
buf[0] = 0;
break;
+ case c ('w', 1):
+ /* Free motor.
+ * - b: aux index. */
+ if (buf[2] < AC_ASSERV_AUX_NB)
+ {
+ pos_reset (&pos_aux[buf[2]]);
+ state_aux[buf[2]].mode = MODE_PWM;
+ pwm_set (&pwm_aux[buf[2]], 0);
+ }
+ else
+ buf[0] = 0;
+ break;
case c ('p', x):
/* Set parameters. */
if (twi_proto_params (&buf[2], size - 2) != 0)
@@ -173,6 +185,15 @@ twi_proto_params (u8 *buf, u8 size)
size--;
switch (*buf++)
{
+ case 'Y':
+ /* Set current aux position.
+ * - b: aux index.
+ * - w: position. */
+ if (buf[0] >= AC_ASSERV_AUX_NB || size < 3)
+ return 1;
+ aux[buf[0]].pos = v8_to_v16 (buf[1], buf[2]);
+ eat = 3;
+ break;
default:
return 1;
}