summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/ai/src/fsm/fsm.host.c6
-rw-r--r--digital/ai/src/twi_master/asserv.c13
-rw-r--r--digital/ai/src/twi_master/asserv.h4
-rw-r--r--digital/ai/src/twi_master/mimot.c16
-rw-r--r--digital/ai/src/twi_master/mimot.h8
-rw-r--r--digital/ai/src/utils/chrono.c11
-rw-r--r--digital/ai/src/utils/timer.avr.c10
-rw-r--r--digital/ai/src/utils/timer.h2
-rw-r--r--digital/ai/tools/test_simu_control_guybrush.py7
-rw-r--r--digital/asserv/src/asserv/traj.c26
-rw-r--r--digital/asserv/src/asserv/traj.h3
-rw-r--r--digital/asserv/src/asserv/twi_proto.c13
-rw-r--r--digital/avr/modules/motor/speed_control/speed_control.c8
-rw-r--r--digital/io-hub/src/guybrush/Makefile1
-rw-r--r--digital/io-hub/src/guybrush/avrconfig.h2
-rw-r--r--digital/io-hub/src/guybrush/bot.h10
-rw-r--r--digital/io-hub/src/guybrush/bottom_clamp.c532
-rw-r--r--digital/io-hub/src/guybrush/bottom_clamp.h2
-rw-r--r--digital/io-hub/src/guybrush/contact_defs.h4
-rw-r--r--digital/io-hub/src/guybrush/logger.avr.c134
-rw-r--r--digital/io-hub/src/guybrush/logger.h63
-rw-r--r--digital/io-hub/src/guybrush/main.c118
-rw-r--r--digital/io-hub/src/guybrush/move.c80
-rw-r--r--digital/io-hub/src/guybrush/path.c20
-rw-r--r--digital/io-hub/src/guybrush/playground_2012.h14
-rw-r--r--digital/io-hub/src/guybrush/pressure.c2
-rw-r--r--digital/io-hub/src/guybrush/radar_defs.c18
-rw-r--r--digital/io-hub/src/guybrush/radar_defs.h3
-rw-r--r--digital/io-hub/src/guybrush/strat.c152
-rw-r--r--digital/io-hub/src/guybrush/strat.h25
-rw-r--r--digital/io-hub/src/guybrush/top.c377
-rw-r--r--digital/io-hub/src/robospierre/clamp.c8
-rw-r--r--digital/io-hub/src/robospierre/logger.h30
-rw-r--r--digital/io-hub/src/robospierre/main.c4
-rw-r--r--digital/io-hub/tools/decode_fsm.py6
-rw-r--r--digital/io-hub/tools/io_hub/io_hub.py2
-rw-r--r--digital/io/src/logger.h30
-rw-r--r--digital/mimot/src/asserv/models.host.c2
-rw-r--r--digital/mimot/src/dirty/twi_proto.c7
39 files changed, 1468 insertions, 305 deletions
diff --git a/digital/ai/src/fsm/fsm.host.c b/digital/ai/src/fsm/fsm.host.c
index 391792e9..ad9d0b00 100644
--- a/digital/ai/src/fsm/fsm.host.c
+++ b/digital/ai/src/fsm/fsm.host.c
@@ -1143,7 +1143,10 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings)
/* Introduction. */
fprintf (f, "/* This file has been generated, do not edit. */\n\n");
+ fprintf (f, "#include \"common.h\"\n\n");
+ fprintf (f, "#include \"io.h\"\n\n");
fprintf (f, "#include \"fsm_%s_gen.h\"\n\n", fsm->name);
+ fprintf (f, "#include \"logger.h\"\n\n");
fprintf (f, "#include \"modules/proto/proto.h\"\n\n");
/* Gen state strings. */
@@ -1357,6 +1360,9 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings)
fsm->name,
fsm->name,
fsm->name);
+ fprintf (f, "\t\t\tlogger_log (old_state, e, "
+ "fsm_%s_active_states[i]);\n",
+ fsm->name);
fprintf (f, "\t\t\tproto_send3b ('F', old_state, e, "
"fsm_%s_active_states[i]);\n",
fsm->name);
diff --git a/digital/ai/src/twi_master/asserv.c b/digital/ai/src/twi_master/asserv.c
index ad36e00b..cff3587b 100644
--- a/digital/ai/src/twi_master/asserv.c
+++ b/digital/ai/src/twi_master/asserv.c
@@ -420,6 +420,19 @@ asserv_set_speed (uint16_t linear_high, uint16_t angular_high,
}
void
+asserv_set_acc (uint16_t linear, uint16_t angular)
+{
+ uint8_t *buffer = twi_master_get_buffer (TWI_MASTER_ID_ASSERV);
+ buffer[0] = 'p';
+ buffer[1] = 'a';
+ buffer[2] = v16_to_v8 (linear, 1);
+ buffer[3] = v16_to_v8 (linear, 0);
+ buffer[4] = v16_to_v8 (angular, 1);
+ buffer[5] = v16_to_v8 (angular, 0);
+ twi_master_send_buffer (6);
+}
+
+void
asserv_set_position (int32_t x, int32_t y, int16_t angle)
{
x = fixed_mul_f824 (x, asserv_scale_inv);
diff --git a/digital/ai/src/twi_master/asserv.h b/digital/ai/src/twi_master/asserv.h
index 1cbcb187..ac2ba146 100644
--- a/digital/ai/src/twi_master/asserv.h
+++ b/digital/ai/src/twi_master/asserv.h
@@ -271,6 +271,10 @@ void
asserv_set_speed (uint16_t linear_high, uint16_t angular_high,
uint16_t linear_low, uint16_t angular_low);
+/** Set acceleration. */
+void
+asserv_set_acc (uint16_t linear, uint16_t angular);
+
/**
* Set the complete position of the bot.
* This is an helpful function preventing you from calling multiples other
diff --git a/digital/ai/src/twi_master/mimot.c b/digital/ai/src/twi_master/mimot.c
index 6663f81f..ff73fdb7 100644
--- a/digital/ai/src/twi_master/mimot.c
+++ b/digital/ai/src/twi_master/mimot.c
@@ -240,21 +240,13 @@ mimot_motor1_clamp (int16_t speed, int16_t pwm)
}
void
-mimot_motor0_free (void)
+mimot_motor_free (uint8_t motor, uint8_t brake)
{
uint8_t *buffer = twi_master_get_buffer (TWI_MASTER_ID_MIMOT);
buffer[0] = 'w';
- buffer[1] = 0;
- twi_master_send_buffer (2);
-}
-
-void
-mimot_motor1_free (void)
-{
- uint8_t *buffer = twi_master_get_buffer (TWI_MASTER_ID_MIMOT);
- buffer[0] = 'w';
- buffer[1] = 1;
- twi_master_send_buffer (2);
+ buffer[1] = motor;
+ buffer[2] = brake;
+ twi_master_send_buffer (3);
}
void
diff --git a/digital/ai/src/twi_master/mimot.h b/digital/ai/src/twi_master/mimot.h
index 78806154..78f0a8c9 100644
--- a/digital/ai/src/twi_master/mimot.h
+++ b/digital/ai/src/twi_master/mimot.h
@@ -111,13 +111,9 @@ mimot_motor0_clamp (int16_t speed, int16_t pwm);
void
mimot_motor1_clamp (int16_t speed, int16_t pwm);
-/** Free motor0. */
+/** Free motor. */
void
-mimot_motor0_free (void);
-
-/** Free motor1. */
-void
-mimot_motor1_free (void);
+mimot_motor_free (uint8_t motor, uint8_t brake);
/** Set raw output for motor. */
void
diff --git a/digital/ai/src/utils/chrono.c b/digital/ai/src/utils/chrono.c
index 41e39b87..8560c3e0 100644
--- a/digital/ai/src/utils/chrono.c
+++ b/digital/ai/src/utils/chrono.c
@@ -44,7 +44,7 @@
static uint32_t chrono_tick_left_;
/** Last timer tick value. */
-static uint8_t chrono_last_tick_;
+static uint16_t chrono_last_tick_;
/** Is chrono started? */
static uint8_t chrono_started_;
@@ -62,8 +62,8 @@ chrono_update (void)
{
if (chrono_started_)
{
- uint8_t new_tick = timer_get_tick ();
- uint8_t diff = new_tick - chrono_last_tick_;
+ uint16_t new_tick = timer_get_tick ();
+ uint16_t diff = new_tick - chrono_last_tick_;
chrono_last_tick_ = new_tick;
if (diff > chrono_tick_left_)
chrono_tick_left_ = 0;
@@ -84,7 +84,10 @@ chrono_is_match_over (void)
uint32_t
chrono_remaining_time (void)
{
- return chrono_tick_left_ * TIMER_PERIOD_MS;
+ if (!chrono_started_)
+ return CHRONO_MATCH_DURATION_MS;
+ else
+ return chrono_tick_left_ * TIMER_PERIOD_MS;
}
void
diff --git a/digital/ai/src/utils/timer.avr.c b/digital/ai/src/utils/timer.avr.c
index d00278f8..9a6e3ede 100644
--- a/digital/ai/src/utils/timer.avr.c
+++ b/digital/ai/src/utils/timer.avr.c
@@ -33,7 +33,7 @@
static volatile uint8_t timer_overflow;
/** Incremented when timer overflowed. */
-static volatile uint8_t timer_tick;
+static volatile uint16_t timer_tick;
ISR (TIMER0_OVF_vect)
{
@@ -79,9 +79,13 @@ timer_wait (void)
return late;
}
-uint8_t
+uint16_t
timer_get_tick (void)
{
- return timer_tick;
+ uint16_t tick;
+ intr_flags_t flags = intr_lock ();
+ tick = timer_tick;
+ intr_restore (flags);
+ return tick;
}
diff --git a/digital/ai/src/utils/timer.h b/digital/ai/src/utils/timer.h
index dd71681f..0d597870 100644
--- a/digital/ai/src/utils/timer.h
+++ b/digital/ai/src/utils/timer.h
@@ -62,7 +62,7 @@ uint8_t
timer_wait (void);
/** Get a tick value, incremented at each tick, never reset. */
-uint8_t
+uint16_t
timer_get_tick (void);
#endif /* timer_h */
diff --git a/digital/ai/tools/test_simu_control_guybrush.py b/digital/ai/tools/test_simu_control_guybrush.py
index e895bf39..661e2140 100644
--- a/digital/ai/tools/test_simu_control_guybrush.py
+++ b/digital/ai/tools/test_simu_control_guybrush.py
@@ -97,6 +97,10 @@ class TestSimuControl (TestSimu):
text = 'Robot is back', padx = 0, pady = 0,
command = self.robot_is_back)
self.robot_is_back_button.pack()
+ self.unblock_button = Button (self.control_frame,
+ text = 'unblock bottom clamp', padx = 0, pady = 0,
+ command = self.unblock_bottom_clamp)
+ self.unblock_button.pack()
self.backward_var = IntVar ()
self.backward_button = Checkbutton (self.control_frame,
text = 'Backward', variable = self.backward_var)
@@ -146,5 +150,8 @@ class TestSimuControl (TestSimu):
def robot_is_back(self):
self.io.robot_is_back()
+ def unblock_bottom_clamp(self):
+ self.io.unblock_bottom_clamp()
+
if __name__ == '__main__':
run ('guybrush', TestSimuControl)
diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c
index 48802fcb..7f1be791 100644
--- a/digital/asserv/src/asserv/traj.c
+++ b/digital/asserv/src/asserv/traj.c
@@ -52,6 +52,8 @@
/** Traj mode enum. */
enum
{
+ /* Detect end of stop command. */
+ TRAJ_STOP,
/* Detect end of speed controled position control. */
TRAJ_SPEED,
/* Go to the wall. */
@@ -110,6 +112,27 @@ traj_init (void)
traj_set_angle_limit (traj_angle_limit);
}
+/** Wait for stop mode. */
+void
+traj_stop (void)
+{
+ if (cs_main.speed_theta.cur_f == 0 && cs_main.speed_alpha.cur_f == 0)
+ {
+ control_state_finished (&cs_main.state);
+ traj_mode = TRAJ_DONE;
+ }
+}
+
+/** Start stop mode. */
+void
+traj_stop_start (void)
+{
+ speed_control_set_speed (&cs_main.speed_theta, 0);
+ speed_control_set_speed (&cs_main.speed_alpha, 0);
+ control_state_set_mode (&cs_main.state, CS_MODE_TRAJ_CONTROL, 0);
+ traj_mode = TRAJ_STOP;
+}
+
/** Wait for zero speed mode. */
void
traj_speed (void)
@@ -447,6 +470,9 @@ traj_update (void)
{
switch (traj_mode)
{
+ case TRAJ_STOP:
+ traj_stop ();
+ break;
case TRAJ_SPEED:
traj_speed ();
break;
diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h
index a2f36cda..3e36b681 100644
--- a/digital/asserv/src/asserv/traj.h
+++ b/digital/asserv/src/asserv/traj.h
@@ -40,6 +40,9 @@ void
traj_update (void);
void
+traj_stop_start (void);
+
+void
traj_speed_start (void);
void
diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c
index 7c649e1c..cd79bf3e 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -138,9 +138,7 @@ twi_proto_callback (u8 *buf, u8 size)
break;
case c ('s', 0):
/* Stop (set zero speed). */
- speed_control_set_speed (&cs_main.speed_theta, 0);
- speed_control_set_speed (&cs_main.speed_alpha, 0);
- control_state_set_mode (&cs_main.state, CS_MODE_SPEED_CONTROL, 0);
+ traj_stop_start ();
break;
case c ('l', 3):
/* Set linear speed controlled position consign.
@@ -315,6 +313,15 @@ twi_proto_params (u8 *buf, u8 size)
cs_main.speed_alpha.slow = v8_to_v16 (buf[6], buf[7]);
eat = 4;
break;
+ case 'a':
+ /* Set acceleration.
+ * - w: theta acc.
+ * - w: alpha acc. */
+ if (size < 4)
+ return 1;
+ cs_main.speed_theta.acc_f = v8_to_v16 (buf[0], buf[1]);
+ cs_main.speed_alpha.acc_f = v8_to_v16 (buf[2], buf[3]);
+ break;
default:
return 1;
}
diff --git a/digital/avr/modules/motor/speed_control/speed_control.c b/digital/avr/modules/motor/speed_control/speed_control.c
index c13b4614..bdb3d375 100644
--- a/digital/avr/modules/motor/speed_control/speed_control.c
+++ b/digital/avr/modules/motor/speed_control/speed_control.c
@@ -58,13 +58,17 @@ speed_control_compute_max_speed_f (int32_t d, int32_t cur_f, int16_t acc_f,
int16_t max)
{
int16_t s;
- int32_t s_f;
+ int32_t d_abs, s_f;
/* Compute maximum speed in order to be able to brake in time.
* The "+ 0xff" is to ceil result.
* s = sqrt (2 * a * d) */
- s = fixed_sqrt_ui32 ((2 * UTILS_ABS (d) * acc_f + 0xff) >> 8);
+ d_abs = UTILS_ABS (d);
+ s = fixed_sqrt_ui32 ((2 * d_abs * acc_f + 0xff) >> 8);
/* Apply consign. */
s = UTILS_MIN (max, s);
+ /* Check it do not overshoot distance. */
+ if (s > d_abs)
+ s = d_abs;
/* Apply sign. */
if (d < 0)
s = -s;
diff --git a/digital/io-hub/src/guybrush/Makefile b/digital/io-hub/src/guybrush/Makefile
index 7db25b77..121a900c 100644
--- a/digital/io-hub/src/guybrush/Makefile
+++ b/digital/io-hub/src/guybrush/Makefile
@@ -6,6 +6,7 @@ PROGS = io_hub
io_hub_SOURCES = main.c top.c strat.c \
radar_defs.c radar.c path.c move.c \
pressure.c \
+ logger.avr.c \
init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \
contact.avr.c contact.host.c \
output.c output.host.c \
diff --git a/digital/io-hub/src/guybrush/avrconfig.h b/digital/io-hub/src/guybrush/avrconfig.h
index 110109b7..b21b25ad 100644
--- a/digital/io-hub/src/guybrush/avrconfig.h
+++ b/digital/io-hub/src/guybrush/avrconfig.h
@@ -136,7 +136,7 @@
/** Use mimot board. */
#define AC_AI_TWI_MASTER_MIMOT 1
/** Use beacon board. */
-#define AC_AI_TWI_MASTER_BEACON 1
+#define AC_AI_TWI_MASTER_BEACON 0
/* beacon. */
/** Number of beacon positions. */
diff --git a/digital/io-hub/src/guybrush/bot.h b/digital/io-hub/src/guybrush/bot.h
index aef18b3b..3d86f9f1 100644
--- a/digital/io-hub/src/guybrush/bot.h
+++ b/digital/io-hub/src/guybrush/bot.h
@@ -38,12 +38,18 @@
#define BOT_SIZE_FRONT 165
/** Distance from the robot axis to the lower clamp back. */
#define BOT_SIZE_LOWER_CLAMP_FRONT 100
+/** Distance from the robot axis to the maximum occupied space when
+ * loading. */
+#define BOT_SIZE_LOADING_FRONT 275
/** Distance from the robot axis to the back. */
-#define BOT_SIZE_BACK 134
+#define BOT_SIZE_BACK 135
/** Distance from the robot axis to the side. */
#define BOT_SIZE_SIDE 172
/** Maximum distance from the robot base center to one of its edge. */
#define BOT_SIZE_RADIUS 230
+/** Maximum distance from the robot base center to one of its edge, at the
+ * back. */
+#define BOT_SIZE_BACK_RADIUS 180
/** Distance between the front contact point and the robot center. */
#define BOT_BACK_CONTACT_DIST_MM BOT_SIZE_BACK
@@ -59,6 +65,6 @@
/** Normal cruise speed. */
#define BOT_SPEED_NORMAL 0x50, 0x60, 0x20, 0x20
/** Approach speed. */
-#define BOT_SPEED_APPROACH 0x20, 0x30, 0x20, 0x20
+#define BOT_SPEED_APPROACH 0x20, 0x30, 0x10, 0x20
#endif /* bot_h */
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: */
diff --git a/digital/io-hub/src/guybrush/bottom_clamp.h b/digital/io-hub/src/guybrush/bottom_clamp.h
index 240b1579..35879c80 100644
--- a/digital/io-hub/src/guybrush/bottom_clamp.h
+++ b/digital/io-hub/src/guybrush/bottom_clamp.h
@@ -28,6 +28,8 @@
/*function to put the clamp in the "clam_mode" (i.e putting the global var ctx.calm_mode at 1)*/
void clamp_calm_mode(int mode);
+uint8_t clamp_calm_mode_read(void);
+
/*function to find out how many times the clamp has been blocked*/
uint8_t clamp_read_blocked_cpt(void);
diff --git a/digital/io-hub/src/guybrush/contact_defs.h b/digital/io-hub/src/guybrush/contact_defs.h
index e28e7b1a..cd922ea2 100644
--- a/digital/io-hub/src/guybrush/contact_defs.h
+++ b/digital/io-hub/src/guybrush/contact_defs.h
@@ -37,6 +37,7 @@
#define CONTACT_GOLD_BAR E, 6
#define CONTACT_TOTEM E, 7
#define CONTACT_LOWER_CLAMP_ZERO D, 7
+#define CONTACT_NB_ROBOTS A, 6
#define CONTACT_LIST \
CONTACT (CONTACT_STRAT, 1) \
@@ -48,6 +49,7 @@
CONTACT (CONTACT_DOOR_CLOSE, 1) \
CONTACT (CONTACT_GOLD_BAR, 1) \
CONTACT (CONTACT_TOTEM, 1) \
- CONTACT (CONTACT_LOWER_CLAMP_ZERO, 0)
+ CONTACT (CONTACT_LOWER_CLAMP_ZERO, 0) \
+ CONTACT (CONTACT_NB_ROBOTS, 1)
#endif /* contact_defs_h */
diff --git a/digital/io-hub/src/guybrush/logger.avr.c b/digital/io-hub/src/guybrush/logger.avr.c
new file mode 100644
index 00000000..3e069bff
--- /dev/null
+++ b/digital/io-hub/src/guybrush/logger.avr.c
@@ -0,0 +1,134 @@
+/* logger.avr.c */
+/* guybrush - Eurobot 2012 AI. {{{
+ *
+ * Copyright (C) 2012 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+#include "common.h"
+#include "logger.h"
+
+#include <avr/eeprom.h>
+
+/** Record log in EEPROM, bufferize data not to disturb real time
+ * constrains.
+ *
+ * Special bytes:
+ * 0xf0 - 0xfd: event counter, to be used for synchronisation.
+ * 0xfe: escape character.
+ * 0xff: overflow.
+ * */
+
+/** Maximum buffer size. */
+#define LOGGER_BUF_SIZE 32
+
+/** Maximum log size. */
+#define LOGGER_SIZE (E2END + 1)
+
+/** Code writen on overflow. */
+#define LOGGER_OVERFLOW 0xff
+
+/** Code writen to escape characters. */
+#define LOGGER_ESCAPE 0xfe
+
+/** Special code mask. */
+#define LOGGER_SPECIAL 0xf0
+
+/** Event counter rollover value. */
+#define LOGGER_EVENT_COUNTER_NB 14
+
+/** Logger context. */
+struct logger_t
+{
+ /** Log buffer. */
+ uint8_t buf[LOGGER_BUF_SIZE];
+ /** Head and tail in buffer. */
+ uint8_t head, tail;
+ /** Current log pointer. */
+ uint16_t current;
+ /** Event counter, incremented at each event, used for synchronisation. */
+ uint8_t event_counter;
+} logger;
+
+EEMEM uint8_t logger_eeprom[LOGGER_SIZE];
+
+void
+logger_init (void)
+{
+ /* Read last log event counter and choose another value. */
+ logger.event_counter = eeprom_read_byte (&logger_eeprom[0]);
+ logger.event_counter = (logger.event_counter + 1) %
+ LOGGER_EVENT_COUNTER_NB;
+}
+
+void
+logger_update (void)
+{
+ if (eeprom_is_ready () && logger.head != logger.tail
+ && logger.current < LOGGER_SIZE)
+ {
+ /* Write next byte. */
+ uint8_t next = logger.buf[logger.head];
+ eeprom_write_byte (&logger_eeprom[logger.current++], next);
+ logger.head = (logger.head + 1) % LOGGER_BUF_SIZE;
+ }
+}
+
+static void
+logger_write_internal (uint8_t b)
+{
+ uint8_t tail_new = (logger.tail + 1) % LOGGER_BUF_SIZE;
+ if (tail_new == logger.head)
+ {
+ /* Overflow! */
+ logger.buf[logger.tail] = LOGGER_OVERFLOW;
+ }
+ else
+ {
+ logger.tail = tail_new;
+ logger.buf[tail_new] = b;
+ }
+}
+
+void
+logger_write_event_counter (void)
+{
+ logger_write_internal (LOGGER_SPECIAL | logger.event_counter);
+ logger.event_counter = (logger.event_counter + 1) %
+ LOGGER_EVENT_COUNTER_NB;
+}
+
+void
+logger_write (uint8_t b)
+{
+ if ((b & LOGGER_SPECIAL) == LOGGER_SPECIAL)
+ {
+ uint8_t tail_after = (logger.tail + 2) % LOGGER_BUF_SIZE;
+ if (tail_after == logger.head)
+ logger_write_internal (LOGGER_OVERFLOW);
+ else
+ {
+ logger_write_internal (LOGGER_ESCAPE);
+ logger_write_internal (b);
+ }
+ }
+ else
+ logger_write_internal (b);
+}
diff --git a/digital/io-hub/src/guybrush/logger.h b/digital/io-hub/src/guybrush/logger.h
new file mode 100644
index 00000000..e672ab71
--- /dev/null
+++ b/digital/io-hub/src/guybrush/logger.h
@@ -0,0 +1,63 @@
+#ifndef logger_h
+#define logger_h
+/* logger.h */
+/* guybrush - Eurobot 2012 AI. {{{
+ *
+ * Copyright (C) 2012 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+
+/** Log a serie of bytes. */
+#define logger_log(args...) \
+ do { \
+ logger_write_event_counter (); \
+ PREPROC_FOR (logger_log_, ## args) \
+ } while (0)
+#define logger_log_(b) logger_write (b);
+
+#ifndef HOST
+
+/** Initialise logger, do not write anything yet. */
+void
+logger_init (void);
+
+/** Write pending information. */
+void
+logger_update (void);
+
+/** Write event counter for synchronisation. */
+void
+logger_write_event_counter (void);
+
+/** Write one byte in buffer. */
+void
+logger_write (uint8_t b);
+
+#else
+
+#define logger_init()
+#define logger_update()
+#define logger_write_event_counter()
+#define logger_write(b)
+
+#endif
+
+#endif /* logger_h */
diff --git a/digital/io-hub/src/guybrush/main.c b/digital/io-hub/src/guybrush/main.c
index 63366c13..49172984 100644
--- a/digital/io-hub/src/guybrush/main.c
+++ b/digital/io-hub/src/guybrush/main.c
@@ -43,6 +43,7 @@
#include "output.h"
#include "radar.h"
#include "pressure.h"
+#include "logger.h"
#define FSM_NAME AI
#include "fsm.h"
@@ -54,8 +55,10 @@
#include "path.h"
#include "move.h"
#include "top.h"
+#include "bottom_clamp.h"
#include "bot.h"
+#include "playground_2012.h"
#include "io.h"
@@ -105,6 +108,9 @@ static uint8_t main_stats_usdist_, main_stats_usdist_cpt_;
/** Pressure stats counters. */
static uint8_t main_stats_pressure_, main_stats_pressure_cpt_;
+/** Chrono stats. */
+static uint8_t main_stats_chrono_, main_stats_chrono_last_s_;
+
/** Clamp zero stats. */
static uint8_t main_stats_clamp_zero_;
@@ -137,6 +143,7 @@ main_init (void)
contact_init ();
output_init ();
usdist_init ();
+ logger_init ();
/* Send some debug aid in case of TWI synchronisation failure.
* If !Z is sent, but not !z, this means that a slave is missing or is not
* functioning. */
@@ -154,6 +161,86 @@ main_init (void)
proto_send0 ('z');
}
+/** Generate demo mode events. */
+static uint8_t
+main_demo_events (void)
+{
+ static uint8_t color_switch_last = 0xff;
+ static uint8_t nb_robots_switch_last = 0xff;
+ static uint8_t tree_step;
+ static uint8_t sleep;
+ /* Bounce detection. */
+ if (sleep)
+ {
+ sleep--;
+ return 0;
+ }
+ /* Look at color switch to control totem picking code. */
+ if (color_switch_last == 0xff)
+ color_switch_last = contact_get_color ();
+ if (color_switch_last != contact_get_color ())
+ {
+ color_switch_last = contact_get_color ();
+ sleep = 125;
+ switch (tree_step)
+ {
+ case 0:
+ clamp_request (FSM_EVENT (AI, tree_detected));
+ break;
+ case 1:
+ clamp_request (FSM_EVENT (AI, empty_tree));
+ break;
+ case 2:
+ clamp_request (FSM_EVENT (AI, robot_is_back));
+ break;
+ }
+ tree_step = (tree_step + 1) % 3;
+ return 1;
+ }
+ /* Look at nb robots switch to control unblocking. */
+ if (nb_robots_switch_last == 0xff)
+ nb_robots_switch_last = IO_GET (CONTACT_NB_ROBOTS);
+ if (nb_robots_switch_last != IO_GET (CONTACT_NB_ROBOTS))
+ {
+ nb_robots_switch_last = IO_GET (CONTACT_NB_ROBOTS);
+ sleep = 125;
+ return FSM_HANDLE (AI, clamp_unblock);
+ }
+ return 0;
+}
+
+static uint8_t
+main_coin_detected_ok (void)
+{
+ position_t robot_pos;
+ asserv_get_position (&robot_pos);
+ int16_t limit = robot_pos.v.y < PG_LENGTH - PG_CAPTAIN_ROOM_LENGTH_MM
+ ? PG_HOLD_NORTH_X + BOT_SIZE_FRONT
+ : PG_CAPTAIN_ROOM_LENGTH_MM + BOT_SIZE_FRONT;
+ if (main_demo)
+ return 1;
+ if (robot_pos.v.x < limit || robot_pos.v.x > PG_MIRROR_X (limit))
+ return 0;
+ if (robot_pos.v.y > PG_COIN_QUARTET_Y - 100
+ && robot_pos.v.y < PG_COIN_QUARTET_Y + 100)
+ {
+ if (robot_pos.a < POSITION_A_DEG (90)
+ || robot_pos.a > POSITION_A_DEG (270))
+ {
+ if (robot_pos.v.x > PG_COIN_QUARTET_X - BOT_SIZE_FRONT - 170
+ && robot_pos.v.x < PG_COIN_QUARTET_X - BOT_SIZE_FRONT + 170)
+ return 0;
+ }
+ else
+ {
+ if (robot_pos.v.x > PG_COIN_QUARTET_X + BOT_SIZE_FRONT - 170
+ && robot_pos.v.x < PG_COIN_QUARTET_X + BOT_SIZE_FRONT + 170)
+ return 0;
+ }
+ }
+ return 1;
+}
+
/** Main events management. */
uint8_t
main_event_to_fsm (void)
@@ -182,8 +269,10 @@ main_event_to_fsm (void)
FSM_HANDLE_E (AI, lower_clamp_rotation_success);
else if (mimot_motor0_status == failure)
FSM_HANDLE_E (AI, lower_clamp_rotation_failure);
- if (IO_GET (CONTACT_LOWER_CLAMP_SENSOR_1)
+ if ((IO_GET (CONTACT_LOWER_CLAMP_SENSOR_1)
|| IO_GET (CONTACT_LOWER_CLAMP_SENSOR_2))
+ && !clamp_calm_mode_read ()
+ && main_coin_detected_ok ())
FSM_HANDLE_E (AI, coin_detected);
if ((int16_t) (mimot_get_motor0_position() - position_to_drop) > 0)
FSM_HANDLE_E (AI, time_to_drop_coin);
@@ -198,6 +287,9 @@ main_event_to_fsm (void)
FSM_HANDLE_E (AI,upper_set_down);
if (!IO_GET(CONTACT_UPPER_CLAMP_UP))
FSM_HANDLE_E (AI,upper_set_up);
+ /* Demo mode. */
+ if (main_demo && main_demo_events ())
+ return 1;
/* Events from the event queue. */
if (fsm_queue_poll ())
{
@@ -228,6 +320,7 @@ main_loop (void)
/* Is match over? */
if (chrono_is_match_over ())
{
+
/* Open everything. */
IO_SET (OUTPUT_UPPER_CLAMP_OPEN);
IO_CLR (OUTPUT_LOWER_CLAMP_1_CLOSE);
@@ -235,8 +328,8 @@ main_loop (void)
IO_SET (OUTPUT_DOOR_OPEN);
IO_CLR (OUTPUT_DOOR_CLOSE);
/* Stop motors. */
- mimot_motor0_free ();
- mimot_motor1_free ();
+ mimot_motor_free (0, 0);
+ mimot_motor_free (1, 0);
/* End it and block here indefinitely. */
chrono_end_match (42);
return;
@@ -255,6 +348,7 @@ main_loop (void)
simu_send_pos_report (main_obstacles_pos, main_obstacles_nb, 0);
}
pressure_update ();
+ logger_update ();
/* Update AI modules. */
top_update ();
path_decay ();
@@ -292,6 +386,12 @@ main_loop (void)
proto_send1w ('F', pressure_get ());
main_stats_pressure_cpt_ = main_stats_pressure_;
}
+ if (main_stats_chrono_
+ && main_stats_chrono_last_s_ != chrono_remaining_time () / 1000)
+ {
+ main_stats_chrono_last_s_ = chrono_remaining_time () / 1000;
+ proto_send1b ('C', main_stats_chrono_last_s_);
+ }
if (main_stats_clamp_zero_
&& IO_GET (CONTACT_LOWER_CLAMP_ZERO)
!= main_stats_clamp_zero_last_io_)
@@ -343,6 +443,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Simulate the robot_is_back command. */
fsm_queue_post_event (FSM_EVENT (AI, robot_is_back));
break;
+ case c ('u',0):
+ /* Simulate the unblock command. */
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_unblock));
+ break;
case c ('c', 1):
/* Simulate clean_start (00), clean_catch (01), clean_load (02). */
if (args[0] == 0)
@@ -413,6 +517,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Pressure stats. */
main_stats_pressure_ = main_stats_pressure_cpt_ = args[0];
break;
+ case c ('C', 1):
+ /* Chrono stats.
+ * - b: start chrono. */
+ main_stats_chrono_ = 1;
+ main_stats_chrono_last_s_ = 0;
+ if (args[0])
+ chrono_start ();
+ break;
case c ('Z', 1):
/* Clamp zero stat. */
main_stats_clamp_zero_ = args[0];
diff --git a/digital/io-hub/src/guybrush/move.c b/digital/io-hub/src/guybrush/move.c
index 3c4c7f46..056ffeb2 100644
--- a/digital/io-hub/src/guybrush/move.c
+++ b/digital/io-hub/src/guybrush/move.c
@@ -36,9 +36,15 @@
#include "path.h"
#include "modules/utils/utils.h"
+#include "modules/proto/proto.h"
#include <math.h>
+#include "playground_2012.h"
+
+/** Number of tries to reach destination. */
+#define MOVE_TRY_AGAIN 1
+
/** Move context. */
struct move_t
{
@@ -80,7 +86,7 @@ move_start (position_t position, uint8_t backward)
move_data.final_move = 0;
move_data.shorten = 0;
/* Reset try counter. */
- move_data.try_again_counter = 3;
+ move_data.try_again_counter = MOVE_TRY_AGAIN;
/* Start the FSM. */
fsm_queue_post_event (FSM_EVENT (AI, move_start));
}
@@ -95,7 +101,7 @@ move_start_noangle (vect_t position, uint8_t backward, int16_t shorten)
move_data.final_move = 0;
move_data.shorten = shorten;
/* Reset try counter. */
- move_data.try_again_counter = 3;
+ move_data.try_again_counter = MOVE_TRY_AGAIN;
/* Start the FSM. */
fsm_queue_post_event (FSM_EVENT (AI, move_start));
}
@@ -112,8 +118,12 @@ move_obstacles_update (void)
{
uint8_t i;
for (i = 0; i < main_obstacles_nb; i++)
+ {
path_obstacle (i, main_obstacles_pos[i], MOVE_OBSTACLE_RADIUS, 0,
MOVE_OBSTACLE_VALIDITY);
+ proto_send2w ('o', main_obstacles_pos[i].x,
+ main_obstacles_pos[i].y);
+ }
}
uint8_t
@@ -138,6 +148,8 @@ FSM_STATES (
MOVE_ROTATING,
/* Moving to a position (intermediate or final). */
MOVE_MOVING,
+ /* Brake when a obstacle is seen. */
+ MOVE_BRAKE,
/* Moving backward to go away from what is blocking the bot. */
MOVE_MOVING_BACKWARD_TO_TURN_FREELY,
/* Waiting for obstacle to disappear. */
@@ -375,24 +387,50 @@ FSM_TRANS (MOVE_MOVING, robot_move_success,
//return FSM_NEXT (MOVE_MOVING, robot_move_success, no_path_found);
}
+/** Test if a point is ok to move back. */
+static uint8_t
+move_test_point (vect_t p, int16_t margin)
+{
+ return p.x >= margin && p.x < PG_WIDTH - margin
+ && p.y >= margin && p.y < PG_LENGTH - margin
+ && !(p.y >= PG_TOTEM_Y - PG_TOTEM_WIDTH_MM / 2 - margin
+ && p.y < PG_TOTEM_Y + PG_TOTEM_WIDTH_MM / 2 + margin
+ && p.x >= PG_TOTEM_LEFT_X - PG_TOTEM_WIDTH_MM / 2 - margin
+ && p.x < PG_TOTEM_RIGHT_X + PG_TOTEM_WIDTH_MM / 2 + margin);
+}
+
static void
move_moving_backward_to_turn_freely (void)
{
move_data.final_move = 0;
+ int16_t dist, back_dist, margin;
+ if (asserv_get_last_moving_direction () == DIRECTION_FORWARD)
+ {
+ dist = BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS;
+ back_dist = -300;
+ margin = BOT_SIZE_BACK_RADIUS;
+ }
+ else
+ {
+ dist = -(BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS);
+ back_dist = 300;
+ margin = BOT_SIZE_RADIUS;
+ }
/* Assume there is an obstacle in front of the robot. */
position_t robot_pos;
asserv_get_position (&robot_pos);
vect_t obstacle_pos;
- int16_t dist = asserv_get_last_moving_direction () == DIRECTION_FORWARD
- ? BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS
- : -(BOT_SIZE_BACK + MOVE_REAL_OBSTACLE_RADIUS);
vect_from_polar_uf016 (&obstacle_pos, dist, robot_pos.a);
vect_translate (&obstacle_pos, &robot_pos.v);
path_obstacle (0, obstacle_pos, MOVE_OBSTACLE_RADIUS, 0,
MOVE_OBSTACLE_VALIDITY);
/* Move backward to turn freely. */
- asserv_move_linearly (asserv_get_last_moving_direction ()
- == DIRECTION_FORWARD ? -300 : 300);
+ vect_t back_pos;
+ vect_from_polar_uf016 (&back_pos, back_dist, robot_pos.a);
+ vect_translate (&back_pos, &robot_pos.v);
+ if (!move_test_point (back_pos, margin))
+ back_dist /= 8;
+ asserv_move_linearly (back_dist);
}
FSM_TRANS (MOVE_MOVING,
@@ -410,19 +448,37 @@ FSM_TRANS_TIMEOUT (MOVE_MOVING, 2500,
return FSM_NEXT_TIMEOUT (MOVE_MOVING);
}
-FSM_TRANS (MOVE_MOVING, obstacle_in_front,
- tryagain, MOVE_WAIT_FOR_CLEAR_PATH,
- tryout, MOVE_IDLE)
+FSM_TRANS (MOVE_MOVING, obstacle_in_front, MOVE_BRAKE)
{
move_data.final_move = 0;
asserv_stop_motor ();
+ return FSM_NEXT (MOVE_MOVING, obstacle_in_front);
+}
+
+FSM_TRANS (MOVE_BRAKE, robot_move_success,
+ tryagain, MOVE_WAIT_FOR_CLEAR_PATH,
+ tryout, MOVE_IDLE)
+{
+ if (--move_data.try_again_counter == 0)
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_failure));
+ return FSM_NEXT (MOVE_BRAKE, robot_move_success, tryout);
+ }
+ else
+ return FSM_NEXT (MOVE_BRAKE, robot_move_success, tryagain);
+}
+
+FSM_TRANS (MOVE_BRAKE, robot_move_failure,
+ tryagain, MOVE_WAIT_FOR_CLEAR_PATH,
+ tryout, MOVE_IDLE)
+{
if (--move_data.try_again_counter == 0)
{
fsm_queue_post_event (FSM_EVENT (AI, move_failure));
- return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryout);
+ return FSM_NEXT (MOVE_BRAKE, robot_move_failure, tryout);
}
else
- return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryagain);
+ return FSM_NEXT (MOVE_BRAKE, robot_move_failure, tryagain);
}
FSM_TRANS (MOVE_MOVING, move_stop, MOVE_IDLE)
diff --git a/digital/io-hub/src/guybrush/path.c b/digital/io-hub/src/guybrush/path.c
index fab40c49..cf55faf3 100644
--- a/digital/io-hub/src/guybrush/path.c
+++ b/digital/io-hub/src/guybrush/path.c
@@ -64,7 +64,7 @@
#define PATH_GRID_NODES_NB (PATH_COLUMNS_NB * PATH_COLUMN_NODES_NB)
/** Number of extra nodes not in the grid. */
-#define PATH_EXTRA_NODES_NB 4
+#define PATH_EXTRA_NODES_NB 6
/** Number of fixed nodes. */
#define PATH_FIXED_NODES_NB (PATH_GRID_NODES_NB + PATH_EXTRA_NODES_NB)
@@ -118,9 +118,9 @@ static const struct path_node_t path_nodes[PATH_NODES_NB] = {
{ 1 }, /* 6 */
{ 1 }, /* 7 column 1. */
{ 1 }, /* 8 */
- { 2 }, /* 9 avoid */
+ { 1 }, /* 9 */
{ 0 }, /* 10 */
- { 2 }, /* 11 avoid */
+ { 1 }, /* 11 */
{ 1 }, /* 12 */
{ 1 }, /* 13 */
{ 1 }, /* 14 column 2. */
@@ -132,9 +132,9 @@ static const struct path_node_t path_nodes[PATH_NODES_NB] = {
{ 1 }, /* 20 */
{ 1 }, /* 21 column 3. */
{ 1 }, /* 22 */
- { 2 }, /* 23 avoid */
+ { 1 }, /* 23 */
{ 0 }, /* 24 */
- { 2 }, /* 25 avoid */
+ { 1 }, /* 25 */
{ 1 }, /* 26 */
{ 1 }, /* 27 */
{ 1 }, /* 28 column 4. */
@@ -148,8 +148,10 @@ static const struct path_node_t path_nodes[PATH_NODES_NB] = {
{ 2 }, /* 36 */
{ 2 }, /* 37 */
{ 2 }, /* 38 */
- { 1 }, /* 39 dst node. */
- { 1 }, /* 40 src node. */
+ { 2 }, /* 39 */
+ { 2 }, /* 40 */
+ { 1 }, /* 41 dst node. */
+ { 1 }, /* 42 src node. */
/* }}} */
};
@@ -183,6 +185,10 @@ static const vect_t path_extra_nodes[PATH_EXTRA_NODES_NB] = {
PG_TOTEM_Y - PATH_TOTEM_CLEAR_MM / 2 },
{ PG_TOTEM_RIGHT_X + PATH_TOTEM_CLEAR_MM,
PG_TOTEM_Y + PATH_TOTEM_CLEAR_MM / 2 },
+ { BOT_SIZE_RADIUS + 30,
+ PG_TOTEM_Y + PATH_TOTEM_CLEAR_MM / 2 },
+ { PG_MIRROR_X (BOT_SIZE_RADIUS + 30),
+ PG_TOTEM_Y + PATH_TOTEM_CLEAR_MM / 2 },
};
/** Compute position of a node. */
diff --git a/digital/io-hub/src/guybrush/playground_2012.h b/digital/io-hub/src/guybrush/playground_2012.h
index 6beef7a2..571ffc09 100644
--- a/digital/io-hub/src/guybrush/playground_2012.h
+++ b/digital/io-hub/src/guybrush/playground_2012.h
@@ -56,10 +56,14 @@
#define PG_CAPTAIN_ROOM_WIDTH_MM 400
#define PG_CAPTAIN_ROOM_LENGTH_MM 500
-/** Bottles, align on path grid. */
-#define PG_BOTTLE0_X (PG_TOTEM_LEFT_X - PATH_TOTEM_CLEAR_MM)
-#define PG_BOTTLE1_X PG_TOTEM_LEFT_X
-#define PG_BOTTLE2_X PG_TOTEM_RIGHT_X
-#define PG_BOTTLE3_X (PG_TOTEM_RIGHT_X + PATH_TOTEM_CLEAR_MM)
+/** Bottles. */
+#define PG_BOTTLE0_X 640
+#define PG_BOTTLE1_X (640 + 477)
+#define PG_BOTTLE2_X PG_MIRROR_X (PG_BOTTLE1_X)
+#define PG_BOTTLE3_X PG_MIRROR_X (PG_BOTTLE0_X)
+
+/** 4 coins. */
+#define PG_COIN_QUARTET_X (PG_WIDTH / 2)
+#define PG_COIN_QUARTET_Y 300
#endif /* playground_2012_h */
diff --git a/digital/io-hub/src/guybrush/pressure.c b/digital/io-hub/src/guybrush/pressure.c
index 9fa909b0..7e0be308 100644
--- a/digital/io-hub/src/guybrush/pressure.c
+++ b/digital/io-hub/src/guybrush/pressure.c
@@ -119,7 +119,7 @@ pressure_update (void)
if (pressure_get () > pressure.high)
{
/* Done. Stop. Open circuit. */
- mimot_motor1_free ();
+ mimot_motor_free (1, 0);
IO_SET (OUTPUT_PNEUM_OPEN);
pressure.state = PRESSURE_IDLE;
pressure.wait = PRESSURE_REST;
diff --git a/digital/io-hub/src/guybrush/radar_defs.c b/digital/io-hub/src/guybrush/radar_defs.c
index d344f958..361438e7 100644
--- a/digital/io-hub/src/guybrush/radar_defs.c
+++ b/digital/io-hub/src/guybrush/radar_defs.c
@@ -31,19 +31,24 @@
/** Define radar configuration. */
struct radar_sensor_t radar_sensors[RADAR_SENSOR_NB] = {
#define RADAR_SENSOR_FRONT 0
- { &usdist_mm[0], { 120, 0 }, G_ANGLE_UF016_DEG (0) },
+ { &usdist_mm[0], { 150, 0 }, G_ANGLE_UF016_DEG (0) },
#define RADAR_SENSOR_LEFT 1
- { &usdist_mm[1], { 120, 160 }, G_ANGLE_UF016_DEG (0) },
+ { &usdist_mm[1], { 50, 147 }, G_ANGLE_UF016_DEG (0) },
#define RADAR_SENSOR_RIGHT 2
- { &usdist_mm[2], { 120, -160 }, G_ANGLE_UF016_DEG (0) },
+ { &usdist_mm[2], { 50, -147 }, G_ANGLE_UF016_DEG (0) },
#define RADAR_SENSOR_BACK 3
- { &usdist_mm[3], { -130, 0 }, G_ANGLE_UF016_DEG (180) },
+ { &usdist_mm[3], { -120, 0 }, G_ANGLE_UF016_DEG (180) },
};
+/** Is radar currently moving? */
+uint8_t radar_def_upper_clamp_moving_;
+
/** Define exclusion area (considered as invalid point). */
uint8_t
radar_valid (vect_t p, uint8_t sensor)
{
+ if (radar_def_upper_clamp_moving_ && sensor != RADAR_SENSOR_BACK)
+ return 0;
return p.x >= RADAR_MARGIN_MM && p.x < PG_WIDTH - RADAR_MARGIN_MM
&& p.y >= RADAR_MARGIN_MM && p.y < PG_LENGTH - RADAR_MARGIN_MM
&& (sensor == RADAR_SENSOR_FRONT || sensor == RADAR_SENSOR_BACK
@@ -55,3 +60,8 @@ radar_valid (vect_t p, uint8_t sensor)
&& p.x < PG_TOTEM_RIGHT_X + PG_TOTEM_WIDTH_MM / 2 + RADAR_MARGIN_MM))));
}
+void
+radar_def_upper_clamp_moving (uint8_t moving)
+{
+ radar_def_upper_clamp_moving_ = moving;
+}
diff --git a/digital/io-hub/src/guybrush/radar_defs.h b/digital/io-hub/src/guybrush/radar_defs.h
index 83f74407..8bb683ee 100644
--- a/digital/io-hub/src/guybrush/radar_defs.h
+++ b/digital/io-hub/src/guybrush/radar_defs.h
@@ -38,4 +38,7 @@
#define RADAR_SENSOR_BACK_FIRST 3
#define RADAR_SENSOR_BACK_NB 1
+void
+radar_def_upper_clamp_moving (uint8_t moving);
+
#endif /* radar_defs_h */
diff --git a/digital/io-hub/src/guybrush/strat.c b/digital/io-hub/src/guybrush/strat.c
index 0c7de8f6..fe029f4d 100644
--- a/digital/io-hub/src/guybrush/strat.c
+++ b/digital/io-hub/src/guybrush/strat.c
@@ -25,11 +25,13 @@
#include "common.h"
#include "strat.h"
+#include "contact.h"
#include "playground_2012.h"
#include "bot.h"
#include "path.h"
#include "asserv.h"
+#include "chrono.h"
/*
* This file implements strategic decisions.
@@ -37,6 +39,11 @@
enum
{
+ /** Unloading area. */
+ STRAT_PLACE_CAPTAIN0,
+ STRAT_PLACE_CAPTAIN1,
+ STRAT_PLACE_HOLD0,
+ STRAT_PLACE_HOLD1,
/** The four collecting places for totems. */
STRAT_PLACE_TOTEM0,
STRAT_PLACE_TOTEM1,
@@ -48,7 +55,9 @@ enum
STRAT_PLACE_BOTTLE2,
STRAT_PLACE_BOTTLE3,
/** Number of places, should be last. */
- STRAT_PLACE_NB
+ STRAT_PLACE_NB,
+ /** Number of unloading area. */
+ STRAT_PLACE_UNLOAD_NB = 4,
};
/** Place static information. */
@@ -57,23 +66,35 @@ struct strat_place_t
/** Collect position. */
vect_t pos;
/** Static score. */
- uint8_t score;
+ int16_t score;
/** Decision code. */
uint8_t decision;
};
static const struct strat_place_t strat_place[STRAT_PLACE_NB] = {
+ { { PG_CAPTAIN_ROOM_LENGTH_MM,
+ PG_LENGTH - PG_CAPTAIN_ROOM_LENGTH_MM / 2 },
+ 0, STRAT_DECISION_UNLOAD },
+ { { PG_MIRROR_X (PG_CAPTAIN_ROOM_LENGTH_MM),
+ PG_LENGTH - PG_CAPTAIN_ROOM_LENGTH_MM / 2 },
+ 0, STRAT_DECISION_UNLOAD },
+ { { (PG_CAPTAIN_ROOM_WIDTH_MM + PG_HOLD_NORTH_X) / 2,
+ (PG_LENGTH - PG_CAPTAIN_ROOM_LENGTH_MM + PG_HOLD_NORTH_Y) / 2 - 75 },
+ 1500, STRAT_DECISION_UNLOAD },
+ { { PG_MIRROR_X ((PG_CAPTAIN_ROOM_WIDTH_MM + PG_HOLD_NORTH_X) / 2),
+ (PG_LENGTH - PG_CAPTAIN_ROOM_LENGTH_MM + PG_HOLD_NORTH_Y) / 2 - 75 },
+ 1500, STRAT_DECISION_UNLOAD },
{ { PG_WIDTH / 2 - PG_TOTEM_X_OFFSET_MM,
- PG_LENGTH / 2 + PATH_PEANUT_CLEAR_MM }, 100, STRAT_DECISION_TOTEM },
+ PG_LENGTH / 2 + PATH_PEANUT_CLEAR_MM }, 1000, STRAT_DECISION_TOTEM },
{ { PG_WIDTH / 2 + PG_TOTEM_X_OFFSET_MM,
- PG_LENGTH / 2 + PATH_PEANUT_CLEAR_MM }, 100, STRAT_DECISION_TOTEM },
+ PG_LENGTH / 2 + PATH_PEANUT_CLEAR_MM }, 1000, STRAT_DECISION_TOTEM },
{ { PG_WIDTH / 2 - PG_TOTEM_X_OFFSET_MM,
PG_LENGTH / 2 - PATH_PEANUT_CLEAR_MM }, 100, STRAT_DECISION_TOTEM },
{ { PG_WIDTH / 2 + PG_TOTEM_X_OFFSET_MM,
PG_LENGTH / 2 - PATH_PEANUT_CLEAR_MM }, 100, STRAT_DECISION_TOTEM },
- { { PG_BOTTLE0_X, BOT_SIZE_RADIUS + 70 }, 100, STRAT_DECISION_BOTTLE },
- { { PG_BOTTLE1_X, BOT_SIZE_RADIUS + 70 }, 100, STRAT_DECISION_BOTTLE },
- { { PG_BOTTLE2_X, BOT_SIZE_RADIUS + 70 }, 100, STRAT_DECISION_BOTTLE },
- { { PG_BOTTLE3_X, BOT_SIZE_RADIUS + 70 }, 100, STRAT_DECISION_BOTTLE },
+ { { PG_BOTTLE0_X, BOT_SIZE_RADIUS + 70 }, 1000, STRAT_DECISION_BOTTLE },
+ { { PG_BOTTLE1_X, BOT_SIZE_RADIUS + 70 }, 1000, STRAT_DECISION_BOTTLE },
+ { { PG_BOTTLE2_X, BOT_SIZE_RADIUS + 70 }, 1000, STRAT_DECISION_BOTTLE },
+ { { PG_BOTTLE3_X, BOT_SIZE_RADIUS + 70 }, 1000, STRAT_DECISION_BOTTLE },
};
/** Place dynamic information. */
@@ -92,8 +113,16 @@ struct strat_t
uint8_t last_decision;
/** Place of last decision. */
uint8_t last_place;
+ /** Decision is prepared in advance. */
+ uint8_t prepared;
+ /** Prepared position. */
+ vect_t prepared_pos;
/** Robot content estimation. */
uint8_t load;
+ /** Number of time the robot unloaded. */
+ uint8_t unload_nb;
+ /** Upper clamp is dead. */
+ uint8_t upper_clamp_dead;
/** Places information. */
struct strat_place_dyn_t place[STRAT_PLACE_NB];
};
@@ -108,11 +137,15 @@ strat_init (void)
strat.place[i].valid = 1;
if (team_color)
{
+ strat.place[STRAT_PLACE_CAPTAIN1].valid = 0;
+ strat.place[STRAT_PLACE_HOLD1].valid = 0;
strat.place[STRAT_PLACE_BOTTLE1].valid = 0;
strat.place[STRAT_PLACE_BOTTLE3].valid = 0;
}
else
{
+ strat.place[STRAT_PLACE_CAPTAIN0].valid = 0;
+ strat.place[STRAT_PLACE_HOLD0].valid = 0;
strat.place[STRAT_PLACE_BOTTLE0].valid = 0;
strat.place[STRAT_PLACE_BOTTLE2].valid = 0;
}
@@ -152,7 +185,39 @@ strat_place_score (uint8_t i)
int32_t position_score = strat_position_score (&strat_place[i].pos);
if (position_score == -1)
return -1;
- return 10000 - position_score;
+ int32_t score = 10000ll - position_score + strat_place[i].score
+ - 100ll * strat.place[i].fail_nb;
+ if (team_color)
+ {
+ if (i == STRAT_PLACE_TOTEM1 || i == STRAT_PLACE_TOTEM3)
+ score -= 1500;
+ }
+ else
+ {
+ if (i == STRAT_PLACE_TOTEM0 || i == STRAT_PLACE_TOTEM2)
+ score -= 1500;
+ }
+ if (i < STRAT_PLACE_UNLOAD_NB)
+ {
+ if (strat.load && chrono_remaining_time () < 7000)
+ score += strat.load * 4000;
+ else if (strat.load && chrono_remaining_time () < 15000)
+ score += strat.load * 2000;
+ else if (strat.load > 3)
+ score += 7000;
+ else
+ score -= 7000;
+ }
+ if (strat_place[i].decision == STRAT_DECISION_TOTEM
+ && chrono_remaining_time () < 20000)
+ score -= 2000;
+ if (strat.upper_clamp_dead
+ && strat_place[i].decision == STRAT_DECISION_TOTEM)
+ score -= 3000;
+ if (strat.load <= 4 && strat.unload_nb != 0
+ && (i == STRAT_PLACE_CAPTAIN0 || i == STRAT_PLACE_CAPTAIN1))
+ score += 2000;
+ return score;
}
uint8_t
@@ -161,13 +226,14 @@ strat_decision (vect_t *pos)
int32_t best_score = -1;
uint8_t best_place = 0;
uint8_t i;
- if (strat.load > 1)
+ /* If decision was prepared, use it now. */
+ if (strat.prepared)
{
- strat.last_decision = STRAT_DECISION_UNLOAD;
- pos->x = PG_X (BOT_SIZE_RADIUS + 30);
- pos->y = PG_Y (PG_LENGTH / 2);
+ strat.prepared = 0;
+ *pos = strat.prepared_pos;
return strat.last_decision;
}
+ /* Else compute the best decision. */
for (i = 0; i < STRAT_PLACE_NB; i++)
{
int32_t score = strat_place_score (i);
@@ -189,18 +255,31 @@ strat_decision (vect_t *pos)
}
void
+strat_prepare (void)
+{
+ strat_decision (&strat.prepared_pos);
+ strat.prepared = 1;
+}
+
+void
strat_success (void)
{
switch (strat.last_decision)
{
case STRAT_DECISION_TOTEM:
- strat.load++;
+ strat.load += 4;
/* no break; */
case STRAT_DECISION_BOTTLE:
strat.place[strat.last_place].valid = 0;
break;
case STRAT_DECISION_UNLOAD:
strat.load = 0;
+ strat.unload_nb++;
+ if (strat.last_place == STRAT_PLACE_CAPTAIN0
+ || strat.last_place == STRAT_PLACE_CAPTAIN1)
+ {
+ strat.place[strat.last_place].valid = 0;
+ }
break;
default:
assert (0);
@@ -221,3 +300,48 @@ strat_failure (void)
}
}
+void
+strat_bad_failure (void)
+{
+ switch (strat.last_decision)
+ {
+ default:
+ if (strat.place[strat.last_place].fail_nb < 256 - 20)
+ strat.place[strat.last_place].fail_nb += 20;
+ break;
+ case STRAT_DECISION_UNLOAD:
+ break;
+ }
+}
+
+void
+strat_giveup (void)
+{
+ switch (strat.last_decision)
+ {
+ default:
+ strat.place[strat.last_place].valid = 0;
+ break;
+ case STRAT_DECISION_UNLOAD:
+ assert (0);
+ }
+}
+
+void
+strat_coin_taken (void)
+{
+ strat.load++;
+}
+
+void
+strat_clamp_dead (void)
+{
+ /* TODO */
+}
+
+void
+strat_upper_clamp_dead (void)
+{
+ strat.upper_clamp_dead = 1;
+}
+
diff --git a/digital/io-hub/src/guybrush/strat.h b/digital/io-hub/src/guybrush/strat.h
index e20a44b8..4f3465c4 100644
--- a/digital/io-hub/src/guybrush/strat.h
+++ b/digital/io-hub/src/guybrush/strat.h
@@ -44,6 +44,11 @@ strat_init (void);
uint8_t
strat_decision (vect_t *pos);
+/** Take a decision in advance so that next strat_decision call is
+ * immediate. */
+void
+strat_prepare (void);
+
/** Report a success of last decision. */
void
strat_success (void);
@@ -52,4 +57,24 @@ strat_success (void);
void
strat_failure (void);
+/** Report a really bad failure. */
+void
+strat_bad_failure (void);
+
+/** Report a failure of last decision, never try again. */
+void
+strat_giveup (void);
+
+/** Signal a coin has been taken. */
+void
+strat_coin_taken (void);
+
+/** Clamp is dead stop using it. */
+void
+strat_clamp_dead (void);
+
+/** Upper clamp is dead. */
+void
+strat_upper_clamp_dead (void);
+
#endif /* strat_h */
diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c
index 869721b3..2bc6c8e1 100644
--- a/digital/io-hub/src/guybrush/top.c
+++ b/digital/io-hub/src/guybrush/top.c
@@ -40,6 +40,7 @@
#include "strat.h"
#include "path.h"
#include "bottom_clamp.h"
+#include "radar_defs.h"
#include "output_defs.h"
@@ -48,6 +49,9 @@
* impression of intelligence... Well...
*/
+/** Number of time to try to unblock clamp. */
+#define TOP_CLAMP_BLOCKED_THRESHOLD 5
+
FSM_INIT
FSM_STATES (
@@ -57,6 +61,12 @@ FSM_STATES (
TOP_INIT_DOOR,
/* Init done, waiting for rount start. */
TOP_INIT,
+ /* Transition state to take a new decision. */
+ TOP_DECISION,
+ /* Move to unblock clamp. */
+ TOP_CLAMP_ERROR_MOVING_DECISION,
+ /* Unblock clamp and take a new decision. */
+ TOP_CLAMP_ERROR_DECISION,
/* Going to a collect position above or below a totem. */
TOP_TOTEM_GOING,
@@ -82,8 +92,12 @@ FSM_STATES (
TOP_TOTEM_GOING_BACK,
/* Put clamps up. */
TOP_TOTEM_CLAMP_UPPING,
+ /* Release a goldbar blocking the robot. */
+ TOP_TOTEM_ERROR_RELEASE,
/* Going back after an error. */
TOP_TOTEM_ERROR_GOING_BACK,
+ /* Going back after a clamp error. */
+ TOP_TOTEM_CLAMP_ERROR_GOING_BACK,
/* Going to push a bottle. */
TOP_BOTTLE_GOING,
@@ -96,8 +110,8 @@ FSM_STATES (
/* Going to an unload position. */
TOP_UNLOAD_GOING,
- /* Unloading, waiting for elements to fall. */
- TOP_UNLOADING)
+ /* Going back in unload zone. */
+ TOP_UNLOAD_GOING_BACK)
FSM_START_WITH (TOP_START)
@@ -110,6 +124,12 @@ struct top_t
int16_t totem_distance;
/** Close door when out of unloading zone. */
uint8_t close_door;
+ /** Check door is closed once this reach 0. */
+ uint8_t close_door_check;
+ /** Close the door once this reach 0. */
+ uint8_t close_door_wait;
+ /** Chaos factor. */
+ uint8_t chaos;
};
/** Global context. */
@@ -118,20 +138,70 @@ struct top_t top;
void
top_update (void)
{
+ /* Close door when out of unloading zone. */
if (top.close_door)
{
position_t robot_pos;
asserv_get_position (&robot_pos);
- if (robot_pos.v.x > PG_HOLD_NORTH_X + BOT_SIZE_BACK
- && robot_pos.v.x < PG_MIRROR_X (PG_HOLD_NORTH_X + BOT_SIZE_BACK))
+ int16_t limit = robot_pos.v.y < PG_LENGTH - PG_CAPTAIN_ROOM_LENGTH_MM
+ ? PG_HOLD_NORTH_X + BOT_SIZE_BACK
+ : PG_CAPTAIN_ROOM_LENGTH_MM + BOT_SIZE_BACK;
+ if (robot_pos.v.x > limit && robot_pos.v.x < PG_MIRROR_X (limit))
{
IO_CLR (OUTPUT_DOOR_OPEN);
IO_SET (OUTPUT_DOOR_CLOSE);
top.close_door = 0;
+ top.close_door_check = 250;
+ }
+ }
+ /* If door is closed, cut air to save power. */
+ if (!IO_GET (CONTACT_DOOR_CLOSE))
+ {
+ IO_CLR (OUTPUT_DOOR_CLOSE);
+ }
+ /* Test if it should be closed. */
+ if (top.close_door_check)
+ {
+ top.close_door_check--;
+ if (!top.close_door_check && IO_GET (CONTACT_DOOR_CLOSE))
+ {
+ IO_SET (OUTPUT_DOOR_OPEN);
+ IO_CLR (OUTPUT_DOOR_CLOSE);
+ top.close_door_wait = 250;
+ }
+ }
+ /* Close door after a delay. */
+ if (top.close_door_wait)
+ {
+ top.close_door_wait--;
+ if (!top.close_door_wait)
+ {
+ IO_CLR (OUTPUT_DOOR_OPEN);
+ IO_SET (OUTPUT_DOOR_CLOSE);
}
}
}
+/** Move in a surprising way to unblock clamp. */
+static void
+top_chaos_move (void)
+{
+ if ((top.chaos % 4) == 3)
+ {
+ int16_t angle = (top.chaos % 8) == 3 ? POSITION_A_DEG (3) :
+ -POSITION_A_DEG (3);
+ asserv_move_angularly (angle);
+ }
+ else
+ {
+ int16_t dist = ((top.chaos + 3) % 8 + 1) * 10;
+ if (!(top.chaos % 2))
+ dist = -dist;
+ asserv_move_linearly (dist);
+ }
+ top.chaos++;
+}
+
/** Go collect a totem. */
static void
top_go_totem (void)
@@ -161,7 +231,10 @@ top_go_unload (void)
{
position_t pos;
pos.v = top.decision_pos;
- pos.a = PG_A_DEG (70);
+ if (pos.v.y > PG_LENGTH - PG_CAPTAIN_ROOM_LENGTH_MM)
+ pos.a = PG_A_DEG (0);
+ else
+ pos.a = PG_A_DEG (45);
move_start (pos, 0);
}
@@ -189,19 +262,6 @@ top_decision (void)
return decision;
}
-#define RETURN_TOP_DECISION_SWITCH(state, event) do { \
- switch (top_decision ()) \
- { \
- default: assert (0); \
- case STRAT_DECISION_TOTEM: \
- return FSM_NEXT (state, event, totem); \
- case STRAT_DECISION_BOTTLE: \
- return FSM_NEXT (state, event, bottle); \
- case STRAT_DECISION_UNLOAD: \
- return FSM_NEXT (state, event, unload); \
- } \
-} while (0)
-
FSM_TRANS (TOP_START, init_actuators, TOP_INIT_DOOR)
{
main_demo = !IO_GET (CONTACT_STRAT);
@@ -214,30 +274,98 @@ FSM_TRANS (TOP_INIT_DOOR, init_done, TOP_INIT)
{
IO_CLR (OUTPUT_DOOR_OPEN);
IO_SET (OUTPUT_DOOR_CLOSE);
+ strat_init ();
+ strat_prepare ();
return FSM_NEXT (TOP_INIT_DOOR, init_done);
}
-FSM_TRANS (TOP_INIT, init_start_round,
- totem, TOP_TOTEM_GOING,
- bottle, TOP_BOTTLE_GOING,
- unload, TOP_UNLOAD_GOING)
+FSM_TRANS (TOP_INIT, init_start_round, TOP_DECISION)
{
- strat_init ();
main_usdist = 1;
- RETURN_TOP_DECISION_SWITCH (TOP_INIT, init_start_round);
+ return FSM_NEXT (TOP_INIT, init_start_round);
+}
+
+FSM_TRANS_TIMEOUT (TOP_DECISION, 1,
+ clamp_blocked, TOP_CLAMP_ERROR_MOVING_DECISION,
+ totem, TOP_TOTEM_GOING,
+ bottle, TOP_BOTTLE_GOING,
+ unload, TOP_UNLOAD_GOING)
+{
+ radar_def_upper_clamp_moving (0);
+ if (FSM_CAN_HANDLE (AI, clamp_unblock))
+ {
+ top_chaos_move ();
+ return FSM_NEXT_TIMEOUT (TOP_DECISION, clamp_blocked);
+ }
+ switch (top_decision ())
+ {
+ default:
+ assert (0);
+ case STRAT_DECISION_TOTEM:
+ return FSM_NEXT_TIMEOUT (TOP_DECISION, totem);
+ case STRAT_DECISION_BOTTLE:
+ return FSM_NEXT_TIMEOUT (TOP_DECISION, bottle);
+ case STRAT_DECISION_UNLOAD:
+ return FSM_NEXT_TIMEOUT (TOP_DECISION, unload);
+ }
+}
+
+FSM_TRANS (TOP_CLAMP_ERROR_MOVING_DECISION, robot_move_success,
+ TOP_CLAMP_ERROR_DECISION)
+{
+ radar_def_upper_clamp_moving (0);
+ FSM_HANDLE (AI, clamp_unblock);
+ return FSM_NEXT (TOP_CLAMP_ERROR_MOVING_DECISION, robot_move_success);
+}
+
+FSM_TRANS (TOP_CLAMP_ERROR_MOVING_DECISION, robot_move_failure,
+ TOP_CLAMP_ERROR_DECISION)
+{
+ FSM_HANDLE (AI, clamp_unblock);
+ return FSM_NEXT (TOP_CLAMP_ERROR_MOVING_DECISION, robot_move_failure);
+}
+
+FSM_TRANS (TOP_CLAMP_ERROR_DECISION, clamps_ready, TOP_DECISION)
+{
+ return FSM_NEXT (TOP_CLAMP_ERROR_DECISION, clamps_ready);
+}
+
+FSM_TRANS (TOP_CLAMP_ERROR_DECISION, clamp_blocked,
+ unblock, TOP_CLAMP_ERROR_DECISION,
+ dead, TOP_DECISION)
+{
+ if (clamp_read_blocked_cpt () < TOP_CLAMP_BLOCKED_THRESHOLD)
+ {
+ FSM_HANDLE (AI, clamp_unblock);
+ return FSM_NEXT (TOP_CLAMP_ERROR_DECISION, clamp_blocked, unblock);
+ }
+ else
+ {
+ strat_clamp_dead ();
+ strat_bad_failure ();
+ FSM_HANDLE (AI, clamp_is_dead);
+ return FSM_NEXT (TOP_CLAMP_ERROR_DECISION, clamp_blocked, dead);
+ }
+}
+
+FSM_TRANS (TOP_CLAMP_ERROR_DECISION, upper_set_is_dead, TOP_DECISION)
+{
+ strat_upper_clamp_dead ();
+ return FSM_NEXT (TOP_CLAMP_ERROR_DECISION, upper_set_is_dead);
}
/** TOTEM */
FSM_TRANS (TOP_TOTEM_GOING, move_success, TOP_TOTEM_CLEAN_STARTING)
{
+ radar_def_upper_clamp_moving (1);
clamp_request (FSM_EVENT (AI, clean_start));
return FSM_NEXT (TOP_TOTEM_GOING, move_success);
}
FSM_TRANS (TOP_TOTEM_CLEAN_STARTING, clamps_ready, TOP_TOTEM_CLEAN_APPROACHING)
{
- int16_t move = top.totem_distance - BOT_SIZE_LOWER_CLAMP_FRONT - 140;
+ int16_t move = top.totem_distance - BOT_SIZE_LOWER_CLAMP_FRONT - 135;
top.totem_distance -= move;
asserv_set_speed (BOT_SPEED_APPROACH);
asserv_move_linearly (move);
@@ -251,10 +379,18 @@ FSM_TRANS (TOP_TOTEM_CLEAN_APPROACHING, robot_move_success,
return FSM_NEXT (TOP_TOTEM_CLEAN_APPROACHING, robot_move_success);
}
+FSM_TRANS (TOP_TOTEM_CLEAN_APPROACHING, robot_move_failure,
+ TOP_TOTEM_CLEAN_CATCH_WAITING)
+{
+ /* Must be blocked on a CD, catch and continue. */
+ FSM_HANDLE (AI, clean_catch);
+ return FSM_NEXT (TOP_TOTEM_CLEAN_APPROACHING, robot_move_failure);
+}
+
FSM_TRANS (TOP_TOTEM_CLEAN_CATCH_WAITING, clamps_ready,
TOP_TOTEM_CLEAN_GOING_BACK)
{
- int16_t move = top.totem_distance - BOT_SIZE_LOWER_CLAMP_FRONT - 240;
+ int16_t move = top.totem_distance - BOT_SIZE_LOADING_FRONT - 30;
top.totem_distance -= move;
asserv_move_linearly (move);
return FSM_NEXT (TOP_TOTEM_CLEAN_CATCH_WAITING, clamps_ready);
@@ -267,6 +403,13 @@ FSM_TRANS (TOP_TOTEM_CLEAN_GOING_BACK, robot_move_success,
return FSM_NEXT (TOP_TOTEM_CLEAN_GOING_BACK, robot_move_success);
}
+FSM_TRANS (TOP_TOTEM_CLEAN_GOING_BACK, robot_move_failure,
+ TOP_TOTEM_CLEAN_LOADING)
+{
+ FSM_HANDLE (AI, clean_load);
+ return FSM_NEXT (TOP_TOTEM_CLEAN_GOING_BACK, robot_move_failure);
+}
+
FSM_TRANS (TOP_TOTEM_CLEAN_LOADING, clamps_ready, TOP_TOTEM_CLAMP_DOWNING)
{
clamp_request (FSM_EVENT (AI, tree_detected));
@@ -296,46 +439,78 @@ FSM_TRANS (TOP_TOTEM_PUSHING, robot_move_success, TOP_TOTEM_EMPTYING)
FSM_TRANS (TOP_TOTEM_EMPTYING, clamps_ready, TOP_TOTEM_GOING_BACK)
{
- strat_success ();
move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
return FSM_NEXT (TOP_TOTEM_EMPTYING, clamps_ready);
}
FSM_TRANS (TOP_TOTEM_GOING_BACK, move_success, TOP_TOTEM_CLAMP_UPPING)
{
+ strat_success ();
FSM_HANDLE (AI, robot_is_back);
return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_success);
}
-FSM_TRANS (TOP_TOTEM_GOING_BACK, move_failure, TOP_TOTEM_CLAMP_UPPING)
+FSM_TRANS (TOP_TOTEM_CLAMP_UPPING, clamps_ready, TOP_DECISION)
{
- FSM_HANDLE (AI, robot_is_back);
- return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_failure);
+ return FSM_NEXT (TOP_TOTEM_CLAMP_UPPING, clamps_ready);
}
-FSM_TRANS (TOP_TOTEM_CLAMP_UPPING, clamps_ready,
- totem, TOP_TOTEM_GOING,
- bottle, TOP_BOTTLE_GOING,
- unload, TOP_UNLOAD_GOING)
+/** TOTEM failures. */
+
+FSM_TRANS (TOP_TOTEM_GOING, move_failure, TOP_DECISION)
{
- RETURN_TOP_DECISION_SWITCH (TOP_TOTEM_CLAMP_UPPING, clamps_ready);
+ strat_failure ();
+ return FSM_NEXT (TOP_TOTEM_GOING, move_failure);
}
-/** TOTEM failures. */
+FSM_TRANS (TOP_TOTEM_CLEAN_STARTING, clamp_blocked,
+ TOP_CLAMP_ERROR_MOVING_DECISION)
+{
+ strat_failure ();
+ top_chaos_move ();
+ return FSM_NEXT (TOP_TOTEM_CLEAN_STARTING, clamp_blocked);
+}
+
+FSM_TRANS_TIMEOUT (TOP_TOTEM_CLEAN_STARTING, 5 * 250, TOP_DECISION)
+{
+ strat_bad_failure ();
+ return FSM_NEXT_TIMEOUT (TOP_TOTEM_CLEAN_STARTING);
+}
-FSM_TRANS (TOP_TOTEM_GOING, move_failure,
- totem, TOP_TOTEM_GOING,
- bottle, TOP_BOTTLE_GOING,
- unload, TOP_UNLOAD_GOING)
+FSM_TRANS (TOP_TOTEM_CLEAN_CATCH_WAITING, clamp_blocked,
+ TOP_TOTEM_CLAMP_ERROR_GOING_BACK)
{
strat_failure ();
- RETURN_TOP_DECISION_SWITCH (TOP_TOTEM_GOING, move_failure);
+ move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
+ return FSM_NEXT (TOP_TOTEM_CLEAN_CATCH_WAITING, clamp_blocked);
+}
+
+FSM_TRANS (TOP_TOTEM_CLEAN_LOADING, clamp_blocked,
+ TOP_CLAMP_ERROR_MOVING_DECISION)
+{
+ strat_failure ();
+ top_chaos_move ();
+ return FSM_NEXT (TOP_TOTEM_CLEAN_LOADING, clamp_blocked);
+}
+
+FSM_TRANS (TOP_TOTEM_CLAMP_DOWNING, clamp_blocked,
+ TOP_CLAMP_ERROR_MOVING_DECISION)
+{
+ strat_failure ();
+ top_chaos_move ();
+ return FSM_NEXT (TOP_TOTEM_CLAMP_DOWNING, clamp_blocked);
+}
+
+FSM_TRANS_TIMEOUT (TOP_TOTEM_CLAMP_DOWNING, 5 * 250, TOP_DECISION)
+{
+ strat_bad_failure ();
+ return FSM_NEXT_TIMEOUT (TOP_TOTEM_CLAMP_DOWNING);
}
FSM_TRANS (TOP_TOTEM_APPROACHING, robot_move_failure,
TOP_TOTEM_ERROR_GOING_BACK)
{
- strat_failure ();
+ strat_giveup ();
move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
return FSM_NEXT (TOP_TOTEM_APPROACHING, robot_move_failure);
}
@@ -343,11 +518,40 @@ FSM_TRANS (TOP_TOTEM_APPROACHING, robot_move_failure,
FSM_TRANS (TOP_TOTEM_PUSHING, robot_move_failure,
TOP_TOTEM_ERROR_GOING_BACK)
{
- strat_failure ();
+ strat_giveup ();
move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
return FSM_NEXT (TOP_TOTEM_PUSHING, robot_move_failure);
}
+FSM_TRANS (TOP_TOTEM_EMPTYING, clamp_blocked,
+ TOP_TOTEM_CLAMP_ERROR_GOING_BACK)
+{
+ strat_giveup ();
+ move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
+ return FSM_NEXT (TOP_TOTEM_EMPTYING, clamp_blocked);
+}
+
+FSM_TRANS (TOP_TOTEM_GOING_BACK, move_failure, TOP_TOTEM_ERROR_RELEASE)
+{
+ strat_giveup ();
+ FSM_HANDLE (AI, stop_tree_approach);
+ return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_failure);
+}
+
+FSM_TRANS (TOP_TOTEM_CLAMP_UPPING, clamp_blocked,
+ TOP_CLAMP_ERROR_MOVING_DECISION)
+{
+ strat_giveup ();
+ top_chaos_move ();
+ return FSM_NEXT (TOP_TOTEM_CLAMP_UPPING, clamp_blocked);
+}
+
+FSM_TRANS (TOP_TOTEM_ERROR_RELEASE, clamps_ready, TOP_TOTEM_ERROR_GOING_BACK)
+{
+ move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
+ return FSM_NEXT (TOP_TOTEM_ERROR_RELEASE, clamps_ready);
+}
+
FSM_TRANS (TOP_TOTEM_ERROR_GOING_BACK, move_success, TOP_TOTEM_CLAMP_UPPING)
{
FSM_HANDLE (AI, stop_tree_approach);
@@ -360,6 +564,20 @@ FSM_TRANS (TOP_TOTEM_ERROR_GOING_BACK, move_failure, TOP_TOTEM_CLAMP_UPPING)
return FSM_NEXT (TOP_TOTEM_ERROR_GOING_BACK, move_failure);
}
+FSM_TRANS (TOP_TOTEM_CLAMP_ERROR_GOING_BACK, move_success,
+ TOP_CLAMP_ERROR_MOVING_DECISION)
+{
+ top_chaos_move ();
+ return FSM_NEXT (TOP_TOTEM_CLAMP_ERROR_GOING_BACK, move_success);
+}
+
+FSM_TRANS (TOP_TOTEM_CLAMP_ERROR_GOING_BACK, move_failure,
+ TOP_CLAMP_ERROR_MOVING_DECISION)
+{
+ top_chaos_move ();
+ return FSM_NEXT (TOP_TOTEM_CLAMP_ERROR_GOING_BACK, move_failure);
+}
+
/** BOTTLE */
FSM_TRANS (TOP_BOTTLE_GOING, move_success, TOP_BOTTLE_APPROACHING)
@@ -393,39 +611,28 @@ FSM_TRANS (TOP_BOTTLE_PUSHING, robot_move_failure, TOP_BOTTLE_GOING_BACK)
return FSM_NEXT (TOP_BOTTLE_PUSHING, robot_move_failure);
}
-FSM_TRANS (TOP_BOTTLE_GOING_BACK, move_success,
- totem, TOP_TOTEM_GOING,
- bottle, TOP_BOTTLE_GOING,
- unload, TOP_UNLOAD_GOING)
+FSM_TRANS (TOP_BOTTLE_GOING_BACK, move_success, TOP_DECISION)
{
- RETURN_TOP_DECISION_SWITCH (TOP_BOTTLE_GOING_BACK, move_success);
+ return FSM_NEXT (TOP_BOTTLE_GOING_BACK, move_success);
}
-FSM_TRANS (TOP_BOTTLE_GOING_BACK, move_failure,
- totem, TOP_TOTEM_GOING,
- bottle, TOP_BOTTLE_GOING,
- unload, TOP_UNLOAD_GOING)
+FSM_TRANS (TOP_BOTTLE_GOING_BACK, move_failure, TOP_DECISION)
{
/* Ignore and continue. */
- RETURN_TOP_DECISION_SWITCH (TOP_BOTTLE_GOING_BACK, move_failure);
+ return FSM_NEXT (TOP_BOTTLE_GOING_BACK, move_failure);
}
/** BOTTLE failures. */
-FSM_TRANS (TOP_BOTTLE_GOING, move_failure,
- totem, TOP_TOTEM_GOING,
- bottle, TOP_BOTTLE_GOING,
- unload, TOP_UNLOAD_GOING)
+FSM_TRANS (TOP_BOTTLE_GOING, move_failure, TOP_DECISION)
{
strat_failure ();
- RETURN_TOP_DECISION_SWITCH (TOP_BOTTLE_GOING, move_failure);
+ return FSM_NEXT (TOP_BOTTLE_GOING, move_failure);
}
FSM_TRANS (TOP_BOTTLE_APPROACHING, robot_move_failure,
try_anyway, TOP_BOTTLE_PUSHING,
- totem, TOP_TOTEM_GOING,
- bottle, TOP_BOTTLE_GOING,
- unload, TOP_UNLOAD_GOING)
+ decision, TOP_DECISION)
{
position_t robot_pos;
asserv_get_position (&robot_pos);
@@ -439,37 +646,55 @@ FSM_TRANS (TOP_BOTTLE_APPROACHING, robot_move_failure,
else
{
strat_failure ();
- RETURN_TOP_DECISION_SWITCH (TOP_BOTTLE_APPROACHING, robot_move_failure);
+ return FSM_NEXT (TOP_BOTTLE_APPROACHING, robot_move_failure,
+ decision);
}
}
/** UNLOAD */
-FSM_TRANS (TOP_UNLOAD_GOING, move_success, TOP_UNLOADING)
+static void
+top_do_unload (void)
{
IO_SET (OUTPUT_DOOR_OPEN);
IO_CLR (OUTPUT_DOOR_CLOSE);
- return FSM_NEXT (TOP_UNLOAD_GOING, move_success);
+ strat_success ();
+ top.close_door = 1;
}
-FSM_TRANS_TIMEOUT (TOP_UNLOADING, 250,
- totem, TOP_TOTEM_GOING,
- bottle, TOP_BOTTLE_GOING,
- unload, TOP_UNLOAD_GOING)
+FSM_TRANS (TOP_UNLOAD_GOING, move_success,
+ hold, TOP_UNLOAD_GOING_BACK,
+ captain, TOP_UNLOAD_GOING_BACK)
{
- strat_success ();
- top.close_door = 1;
- RETURN_TOP_DECISION_SWITCH (TOP_UNLOADING, TOP_UNLOADING_TIMEOUT);
+ if (top.decision_pos.y < PG_LENGTH - PG_CAPTAIN_ROOM_LENGTH_MM)
+ {
+ asserv_move_linearly (-100);
+ return FSM_NEXT (TOP_UNLOAD_GOING, move_success, hold);
+ }
+ else
+ {
+ asserv_move_linearly (-50);
+ return FSM_NEXT (TOP_UNLOAD_GOING, move_success, captain);
+ }
+}
+
+FSM_TRANS (TOP_UNLOAD_GOING_BACK, robot_move_success, TOP_DECISION)
+{
+ top_do_unload ();
+ return FSM_NEXT (TOP_UNLOAD_GOING_BACK, robot_move_success);
+}
+
+FSM_TRANS (TOP_UNLOAD_GOING_BACK, robot_move_failure, TOP_DECISION)
+{
+ top_do_unload ();
+ return FSM_NEXT (TOP_UNLOAD_GOING_BACK, robot_move_failure);
}
/** UNLOAD failures. */
-FSM_TRANS (TOP_UNLOAD_GOING, move_failure,
- totem, TOP_TOTEM_GOING,
- bottle, TOP_BOTTLE_GOING,
- unload, TOP_UNLOAD_GOING)
+FSM_TRANS (TOP_UNLOAD_GOING, move_failure, TOP_DECISION)
{
strat_failure ();
- RETURN_TOP_DECISION_SWITCH (TOP_UNLOAD_GOING, move_failure);
+ return FSM_NEXT (TOP_UNLOAD_GOING, move_failure);
}
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
index 1f8c5072..39ae8dfa 100644
--- a/digital/io-hub/src/robospierre/clamp.c
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -547,8 +547,8 @@ clamp_blocked (void)
else
logistic_dump (DIRECTION_BACKWARD, 0);
}
- mimot_motor0_free ();
- mimot_motor1_free ();
+ mimot_motor_free (0, 1);
+ mimot_motor_free (1, 1);
/* Signal problem. */
fsm_queue_post_event (FSM_EVENT (AI, clamp_move_failure));
}
@@ -649,8 +649,8 @@ FSM_TRANS (CLAMP_INIT_FINDING_TOP, clamp_elevation_success,
FSM_TRANS (CLAMP_INIT_GOING_REST, clamp_move_success, CLAMP_INIT_READY)
{
- mimot_motor0_free ();
- mimot_motor1_free ();
+ mimot_motor_free (0, 1);
+ mimot_motor_free (1, 1);
return FSM_NEXT (CLAMP_INIT_GOING_REST, clamp_move_success);
}
diff --git a/digital/io-hub/src/robospierre/logger.h b/digital/io-hub/src/robospierre/logger.h
new file mode 100644
index 00000000..e8668b3f
--- /dev/null
+++ b/digital/io-hub/src/robospierre/logger.h
@@ -0,0 +1,30 @@
+#ifndef logger_h
+#define logger_h
+/* logger.h */
+/* robospierre - Eurobot 2011 AI. {{{
+ *
+ * Copyright (C) 2012 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+
+#define logger_log(args...)
+
+#endif /* logger_h */
diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c
index f16333c5..e68de49c 100644
--- a/digital/io-hub/src/robospierre/main.c
+++ b/digital/io-hub/src/robospierre/main.c
@@ -340,8 +340,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
case c ('w', 0):
/* Disable all motor control. */
- mimot_motor0_free ();
- mimot_motor1_free ();
+ mimot_motor_free (0, 0);
+ mimot_motor_free (1, 0);
asserv_free_motor ();
break;
/* Stats commands.
diff --git a/digital/io-hub/tools/decode_fsm.py b/digital/io-hub/tools/decode_fsm.py
index e8c38231..41e9fe37 100644
--- a/digital/io-hub/tools/decode_fsm.py
+++ b/digital/io-hub/tools/decode_fsm.py
@@ -22,7 +22,8 @@ def parse_header(fsm_name, fname):
def parse_proto(states, events):
"""Parse proto output from stdin and output FSM transitions."""
- fsm_re = re.compile(r'!F([0-9af]{2})([0-9af]{2})([0-9af]{2})')
+ fsm_re = re.compile(r'!F([0-9a-f]{2})([0-9a-f]{2})([0-9a-f]{2})')
+ obstacle_re = re.compile(r'!o([0-9a-f]{4})([0-9a-f]{4})')
for l in sys.stdin:
for m in fsm_re.finditer(l):
old, event, new = [ int(i, 16) for i in m.groups() ]
@@ -34,6 +35,9 @@ def parse_proto(states, events):
print "unknown transition"
else:
print "%s -> %s -> %s" % (old, event, new)
+ for m in obstacle_re.finditer(l):
+ x, y = [ int(i, 16) for i in m.groups() ]
+ print "obstacle %s %s" % (x, y)
op = optparse.OptionParser(description=__doc__)
op.add_option('--file', '-f', metavar='HEADER',
diff --git a/digital/io-hub/tools/io_hub/io_hub.py b/digital/io-hub/tools/io_hub/io_hub.py
index 4aa388b0..7c4d2bb5 100644
--- a/digital/io-hub/tools/io_hub/io_hub.py
+++ b/digital/io-hub/tools/io_hub/io_hub.py
@@ -100,3 +100,5 @@ class ProtoGuybrush (Proto):
self.proto.send('e')
def robot_is_back(self):
self.proto.send('r')
+ def unblock_bottom_clamp(self):
+ self.proto.send('u')
diff --git a/digital/io/src/logger.h b/digital/io/src/logger.h
new file mode 100644
index 00000000..80fad10e
--- /dev/null
+++ b/digital/io/src/logger.h
@@ -0,0 +1,30 @@
+#ifndef logger_h
+#define logger_h
+/* logger.h */
+/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+ *
+ * Copyright (C) 2012 Nicolas Schodet
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+
+#define logger_log(args...)
+
+#endif /* logger_h */
diff --git a/digital/mimot/src/asserv/models.host.c b/digital/mimot/src/asserv/models.host.c
index e97d3958..ab55c341 100644
--- a/digital/mimot/src/asserv/models.host.c
+++ b/digital/mimot/src/asserv/models.host.c
@@ -75,7 +75,7 @@ static const struct robot_t guybrush_robot =
* table. */
simu_table_test_guybrush,
/** Robot corners, from front left, then clockwise. */
- { { 150, 171.5 }, { 150, -171.5 }, { -134, -121.5 }, { -134, 121.5 },
+ { { 150, 171.5 }, { 150, -171.5 }, { -135, -121.5 }, { -135, 121.5 },
{ 150, 0 } },
/** Initialisation function. */
NULL,
diff --git a/digital/mimot/src/dirty/twi_proto.c b/digital/mimot/src/dirty/twi_proto.c
index 66cdb5aa..2de2b1a1 100644
--- a/digital/mimot/src/dirty/twi_proto.c
+++ b/digital/mimot/src/dirty/twi_proto.c
@@ -156,13 +156,14 @@ twi_proto_callback (u8 *buf, u8 size)
else
buf[0] = 0;
break;
- case c ('w', 1):
+ case c ('w', 2):
/* Free motor.
- * - b: aux index. */
+ * - b: aux index.
+ * - b: brake. */
if (buf[2] < AC_ASSERV_AUX_NB)
{
output_set (&output_aux[buf[2]], 0);
- output_brake (&output_aux[buf[2]], 0);
+ output_brake (&output_aux[buf[2]], buf[3]);
control_state_set_mode (&cs_aux[buf[2]].state, CS_MODE_NONE, 0);
}
else