summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/robospierre/main.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io-hub/src/robospierre/main.c')
-rw-r--r--digital/io-hub/src/robospierre/main.c173
1 files changed, 170 insertions, 3 deletions
diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c
index 3a32a1ae..9ae644e6 100644
--- a/digital/io-hub/src/robospierre/main.c
+++ b/digital/io-hub/src/robospierre/main.c
@@ -28,6 +28,8 @@
#include "modules/uart/uart.h"
#include "modules/proto/proto.h"
+#include "modules/devices/usdist/usdist.h"
+
#include "timer.h"
#include "chrono.h"
#include "simu.host.h"
@@ -38,30 +40,60 @@
#include "pwm.h"
#include "contact.h"
+#include "codebar.h"
+#include "pawn_sensor.h"
+#include "radar.h"
#define FSM_NAME AI
#include "fsm.h"
#ifdef HOST
# include <string.h>
#endif
+#include "fsm_queue.h"
#include "clamp.h"
+#include "logistic.h"
+#include "path.h"
+#include "move.h"
+
+#include "bot.h"
#include "io.h"
+#ifndef HOST
+# include <avr/wdt.h>
+#endif
+
/** Our color. */
enum team_color_e team_color;
+/** Obstacles positions, updated using radar module. */
+vect_t main_obstacles_pos[2];
+
+/** Number of obstacles in main_obstacles_pos. */
+uint8_t main_obstacles_nb;
+
/** Asserv stats counters. */
static uint8_t main_stats_asserv_, main_stats_asserv_cpt_;
/** Contact stats counters. */
static uint8_t main_stats_contact_, main_stats_contact_cpt_;
+/** Codebar stats counters. */
+static uint8_t main_stats_codebar_, main_stats_codebar_cpt_;
+
+/** US sensors stats counters. */
+static uint8_t main_stats_usdist_, main_stats_usdist_cpt_;
+
/** Main initialisation. */
static void
main_init (void)
{
+#ifndef HOST
+ /* Disable watchdog (enabled in bootloader). */
+ MCUSR &= ~(1 << WDRF);
+ wdt_disable ();
+#endif
/* Serial port */
uart0_init ();
/* Enable interrupts */
@@ -76,6 +108,12 @@ main_init (void)
/* IO modules. */
pwm_init ();
contact_init ();
+ codebar_init ();
+ usdist_init ();
+ /* AI modules. */
+ clamp_init ();
+ logistic_init ();
+ path_init ();
/* Initialization done. */
proto_send0 ('z');
}
@@ -93,18 +131,52 @@ main_event_to_fsm (void)
#define FSM_HANDLE_TIMEOUT_E(fsm) \
do { if (FSM_HANDLE_TIMEOUT (fsm)) return; } while (0)
/* Update FSM timeouts. */
- //FSM_HANDLE_TIMEOUT_E (AI);
+ FSM_HANDLE_TIMEOUT_E (AI);
/* Motor status. */
- asserv_status_e mimot_motor0_status, mimot_motor1_status;
+ asserv_status_e robot_move_status, mimot_motor0_status,
+ mimot_motor1_status;
+ robot_move_status = asserv_move_cmd_status ();
mimot_motor0_status = mimot_motor0_cmd_status ();
mimot_motor1_status = mimot_motor1_cmd_status ();
+ if (robot_move_status == success)
+ FSM_HANDLE_E (AI, robot_move_success);
+ else if (robot_move_status == failure)
+ FSM_HANDLE_E (AI, robot_move_failure);
if (mimot_motor0_status == success
&& mimot_motor1_status == success)
FSM_HANDLE_E (AI, clamp_elevation_rotation_success);
+ if (mimot_motor0_status == failure
+ || mimot_motor1_status == failure)
+ FSM_HANDLE_E (AI, clamp_elevation_or_rotation_failure);
+ if (mimot_motor0_status == success)
+ FSM_HANDLE_E (AI, clamp_elevation_success);
else if (mimot_motor0_status == failure)
FSM_HANDLE_E (AI, clamp_elevation_failure);
+ if (mimot_motor1_status == success)
+ FSM_HANDLE_E (AI, clamp_rotation_success);
else if (mimot_motor1_status == failure)
FSM_HANDLE_E (AI, clamp_rotation_failure);
+ /* Clamp specific events. */
+ if (clamp_handle_event ())
+ return;
+ /* Jack. */
+ if (!contact_get_jack ())
+ FSM_HANDLE_E (AI, jack_inserted);
+ else
+ FSM_HANDLE_E (AI, jack_removed);
+ /* Events from the event queue. */
+ if (fsm_queue_poll ())
+ {
+ /* We must post the event at the end of this block because if it is
+ * handled, the function will return and every instruction after will
+ * never be executed. */
+ uint8_t save_event = fsm_queue_pop_event ();
+ /* Post the event */
+ FSM_HANDLE_VAR_E (AI, save_event);
+ }
+ /* Check obstables. */
+ if (move_check_obstacles ())
+ return;
}
/** Main (and infinite) loop. */
@@ -120,6 +192,12 @@ main_loop (void)
/* Is match over? */
if (chrono_is_match_over ())
{
+ /* Power off doors. */
+ pwm_set (BOT_PWM_DOOR_FRONT_BOTTOM, 0);
+ pwm_set (BOT_PWM_DOOR_FRONT_TOP, 0);
+ pwm_set (BOT_PWM_DOOR_BACK_BOTTOM, 0);
+ pwm_set (BOT_PWM_DOOR_BACK_TOP, 0);
+ pwm_set (BOT_PWM_CLAMP, 0);
/* End it and block here indefinitely. */
chrono_end_match (42);
return;
@@ -130,6 +208,18 @@ main_loop (void)
/* Update IO modules. */
pwm_update ();
contact_update ();
+ pawn_sensor_update ();
+ if (usdist_update ())
+ {
+ position_t robot_pos;
+ asserv_get_position (&robot_pos);
+ main_obstacles_nb = radar_update (&robot_pos, main_obstacles_pos);
+ move_obstacles_update ();
+ simu_send_pos_report (main_obstacles_pos, main_obstacles_nb, 0);
+ }
+ /* Update AI modules. */
+ logistic_update ();
+ path_decay ();
/* Only manage events if slaves are synchronised. */
if (twi_master_sync ())
main_event_to_fsm ();
@@ -146,9 +236,21 @@ main_loop (void)
}
if (main_stats_contact_ && !--main_stats_contact_cpt_)
{
- proto_send1d ('P', contact_all ());
+ proto_send1d ('P', contact_all () | (uint32_t) mimot_get_input () << 24);
main_stats_contact_cpt_ = main_stats_contact_;
}
+ if (main_stats_codebar_ && !--main_stats_codebar_cpt_)
+ {
+ proto_send2b ('B', codebar_get (DIRECTION_FORWARD),
+ codebar_get (DIRECTION_BACKWARD));
+ main_stats_codebar_cpt_ = main_stats_codebar_;
+ }
+ if (main_stats_usdist_ && !--main_stats_usdist_cpt_)
+ {
+ proto_send4w ('U', usdist_mm[0], usdist_mm[1], usdist_mm[2],
+ usdist_mm[3]);
+ main_stats_usdist_cpt_ = main_stats_usdist_;
+ }
}
}
@@ -163,6 +265,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Reset */
utils_reset ();
break;
+ case c ('j', 0):
+ /* Simulate jack insertion. */
+ fsm_queue_post_event (FSM_EVENT (AI, jack_inserted));
+ break;
case c ('w', 3):
/* Set PWM.
* - 1b: index.
@@ -179,11 +285,64 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
v8_to_v16 (args[3], args[4]),
v8_to_v16 (args[5], args[6]));
break;
+ case c ('m', 5):
+ /* Go to position.
+ * - 2w: x, y.
+ * - 1b: backward. */
+ {
+ vect_t position = { v8_to_v16 (args[0], args[1]),
+ v8_to_v16 (args[2], args[3]) };
+ move_start_noangle (position, args[4], 0);
+ }
+ break;
case c ('c', 1):
/* Move clamp.
* - 1b: position. */
clamp_move (args[0]);
break;
+ case c ('c', 2):
+ /* Move element using clamp.
+ * - 1b: source.
+ * - 1b: destination. */
+ clamp_move_element (args[0], args[1]);
+ break;
+ case c ('n', 2):
+ /* Simulate the presence of a new element.
+ * - 1b: position.
+ * - 1b: type. */
+ clamp_new_element (args[0], args[1]);
+ break;
+ case c ('d', 0):
+ /* Open all doors. */
+ pwm_set_timed (BOT_PWM_DOOR_FRONT_BOTTOM,
+ BOT_PWM_DOOR_OPEN (CLAMP_SLOT_FRONT_BOTTOM));
+ pwm_set_timed (BOT_PWM_DOOR_FRONT_TOP,
+ BOT_PWM_DOOR_OPEN (CLAMP_SLOT_FRONT_TOP));
+ pwm_set_timed (BOT_PWM_DOOR_BACK_BOTTOM,
+ BOT_PWM_DOOR_OPEN (CLAMP_SLOT_BACK_BOTTOM));
+ pwm_set_timed (BOT_PWM_DOOR_BACK_TOP,
+ BOT_PWM_DOOR_OPEN (CLAMP_SLOT_BACK_TOP));
+ break;
+ case c ('d', 1):
+ /* Drop elements.
+ * - 1b: 00: drop clear, 01: drop forward, 02: drop backward. */
+ if (args[0] == 0x00)
+ clamp_drop_clear ();
+ else
+ clamp_drop (args[0]);
+ break;
+ case c ('d', 2):
+ /* Open or close door or clamp.
+ * - 1b: pos, or 0xff for clamp.
+ * - 1b: non zero to open. */
+ clamp_door (args[0], args[1]);
+ break;
+ case c ('w', 0):
+ /* Disable all motor control. */
+ mimot_motor0_free ();
+ mimot_motor1_free ();
+ asserv_free_motor ();
+ break;
/* Stats commands.
* - b: interval between stats. */
case c ('A', 1):
@@ -194,6 +353,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Contact stats. */
main_stats_contact_ = main_stats_contact_cpt_ = args[0];
break;
+ case c ('B', 1):
+ /* Codebar stats. */
+ main_stats_codebar_ = main_stats_codebar_cpt_ = args[0];
+ break;
+ case c ('U', 1):
+ /* US sensors stats. */
+ main_stats_usdist_ = main_stats_usdist_cpt_ = args[0];
+ break;
default:
/* Unknown commands */
proto_send0 ('?');