robofsm: Add pick two green figures strategy.

diff --git a/src/robofsm/strategy_pick_two_our_figures.cc b/src/robofsm/strategy_pick_two_our_figures.cc
new file mode 100644 (file)
index 0000000..a97fa2d
--- /dev/null
+++ b/src/robofsm/strategy_pick_two_our_figures.cc
@@ -0,0 +1,109 @@
+#include "common-states.h"
+#include "robot.h"
+#include <ul_log.h>
+
+UL_LOG_CUST(ulogd_strategy_pick_two_our_figures); /* Log domain name = ulogd + name of the file */
+
+static FSM_STATE_DECL(bypass_figure);
+
+FSM_STATE(start_pick_two_our_figures)
+{
+       switch (FSM_EVENT) {
+               case EV_ENTRY:
+                       start_entry();
+//#ifdef COMPETITION
+                       ul_logmsg("waiting for start\n");
+                       FSM_TIMER(1000);
+                       break;
+//#endif
+               case EV_START:
+                       start_go();
+                       FSM_TRANSITION(bypass_figure);
+                       break;
+               case EV_TIMER:
+                       FSM_TIMER(1000);
+                        read_sharp();
+                       start_timer();
+                       break;
+               case EV_EXIT:
+                       start_exit();
+                       break;
+               case EV_SWITCH_STRATEGY:
+                       FSM_TRANSITION(start_pick_third_figure);
+                       break;
+               default:;
+       }
+}
+
+
+#undef FSM_STATE_VISIBILITY
+#define FSM_STATE_VISIBILITY static
+
+FSM_STATE_DECL(pick_second_green_figure);
+FSM_STATE_DECL(pick_third_green_figure);
+FSM_STATE_DECL(move_around_opp_squares);
+FSM_STATE_DECL(pick_opp_bonus_figure);
+
+FSM_STATE(bypass_figure)
+{
+       switch (FSM_EVENT) {
+       case EV_ENTRY:
+               SUBFSM_TRANSITION(bypass_figure_in_front_of_start, NULL);
+               break;
+       case EV_RETURN:
+               FSM_TRANSITION(pick_second_green_figure);
+               break;
+       default: break;
+       }
+}
+
+FSM_STATE(pick_second_green_figure)
+{
+       switch (FSM_EVENT) {
+       case EV_ENTRY:
+               SUBFSM_TRANSITION(approach_second_green_figure, NULL);
+               break;
+       case EV_RETURN:
+               FSM_TRANSITION(pick_third_green_figure);
+               break;
+       default: break;
+       }
+}
+
+FSM_STATE(pick_third_green_figure)
+{
+       switch (FSM_EVENT) {
+       case EV_ENTRY:
+               SUBFSM_TRANSITION(approach_third_green_figure, NULL);
+               break;
+       case EV_RETURN:
+               FSM_TRANSITION(move_around_opp_squares);
+               break;
+       default: break;
+       }
+}
+
+FSM_STATE(move_around_opp_squares)
+{
+        switch (FSM_EVENT) {
+        case EV_ENTRY:
+                SUBFSM_TRANSITION(move_around, NULL);
+                break;
+        case EV_RETURN:
+                FSM_TRANSITION(pick_opp_bonus_figure);
+                break;
+        default: break;
+        }
+}
+
+FSM_STATE(pick_opp_bonus_figure)
+{
+        switch (FSM_EVENT) {
+        case EV_ENTRY:
+                SUBFSM_TRANSITION(approach_opp_bonus_figure, NULL);
+                break;
+        case EV_RETURN:
+                break;
+        default: break;
+        }
+}
\ No newline at end of file