summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/guybrush/bottom_clamp.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io-hub/src/guybrush/bottom_clamp.c')
-rw-r--r--digital/io-hub/src/guybrush/bottom_clamp.c532
1 files changed, 395 insertions, 137 deletions
diff --git a/digital/io-hub/src/guybrush/bottom_clamp.c b/digital/io-hub/src/guybrush/bottom_clamp.c
index c4f58857..52a61705 100644
--- a/digital/io-hub/src/guybrush/bottom_clamp.c
+++ b/digital/io-hub/src/guybrush/bottom_clamp.c
@@ -31,6 +31,7 @@
#include "main.h"
#include "bottom_clamp.h"
#include "pressure.h"
+#include "strat.h"
#define FSM_NAME AI
@@ -52,8 +53,10 @@ FSM_STATES (
CLAMP_START,
/* ---------------------Initialisation sequence---------------------- */
- /*Opening the 2 clamps. */
- CLAMP_INIT_OPEN,
+ /*Opening all the clamps. */
+ CLAMP_INIT_OPEN_ALL_CLAMPS,
+ /*Closing all the clamps (except bottom clamp 2). */
+ CLAMP_INIT_CLOSE_ALL_CLAMPS,
/*Finding 0 position. */
CLAMP_INIT_FIND_0,
/* Hide the clamp in order not to go over the robot. */
@@ -105,7 +108,6 @@ FSM_STATES (
/*Make the coin "fall" into the clamp (the clamp has to be 90° from the floor*/
CLAMP_READY_TO_RECALE,
CLAMP_READY_TO_RECALE_2,
- CLAMP_BEGIN_RECALE,
CLAMP_TEMPO_RECALE,
CLAMP_END_RECALE,
@@ -116,12 +118,18 @@ FSM_STATES (
CLAMP_BOTTOM_CLAMP_BACK,
/*------------------------ Clamp blocked------------------------------. */
+ CLAMP_BLOCKED_UPPER_SET_DOWN,
CLAMP_BLOCKED,
CLAMP_OPEN_BOTTOM_CLAMPS,
CLAMP_TURN_BACKWARD,
CLAMP_WAIT,
CLAMP_TURN_FORWARD,
- CLAMP_SHITTY_STATE
+ CLAMP_SHITTY_STATE,
+ /*-------------------------Upper set not going down-------------------- */
+ CLAMP_UPPER_SET_BLOCKED_UP,
+ CLAMP_UPPER_SET_FOLD_BACK,
+ CLAMP_UPPER_SET_DEAD,
+ CLAMP_TRY_ROTATION
)
FSM_EVENTS (
@@ -159,8 +167,12 @@ FSM_EVENTS (
robot_is_back,
/* The clamp is blocked. We can try to unblock it*/
clamp_blocked,
+ /* Try to unblock clamp. */
+ clamp_unblock,
/* We tryed to unblock the clamp too many time. we can now say that the bottom clamp is out of order*/
- clamp_is_dead
+ clamp_is_dead,
+ /* The upper set is not moving anymore*/
+ upper_set_is_dead
)
@@ -176,35 +188,42 @@ FSM_START_WITH (CLAMP_START)
#define TIMEOUT_RECENTRAGE 100
#define TIMEOUT_BLOCKED 100
#define TIMEOUT_IDLE 70
+#define TIMEOUT_UPPER_SET_DOWN 1000
/*-------------------------------------
- ROTATION DEFINITION
+ ROTATON SPEED DEFINITION
---------------------------------------*/
-#define POS_DELAY 1200
-#define INIT_BOTTOM_POS (6.25+HALF_TURN)
-#define HIDE_POS 3
-#define BACK_TO_READY (16-HIDE_POS)
-#define HALF_TURN 8
-#define QUARTER_TURN 4
-#define DECALAGE_CD_BAS 1
-#define HIDE_POS_TREE 3
-#define HIDE_POS_TREE_2 (2-DECALAGE_CD_BAS)
-#define BACK_TO_READY_TREE (-HIDE_POS_TREE + DECALAGE_CD_BAS)
-#define BACK_TO_READY_TREE_2 (HALF_TURN - HIDE_POS_TREE_2 - DECALAGE_CD_BAS)
-#define RECALE_COIN (QUARTER_TURN - HIDE_POS_TREE_2 - DECALAGE_CD_BAS)
-#define BACK_TO_READY_RECALE_COIN (-QUARTER_TURN)
#define FAST_ROTATION 0x30
#define MEDIUM_ROTATION 0x10
#define SLOW_ROTATION 0x01
+
+/*-------------------------------------
+ PRESSURE DEFINITION
+---------------------------------------*/
#define LOW_PRESSURE 0x300
#define HIGH_PRESSURE 0x380
+
+/*-------------------------------------
+ POSITION DEFINITION
+---------------------------------------*/
+#define RECALE_COIN 1100
+#define CLAMP_1_DOWN 3929
+#define CLAMP_1_UP 1929
+#define POS_TO_DROP 800
+#define HIDE_POS_TREE 550
+#define HIDE_POS_TREE_PLUS (HIDE_POS_TREE + 100)
+#define DECALAGE_CD_BAS 319
+#define LET_UPPER_SET_UP 500
+#define DECALAGE_INIT 1662
/*-------------------------------------
Clamp context
---------------------------------------*/
struct clamp_t
{
/** Current position. */
- int pos_current;
+ int32_t current_pos;
+ /** Init position. */
+ uint16_t init_pos;
/* Clamp_1 is low, which means, it is ready to take a coin*/
uint8_t clamp_1_down;
/** True we are stopping the tree approach. */
@@ -215,6 +234,10 @@ struct clamp_t
uint8_t cpt_blocked;
/** Request from top FSM to be processed once in IDLE state. */
uint16_t idle_request;
+ /** Compting how many times the upper set is blocked*/
+ uint8_t upper_set_blocked_cpt;
+ uint8_t unblocking_upper_set;
+ uint8_t upper_set_is_dead;
};
/*Global context. */
@@ -227,17 +250,57 @@ clamp_blocked (void)
{
}
-static void move_needed(int move,int speed)
+static int is_clamp_1_down(uint32_t pos)
+{
+ if ((pos%4000 > 3000) || (pos%4000 < 1000))
+ return 1;
+ else
+ return 0;
+}
+
+
+
+static void move_needed2(uint16_t angle,uint8_t speed,int8_t direction)
{
- ctx.pos_current += move;
- mimot_move_motor0_absolute (ctx.pos_current, speed);
+ int32_t new_pos;
+ if (angle >= ctx.current_pos%4000)
+ {
+ if (direction==1)
+ {
+ new_pos=ctx.current_pos-ctx.current_pos%4000+angle;
+ }
+ else
+ {
+ new_pos=ctx.current_pos-ctx.current_pos%4000+4000-angle;
+ }
+
+ }
+ else
+ {
+ if (direction==1)
+ {
+ new_pos=ctx.current_pos-ctx.current_pos%4000+angle+4000;
+ }
+ else
+ {
+ new_pos=ctx.current_pos-ctx.current_pos%4000+angle;
+ }
+ }
+ mimot_move_motor0_absolute (new_pos+ctx.init_pos, speed);
+ ctx.current_pos = new_pos;
}
+
void clamp_calm_mode(int mode)
{
ctx.calm_mode=mode;
}
+uint8_t clamp_calm_mode_read(void)
+{
+ return ctx.calm_mode;
+}
+
void
clamp_request (uint16_t event)
{
@@ -261,41 +324,61 @@ uint8_t clamp_read_blocked_cpt(void)
/*---------------------------------------------------------*/
/* Init Bottom clamp. */
-
-FSM_TRANS (CLAMP_START, init_actuators, CLAMP_INIT_OPEN)
+FSM_TRANS (CLAMP_START, init_actuators, CLAMP_INIT_OPEN_ALL_CLAMPS)
{
- ctx.pos_current = 0;
/* Starting the pump */
pressure_set(LOW_PRESSURE);
- /* Opening the 2 clamps. */
+ /*Opening bottom Clamps*/
+ IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE);
IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE);
- IO_SET (OUTPUT_LOWER_CLAMP_2_CLOSE);
- /* Open upper clamp. */
+ /*Opening upper clamps*/
IO_SET (OUTPUT_UPPER_CLAMP_OPEN);
- /* recentrage the middle clamp. */
- IO_SET (OUTPUT_UPPER_CLAMP_OUT);
- IO_CLR (OUTPUT_UPPER_CLAMP_IN);
- /*Contrepression*/
- IO_SET (OUTPUT_UPPER_CLAMP_UP);
-
+
return FSM_NEXT (CLAMP_START,init_actuators);
+
+}
+
+FSM_TRANS_TIMEOUT (CLAMP_INIT_OPEN_ALL_CLAMPS,TIMEOUT_OPEN_CLAMPS,
+ upper_set_in,CLAMP_INIT_CLOSE_ALL_CLAMPS,
+ upper_set_out,CLAMP_SHITTY_STATE)
+{
+ /*if upper set is inside the robot*/
+ if(IO_GET(CONTACT_UPPER_CLAMP_DOWN))
+ {
+ /* closing all the clamps except the clamp 1 to proceed the find_O function. */
+ IO_SET (OUTPUT_LOWER_CLAMP_2_CLOSE);
+ IO_CLR (OUTPUT_UPPER_CLAMP_OPEN);
+ return FSM_NEXT_TIMEOUT (CLAMP_INIT_OPEN_ALL_CLAMPS,upper_set_in);
+ }
+ /*if it is outside, go to shitty state and stop everything*/
+ else
+ {
+ return FSM_NEXT_TIMEOUT (CLAMP_INIT_OPEN_ALL_CLAMPS,upper_set_out);
+ }
+
}
-FSM_TRANS_TIMEOUT (CLAMP_INIT_OPEN, 5*TIMEOUT_OPEN_CLAMPS, CLAMP_INIT_FIND_0)
+FSM_TRANS_TIMEOUT (CLAMP_INIT_CLOSE_ALL_CLAMPS, 5*TIMEOUT_OPEN_CLAMPS, CLAMP_INIT_FIND_0)
{
/*Findig the 0 position. */
- move_needed(8000,SLOW_ROTATION);
- return FSM_NEXT_TIMEOUT (CLAMP_INIT_OPEN);
+ ctx.current_pos = 0;
+ /* Init of the upper set blocked cpt */
+ ctx.upper_set_blocked_cpt = 0;
+ ctx.unblocking_upper_set = 0;
+ ctx.upper_set_is_dead = 0;
+ move_needed2(8000,SLOW_ROTATION,1);
+ return FSM_NEXT_TIMEOUT (CLAMP_INIT_CLOSE_ALL_CLAMPS);
}
FSM_TRANS (CLAMP_INIT_FIND_0, 0_found, CLAMP_INIT_HIDE_CLAMP)
{
/*init of the position*/
- ctx.pos_current = mimot_get_motor0_position();
-
- /* Hidding the clamp inside the robot. */
- move_needed(INIT_BOTTOM_POS * 250,FAST_ROTATION);
+ ctx.init_pos = mimot_get_motor0_position() + DECALAGE_INIT;
+ ctx.current_pos = 0;
+ /* Robot is ready with clamp 1 at the bottom. */
+ move_needed2(CLAMP_1_DOWN,FAST_ROTATION,1);
+ ctx.clamp_1_down = 1;
return FSM_NEXT (CLAMP_INIT_FIND_0, 0_found);
}
@@ -303,6 +386,8 @@ FSM_TRANS (CLAMP_INIT_HIDE_CLAMP, lower_clamp_rotation_success,
normal_start,CLAMP_INIT_READY,
demo_start,CLAMP_GOING_IDLE)
{
+ /*Opening the clamp 2. We do it know to save the batteries*/
+ IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE);
/*Looking if we are in demo mode or normal mode*/
if (!main_demo)
return FSM_NEXT (CLAMP_INIT_HIDE_CLAMP,lower_clamp_rotation_success,normal_start);
@@ -310,19 +395,23 @@ FSM_TRANS (CLAMP_INIT_HIDE_CLAMP, lower_clamp_rotation_success,
return FSM_NEXT (CLAMP_INIT_HIDE_CLAMP,lower_clamp_rotation_success,demo_start);
}
-
FSM_TRANS (CLAMP_INIT_READY,init_start_round, CLAMP_GOING_IDLE)
{
return FSM_NEXT (CLAMP_INIT_READY, init_start_round);
}
-FSM_TRANS (CLAMP_GOING_IDLE, lower_clamp_rotation_success, CLAMP_WAIT_BEFORE_IDLE)
+FSM_TRANS_TIMEOUT (CLAMP_GOING_IDLE, 3*TIMEOUT_OPEN_CLAMPS, CLAMP_WAIT_BEFORE_IDLE)
{
/*Going back to the idle position, ready for showtime.*/
ctx.cpt_blocked = 0;
- ctx.clamp_1_down = 0;
- IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE);
- return FSM_NEXT (CLAMP_GOING_IDLE, lower_clamp_rotation_success);
+ /* Open upper clamp. */
+ IO_SET (OUTPUT_UPPER_CLAMP_OPEN);
+ /* recentrage the middle clamp. */
+ IO_SET (OUTPUT_UPPER_CLAMP_OUT);
+ IO_CLR (OUTPUT_UPPER_CLAMP_IN);
+ /*Contrepression*/
+ IO_SET (OUTPUT_UPPER_CLAMP_UP);
+ return FSM_NEXT_TIMEOUT (CLAMP_GOING_IDLE);
}
/*---------------------------------------------------------*/
@@ -346,13 +435,13 @@ FSM_TRANS_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE, TIMEOUT_IDLE,
else if (idle_request == FSM_EVENT (AI, tree_detected))
{
/* Hidding the clamp inside the robot. */
- if (ctx.clamp_1_down)
+ if (is_clamp_1_down(ctx.current_pos))
{
- move_needed (HIDE_POS_TREE * 250, FAST_ROTATION);
+ move_needed2 (HIDE_POS_TREE, FAST_ROTATION,1);
}
else
{
- move_needed ((HALF_TURN + HIDE_POS_TREE) * 250, FAST_ROTATION);
+ move_needed2 (HIDE_POS_TREE, FAST_ROTATION,1);
ctx.clamp_1_down = 1;
}
return FSM_NEXT_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE,
@@ -371,7 +460,6 @@ FSM_TRANS_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE, TIMEOUT_IDLE,
return FSM_NEXT_TIMEOUT (CLAMP_WAIT_BEFORE_IDLE, idle);
}
}
-
/*---------------------------------------------------------*/
/* parts of the FSM that Clean areas */
/*---------------------------------------------------------*/
@@ -384,7 +472,7 @@ FSM_TRANS (CLAMP_IDLE, clean_start,CLAMP_READY_TO_CLEAN)
FSM_TRANS (CLAMP_READY_TO_CLEAN, clean_catch,CLAMP_CATCH_COIN)
{
/*If the clamp 1 has the CD.*/
- if (ctx.clamp_1_down)
+ if (is_clamp_1_down(ctx.current_pos))
{
/*Close it.*/
IO_SET (OUTPUT_LOWER_CLAMP_1_CLOSE);
@@ -407,60 +495,53 @@ FSM_TRANS (CLAMP_READY_TO_LOAD, clean_load,CLAMP_TAKE_COIN)
return FSM_NEXT(CLAMP_READY_TO_LOAD,clean_load);
}
-
-
/*---------------------------------------------------------*/
/* parts of the FSM that Takes coin */
/*---------------------------------------------------------*/
-FSM_TRANS (CLAMP_IDLE, coin_detected,
- normal_clamp,CLAMP_TAKE_COIN,
- calm_clamp,CLAMP_IDLE)
+FSM_TRANS (CLAMP_IDLE, coin_detected,CLAMP_TAKE_COIN)
{
- if (!ctx.calm_mode)
+ strat_coin_taken ();
+ if (is_clamp_1_down(ctx.current_pos))
{
- if (ctx.clamp_1_down)
- {
- /*Close it.*/
- IO_SET (OUTPUT_LOWER_CLAMP_1_CLOSE);
- }
- /*If the clamp 2 is ready*/
- else
- {
- IO_SET(OUTPUT_LOWER_CLAMP_2_CLOSE);
- }
- fsm_queue_post_event (FSM_EVENT (AI, taking_coin));
- return FSM_NEXT (CLAMP_IDLE, coin_detected,normal_clamp);
+ /*Close it.*/
+ IO_SET (OUTPUT_LOWER_CLAMP_1_CLOSE);
}
- else
+ /*If the clamp 2 is ready*/
+ else
{
- return FSM_NEXT (CLAMP_IDLE, coin_detected,calm_clamp);
+ IO_SET(OUTPUT_LOWER_CLAMP_2_CLOSE);
}
+ fsm_queue_post_event (FSM_EVENT (AI, taking_coin));
+ return FSM_NEXT (CLAMP_IDLE, coin_detected);
}
-
FSM_TRANS_TIMEOUT (CLAMP_TAKE_COIN, TIMEOUT_CLOSE_CLAMPS, CLAMP_TURN_HALF_WAY)
{
- main_set_drop_coin_pos(ctx.pos_current + (HALF_TURN * 250) - POS_DELAY);
- move_needed(HALF_TURN * 250,FAST_ROTATION);
+ main_set_drop_coin_pos(ctx.current_pos + POS_TO_DROP + ctx.init_pos);
+ if (is_clamp_1_down(ctx.current_pos))
+ {
+ move_needed2(CLAMP_1_UP,FAST_ROTATION,1);
+ }
+ else
+ {
+ move_needed2(CLAMP_1_DOWN,FAST_ROTATION,1);
+ }
+
return FSM_NEXT_TIMEOUT (CLAMP_TAKE_COIN);
}
FSM_TRANS (CLAMP_TURN_HALF_WAY, time_to_drop_coin, CLAMP_DROP_CD)
{
- /*If the clamp 1 has the CD.*/
- if (ctx.clamp_1_down)
+ /*If the clamp 1 up (ready to drop the cd).*/
+ if (!is_clamp_1_down(ctx.current_pos))
{
/*Open it.*/
IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE);
- /*Clamp 1 is now up (clamp 2 is down).*/
- ctx.clamp_1_down = 0;
}
/*If the clamp 2 is closed. */
else
{
- IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE);
- /*Clamp 1 is now down (clamp 2 is up). */
- ctx.clamp_1_down = 1;
+ IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE);
}
return FSM_NEXT (CLAMP_TURN_HALF_WAY,time_to_drop_coin);
}
@@ -485,29 +566,20 @@ FSM_TRANS (CLAMP_DROP_CD, lower_clamp_rotation_failure, CLAMP_BLOCKED)
/* Parts of the FSM that is Approaching the tree (totem) */
/*---------------------------------------------------------*/
-FSM_TRANS (CLAMP_IDLE, tree_detected,
- normal_clamp,CLAMP_BOTTOM_CLAMP_HIDE_POS,
- calm_clamp,CLAMP_IDLE)
+FSM_TRANS (CLAMP_IDLE, tree_detected,CLAMP_BOTTOM_CLAMP_HIDE_POS)
{
- ctx.calm_mode=0;
- if (!ctx.calm_mode)
+ uint16_t rotation;
+ if (ctx.upper_set_blocked_cpt>0)
{
- /*Hidding the clamp inside the robot*/
- if (ctx.clamp_1_down)
- {
- move_needed(HIDE_POS_TREE * 250,FAST_ROTATION);
- }
- else
- {
- move_needed((HALF_TURN + HIDE_POS_TREE) * 250,FAST_ROTATION);
- ctx.clamp_1_down = 1;
- }
- return FSM_NEXT (CLAMP_IDLE, tree_detected,normal_clamp);
+ rotation = HIDE_POS_TREE_PLUS;
}
else
{
- return FSM_NEXT (CLAMP_IDLE, tree_detected,calm_clamp);
+ rotation = HIDE_POS_TREE;
}
+ /*Hidding the clamp inside the robot*/
+ move_needed2(rotation,FAST_ROTATION,1);
+ return FSM_NEXT (CLAMP_IDLE, tree_detected);
}
@@ -529,15 +601,21 @@ FSM_TRANS (CLAMP_BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_failure, CLAMP_BLOC
FSM_TRANS (CLAMP_UNFOLD_UPPER_SET, upper_set_down, CLAMP_BOTTOM_CLAMP_READY)
{
/*Putting the bottom clamp back to ready.*/
- move_needed(BACK_TO_READY_TREE * 250,FAST_ROTATION);
+ move_needed2(DECALAGE_CD_BAS,FAST_ROTATION,-1);
/*Opening the top clamp.*/
- IO_SET (OUTPUT_UPPER_CLAMP_OPEN);
+ IO_SET (OUTPUT_UPPER_CLAMP_OPEN);
return FSM_NEXT (CLAMP_UNFOLD_UPPER_SET, upper_set_down);
}
+/** If the upper set doesn't go down*/
+FSM_TRANS_TIMEOUT (CLAMP_UNFOLD_UPPER_SET, TIMEOUT_UPPER_SET_DOWN, CLAMP_UPPER_SET_BLOCKED_UP)
+{
+ return FSM_NEXT_TIMEOUT (CLAMP_UNFOLD_UPPER_SET);
+}
+
FSM_TRANS (CLAMP_BOTTOM_CLAMP_READY, lower_clamp_rotation_success, CLAMP_READY_TO_EMPTY_TREE)
{
@@ -546,10 +624,8 @@ FSM_TRANS (CLAMP_BOTTOM_CLAMP_READY, lower_clamp_rotation_success, CLAMP_READY_T
}
-FSM_TRANS (CLAMP_BOTTOM_CLAMP_READY, lower_clamp_rotation_failure, CLAMP_BLOCKED)
+FSM_TRANS (CLAMP_BOTTOM_CLAMP_READY, lower_clamp_rotation_failure, CLAMP_BLOCKED_UPPER_SET_DOWN)
{
- /*The clamp is blocked somehow.*/
- clamp_blocked();
return FSM_NEXT (CLAMP_BOTTOM_CLAMP_HIDE_POS, lower_clamp_rotation_failure);
}
@@ -581,15 +657,24 @@ FSM_TRANS(CLAMP_CLOSE_ALL_CLAMPS, robot_is_back,CLAMP_REARRANGE_CD)
}
+FSM_TRANS (CLAMP_CLOSE_ALL_CLAMPS, stop_tree_approach, CLAMP_READY_TO_EMPTY_TREE)
+{
+ IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE);
+ IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE);
+ IO_SET (OUTPUT_UPPER_CLAMP_OPEN);
+ fsm_queue_post_event (FSM_EVENT (AI, clamps_ready));
+ return FSM_NEXT (CLAMP_CLOSE_ALL_CLAMPS, stop_tree_approach);
+}
+
FSM_TRANS_TIMEOUT (CLAMP_REARRANGE_CD, TIMEOUT_RECENTRAGE, CLAMP_BOTTOM_CLAMP_HIDE_POS2)
{
/*Hidding the clamp inside the robot.*/
- move_needed(HIDE_POS_TREE_2 * 250,FAST_ROTATION);
+ move_needed2(LET_UPPER_SET_UP,MEDIUM_ROTATION,1);
return FSM_NEXT_TIMEOUT (CLAMP_REARRANGE_CD);
}
FSM_TRANS (CLAMP_BOTTOM_CLAMP_HIDE_POS2, lower_clamp_rotation_success, CLAMP_RELEASE_ASSERV)
{
- mimot_motor0_free();
+ mimot_motor_free (0, 1);
return FSM_NEXT (CLAMP_BOTTOM_CLAMP_HIDE_POS2, lower_clamp_rotation_success);
}
@@ -621,31 +706,28 @@ FSM_TRANS_TIMEOUT (CLAMP_OPEN_UPPER_CLAMPS, TIMEOUT_OPEN_CLAMPS,
stop_tree_branch,CLAMP_TURN_HALF_WAY,
continue_empty_tree_branch,CLAMP_READY_TO_RECALE)
{
- if (ctx.clamp_1_down)
+ if (is_clamp_1_down(ctx.current_pos))
IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE);
else
IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE);
if (ctx.stop_tree_approach)
{
- main_set_drop_coin_pos(ctx.pos_current + ((HALF_TURN - HIDE_POS_TREE) * 250) - POS_DELAY);
- move_needed((HALF_TURN - HIDE_POS_TREE) * 250,FAST_ROTATION);
+ main_set_drop_coin_pos(ctx.current_pos + POS_TO_DROP + ctx.init_pos);
+ move_needed2(CLAMP_1_UP,FAST_ROTATION,1);
ctx.stop_tree_approach = 0;
fsm_queue_post_event (FSM_EVENT (AI, clamps_ready));
return FSM_NEXT_TIMEOUT (CLAMP_OPEN_UPPER_CLAMPS,stop_tree_branch);
}
else
{
- move_needed(RECALE_COIN * 250,SLOW_ROTATION);
+ move_needed2(RECALE_COIN,MEDIUM_ROTATION,1);
return FSM_NEXT_TIMEOUT (CLAMP_OPEN_UPPER_CLAMPS,continue_empty_tree_branch);
}
}
FSM_TRANS (CLAMP_READY_TO_RECALE, lower_clamp_rotation_success, CLAMP_READY_TO_RECALE_2)
{
- if (ctx.clamp_1_down)
- IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE);
- else
- IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE);
+ IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE);
return FSM_NEXT (CLAMP_READY_TO_RECALE,lower_clamp_rotation_success);
}
@@ -656,16 +738,13 @@ FSM_TRANS (CLAMP_READY_TO_RECALE, lower_clamp_rotation_failure, CLAMP_BLOCKED)
FSM_TRANS_TIMEOUT (CLAMP_READY_TO_RECALE_2, TIMEOUT_OPEN_CLAMPS, CLAMP_TEMPO_RECALE)
{
- if (ctx.clamp_1_down)
- IO_SET (OUTPUT_LOWER_CLAMP_1_CLOSE);
- else
- IO_SET (OUTPUT_LOWER_CLAMP_2_CLOSE);
+ IO_SET (OUTPUT_LOWER_CLAMP_1_CLOSE);
return FSM_NEXT_TIMEOUT (CLAMP_READY_TO_RECALE_2);
}
FSM_TRANS_TIMEOUT (CLAMP_TEMPO_RECALE, TIMEOUT_OPEN_CLAMPS,CLAMP_END_RECALE)
{
- move_needed(BACK_TO_READY_RECALE_COIN * 250,FAST_ROTATION);
+ move_needed2(CLAMP_1_DOWN,FAST_ROTATION,-1);
return FSM_NEXT_TIMEOUT (CLAMP_TEMPO_RECALE);
}
@@ -676,8 +755,9 @@ FSM_TRANS (CLAMP_END_RECALE, lower_clamp_rotation_failure,CLAMP_BLOCKED)
FSM_TRANS (CLAMP_END_RECALE, lower_clamp_rotation_success,CLAMP_TURN_HALF_WAY)
{
- main_set_drop_coin_pos(ctx.pos_current + (HALF_TURN * 250) - POS_DELAY);
- move_needed(HALF_TURN * 250,FAST_ROTATION);
+
+ main_set_drop_coin_pos(ctx.current_pos + POS_TO_DROP + ctx.init_pos);
+ move_needed2(CLAMP_1_UP,FAST_ROTATION,1);
return FSM_NEXT (CLAMP_END_RECALE, lower_clamp_rotation_success);
}
/*---------------------------------------------------------------------------------*/
@@ -686,13 +766,13 @@ FSM_TRANS (CLAMP_END_RECALE, lower_clamp_rotation_success,CLAMP_TURN_HALF_WAY)
FSM_TRANS (CLAMP_BOTTOM_CLAMP_HIDE_POS,stop_tree_approach,CLAMP_BOTTOM_CLAMP_BACK)
{
/*Putting the clamp back to take coin*/
- if (ctx.clamp_1_down)
+ if (is_clamp_1_down(ctx.current_pos))
{
- move_needed(BACK_TO_READY_TREE * 250,FAST_ROTATION);
+ move_needed2(CLAMP_1_UP,FAST_ROTATION,1);
}
else
{
- move_needed((16-BACK_TO_READY_TREE) * 250,FAST_ROTATION);
+ move_needed2(CLAMP_1_DOWN,FAST_ROTATION,1);
}
return FSM_NEXT(CLAMP_BOTTOM_CLAMP_HIDE_POS,stop_tree_approach);
@@ -737,29 +817,117 @@ FSM_TRANS (CLAMP_FINISH_BOTTOM_CLAMP_READY, lower_clamp_rotation_failure,CLAMP_B
FSM_TRANS (CLAMP_READY_TO_EMPTY_TREE, stop_tree_approach, CLAMP_REARRANGE_CD)
{
+ /*Closgin bottom clamp. */
+ IO_SET (OUTPUT_LOWER_CLAMP_1_CLOSE);
+ /*We need to close both clamp to have an easier turn around. */
+ IO_SET (OUTPUT_LOWER_CLAMP_2_CLOSE);
+
+ /*Closing upper & middle clamps.*/
+ IO_CLR (OUTPUT_UPPER_CLAMP_OPEN);
return FSM_NEXT (CLAMP_READY_TO_EMPTY_TREE, stop_tree_approach);
}
/*---------------------------------------------------------------------------------*/
/* Parts of the FSM that takes care of the bottom clamp when it's blocked */
+/* with the uppser set is DOWN */
+/*---------------------------------------------------------------------------------*/
+FSM_TRANS (CLAMP_BLOCKED_UPPER_SET_DOWN, upper_set_up, CLAMP_BLOCKED)
+{
+ return FSM_NEXT (CLAMP_BLOCKED_UPPER_SET_DOWN, upper_set_up);
+}
+
+FSM_TRANS_TIMEOUT (CLAMP_BLOCKED_UPPER_SET_DOWN, TIMEOUT_UPPER_SET_DOWN, CLAMP_BLOCKED)
+{
+ /** folding upper set */
+ IO_CLR (OUTPUT_UPPER_CLAMP_DOWN);
+ IO_SET (OUTPUT_UPPER_CLAMP_UP);
+ return FSM_NEXT_TIMEOUT (CLAMP_BLOCKED_UPPER_SET_DOWN);
+}
+
+/*---------------------------------------------------------------------------------*/
+/* Parts of the FSM that takes care of the bottom clamp when it's blocked */
+/* with the uppser set up */
/*---------------------------------------------------------------------------------*/
FSM_TRANS_TIMEOUT (CLAMP_BLOCKED,TIMEOUT_OPEN_CLAMPS,CLAMP_OPEN_BOTTOM_CLAMPS)
{
fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked));
- /* Opening the 2 clamps. */
- IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE);
- IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE);
+ /* Opening OR Closing the 2 clamps. */
+ if (ctx.cpt_blocked%2==0)
+ {
+ IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE);
+ IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE);
+ }
+ else
+ {
+ IO_SET (OUTPUT_LOWER_CLAMP_1_CLOSE);
+ IO_SET (OUTPUT_LOWER_CLAMP_2_CLOSE);
+ }
+ /*Setting upper clamp up*/
+ IO_CLR (OUTPUT_UPPER_CLAMP_DOWN);
+ IO_SET (OUTPUT_UPPER_CLAMP_UP);
return FSM_NEXT_TIMEOUT (CLAMP_BLOCKED);
}
-FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS,robot_is_back, CLAMP_WAIT)
+FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS, clamp_unblock, CLAMP_WAIT)
{
- mimot_move_motor0_absolute (mimot_get_motor0_position() - 16 * 250, MEDIUM_ROTATION);
+ uint32_t pos;
+ pos = mimot_get_motor0_position();
+ if (ctx.cpt_blocked%2==0)
+ {
+ mimot_move_motor0_absolute (pos-4000, MEDIUM_ROTATION);
+ }
+ else
+ {
+ mimot_move_motor0_absolute (4000-pos, MEDIUM_ROTATION);
+ }
+ return FSM_NEXT (CLAMP_OPEN_BOTTOM_CLAMPS, clamp_unblock);
+}
+
+FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS, tree_detected, CLAMP_OPEN_BOTTOM_CLAMPS)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked));
+ return FSM_NEXT (CLAMP_OPEN_BOTTOM_CLAMPS, tree_detected);
+}
+
+FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS, robot_is_back, CLAMP_OPEN_BOTTOM_CLAMPS)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked));
return FSM_NEXT (CLAMP_OPEN_BOTTOM_CLAMPS, robot_is_back);
}
+FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS, clean_start, CLAMP_OPEN_BOTTOM_CLAMPS)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked));
+ return FSM_NEXT (CLAMP_OPEN_BOTTOM_CLAMPS, clean_start);
+}
+
+FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS,clean_catch, CLAMP_OPEN_BOTTOM_CLAMPS)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked));
+ return FSM_NEXT (CLAMP_OPEN_BOTTOM_CLAMPS, clean_catch);
+}
+
+FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS,clean_load, CLAMP_OPEN_BOTTOM_CLAMPS)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked));
+ return FSM_NEXT (CLAMP_OPEN_BOTTOM_CLAMPS, clean_load);
+}
+
+FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS,empty_tree, CLAMP_OPEN_BOTTOM_CLAMPS)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked));
+ return FSM_NEXT (CLAMP_OPEN_BOTTOM_CLAMPS, empty_tree);
+}
+
+FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS,stop_tree_approach, CLAMP_OPEN_BOTTOM_CLAMPS)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked));
+ return FSM_NEXT (CLAMP_OPEN_BOTTOM_CLAMPS, stop_tree_approach);
+}
+
+
FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS,clamp_is_dead, CLAMP_SHITTY_STATE)
{
return FSM_NEXT (CLAMP_OPEN_BOTTOM_CLAMPS, clamp_is_dead);
@@ -767,6 +935,17 @@ FSM_TRANS (CLAMP_OPEN_BOTTOM_CLAMPS,clamp_is_dead, CLAMP_SHITTY_STATE)
FSM_TRANS (CLAMP_WAIT,lower_clamp_rotation_success, CLAMP_TURN_BACKWARD)
{
+ /* Opening OR Closing the 2 clamps. */
+ if (ctx.cpt_blocked%2==1)
+ {
+ IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE);
+ IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE);
+ }
+ else
+ {
+ IO_SET (OUTPUT_LOWER_CLAMP_1_CLOSE);
+ IO_SET (OUTPUT_LOWER_CLAMP_2_CLOSE);
+ }
return FSM_NEXT (CLAMP_WAIT, lower_clamp_rotation_success);
}
@@ -778,24 +957,49 @@ FSM_TRANS (CLAMP_WAIT,lower_clamp_rotation_failure, CLAMP_BLOCKED)
FSM_TRANS_TIMEOUT (CLAMP_TURN_BACKWARD,TIMEOUT_BLOCKED, CLAMP_TURN_FORWARD)
{
- move_needed(0,MEDIUM_ROTATION);
- if (ctx.clamp_1_down)
+ int sens;
+ if (ctx.cpt_blocked%2==0)
{
- /*Clamp 1 is now up (clamp 2 is down).*/
- ctx.clamp_1_down = 0;
+ sens=1;
}
else
{
- ctx.clamp_1_down = 1;
+ sens=-1;
}
+ move_needed2(CLAMP_1_DOWN,MEDIUM_ROTATION,sens);
return FSM_NEXT_TIMEOUT (CLAMP_TURN_BACKWARD);
}
-FSM_TRANS (CLAMP_TURN_FORWARD,lower_clamp_rotation_success, CLAMP_WAIT_BEFORE_IDLE)
+FSM_TRANS (CLAMP_TURN_FORWARD,lower_clamp_rotation_success,
+ back_to_idle,CLAMP_WAIT_BEFORE_IDLE,
+ back_to_empty_tree, CLAMP_BOTTOM_CLAMP_HIDE_POS)
{
- ctx.cpt_blocked = 0;
- return FSM_NEXT (CLAMP_TURN_FORWARD, lower_clamp_rotation_success);
+ IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE);
+ IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE);
+ if (ctx.unblocking_upper_set)
+ {
+ /*Hidding the clamp inside the robot*/
+ if (is_clamp_1_down(ctx.current_pos))
+ {
+ move_needed2(HIDE_POS_TREE,FAST_ROTATION,1);
+ }
+ else
+ {
+ move_needed2(HIDE_POS_TREE,FAST_ROTATION,1);
+ }
+ ctx.unblocking_upper_set = 0;
+ return FSM_NEXT (CLAMP_TURN_FORWARD, lower_clamp_rotation_success, back_to_empty_tree);
+ }
+ else
+ {
+ ctx.cpt_blocked = 0;
+ if (ctx.upper_set_is_dead)
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, upper_set_is_dead));
+ }
+ return FSM_NEXT (CLAMP_TURN_FORWARD, lower_clamp_rotation_success,back_to_idle);
+ }
}
@@ -811,5 +1015,59 @@ FSM_TRANS (CLAMP_TURN_FORWARD,lower_clamp_rotation_failure, CLAMP_BLOCKED)
ctx.cpt_blocked += 1;
return FSM_NEXT (CLAMP_TURN_FORWARD, lower_clamp_rotation_failure);
}
+/*---------------------------------------------------------------------------------*/
+/* Parts of the FSM that takes care of the upper set when it's not going down */
+/*---------------------------------------------------------------------------------*/
+FSM_TRANS_TIMEOUT (CLAMP_UPPER_SET_BLOCKED_UP, TIMEOUT_OPEN_CLAMPS,
+ branch_upper_set_is_dead, CLAMP_UPPER_SET_DEAD,
+ branch_try_to_deblock, CLAMP_UPPER_SET_FOLD_BACK)
+{
+ ctx.upper_set_blocked_cpt ++;
+ IO_CLR (OUTPUT_UPPER_CLAMP_DOWN);
+ IO_SET (OUTPUT_UPPER_CLAMP_UP);
+ if (ctx.upper_set_blocked_cpt>=2)
+ {
+ return FSM_NEXT_TIMEOUT (CLAMP_UPPER_SET_BLOCKED_UP, branch_upper_set_is_dead);
+ }
+ else
+ {
+ return FSM_NEXT_TIMEOUT (CLAMP_UPPER_SET_BLOCKED_UP,branch_try_to_deblock);
+ }
+}
+
+FSM_TRANS_TIMEOUT (CLAMP_UPPER_SET_DEAD,TIMEOUT_OPEN_CLAMPS, CLAMP_BLOCKED)
+{
+ ctx.upper_set_is_dead = 1;
+ return FSM_NEXT_TIMEOUT (CLAMP_UPPER_SET_DEAD);
+}
+
+FSM_TRANS (CLAMP_UPPER_SET_FOLD_BACK,upper_set_up, CLAMP_TRY_ROTATION)
+{
+ ctx.upper_set_is_dead = 1;
+ mimot_move_motor0_absolute (ctx.current_pos-4000, MEDIUM_ROTATION);
+ return FSM_NEXT (CLAMP_UPPER_SET_FOLD_BACK,upper_set_up);
+}
+
+FSM_TRANS (CLAMP_TRY_ROTATION,lower_clamp_rotation_success,CLAMP_BOTTOM_CLAMP_HIDE_POS)
+{
+ /*Hidding the clamp inside the robot*/
+ if (is_clamp_1_down(ctx.current_pos))
+ {
+ move_needed2(HIDE_POS_TREE,FAST_ROTATION,1);
+ }
+ else
+ {
+ move_needed2(HIDE_POS_TREE,FAST_ROTATION,1);
+ }
+ return FSM_NEXT (CLAMP_TRY_ROTATION,lower_clamp_rotation_success);
+}
+
+FSM_TRANS (CLAMP_TRY_ROTATION,lower_clamp_rotation_failure,CLAMP_BLOCKED)
+{
+ ctx.unblocking_upper_set = 1;
+ return FSM_NEXT (CLAMP_TRY_ROTATION,lower_clamp_rotation_failure);
+}
+
+
/* vim: set cino={0\:0t0(0 et: */