summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/guybrush/main.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io-hub/src/guybrush/main.c')
-rw-r--r--digital/io-hub/src/guybrush/main.c118
1 files changed, 115 insertions, 3 deletions
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];