robofsm: add "real" six_oranges strategy (copy and modify the 12 oranges strategy)

diff --git a/src/robofsm/Makefile.omk b/src/robofsm/Makefile.omk
index e14e920b494189df5a0ef417847791e017fdeaa0..726e7325b9560ebfd9ee86c467654d683a1f9656 100644 (file)
--- a/src/robofsm/Makefile.omk
+++ b/src/robofsm/Makefile.omk
@@ -10,7 +10,7 @@ robot_config_DEFINES = CONFIG_LOCK_CHECKING HAVE_PRIO_INHERIT
 bin_PROGRAMS += competition
 competition_SOURCES = competition.cc common-states.cc                  \
                      strategy_opp_corn.cc strategy_opp_oranges.cc      \
-                     strategy_12_oranges.cc
+                     strategy_12_oranges.cc strategy_six_oranges.cc
 
 bin_PROGRAMS += homologation
 homologation_SOURCES = homologation.cc

diff --git a/src/robofsm/common-states.h b/src/robofsm/common-states.h
index 23d7e84b17026792d24b8aeae37aadf223fec843..52573fc0036397817026de69facadd697b2ebde1 100644 (file)
--- a/src/robofsm/common-states.h
+++ b/src/robofsm/common-states.h
@@ -8,9 +8,10 @@
 
 extern struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
 
-FSM_STATE_DECL(start_opp_oranges);
+FSM_STATE_DECL(start_six_oranges);
 FSM_STATE_DECL(start_12_oranges);
 FSM_STATE_DECL(start_opp_corn);
+FSM_STATE_DECL(start_opp_oranges);
 
 
 FSM_STATE_DECL(climb_the_slope);

diff --git a/src/robofsm/competition.cc b/src/robofsm/competition.cc
index 0c31b0385f093cc1e447dfa2f20d13248697a9f4..0255eb3b39ebc0f0a5bc5958ccc63f2a7d23d54c 100644 (file)
--- a/src/robofsm/competition.cc
+++ b/src/robofsm/competition.cc
@@ -46,7 +46,7 @@ int main()
 
        //robot.fsm.main.state = &fsm_state_main_start_opp_corn;
        //robot.fsm.main.state = &fsm_state_main_start_opp_oranges;
-       robot.fsm.main.state = &fsm_state_main_start_12_oranges;
+       robot.fsm.main.state = &fsm_state_main_start_six_oranges;
 
        //robot.fsm.main.transition_callback = trans_callback;
        //robot.fsm.motion.transition_callback = move_trans_callback;

diff --git a/src/robofsm/strategy_opp_oranges.cc b/src/robofsm/strategy_opp_oranges.cc
index 608078e481aff1edfece09aec25cf625150cb3db..86a57812348c62e6d5cdb560d13f0bd65d2084ba 100644 (file)
--- a/src/robofsm/strategy_opp_oranges.cc
+++ b/src/robofsm/strategy_opp_oranges.cc
@@ -26,7 +26,7 @@ FSM_STATE(start_opp_oranges)
                        start_exit();
                        break;
                case EV_SWITCH_STRATEGY:
-                       FSM_TRANSITION(start_12_oranges);
+                       FSM_TRANSITION(start_six_oranges);
                        break;
                default:;
        }

diff --git a/src/robofsm/strategy_six_oranges.cc b/src/robofsm/strategy_six_oranges.cc
new file mode 100644 (file)
index 0000000..c3e8e4d
--- /dev/null
+++ b/src/robofsm/strategy_six_oranges.cc
@@ -0,0 +1,78 @@
+#include "common-states.h"
+#include "robot.h"
+#include "eb2010misc.h"
+
+static FSM_STATE_DECL(pick_our_oranges);
+
+FSM_STATE(start_six_oranges)
+{
+       switch (FSM_EVENT) {
+               case EV_ENTRY:
+                       start_entry();
+#ifdef COMPETITION
+                       printf("waiting for start\n");
+                       FSM_TIMER(1000);
+                       break;
+#endif
+               case EV_START:
+                       start_go();
+                       FSM_TRANSITION(pick_our_oranges);
+                       break;
+               case EV_TIMER:
+                       FSM_TIMER(1000);
+                       start_timer();
+                       break;
+               case EV_EXIT:
+                       start_exit();
+                       break;
+               case EV_SWITCH_STRATEGY:
+                       FSM_TRANSITION(start_12_oranges);
+                       break;
+               default:;
+       }
+}
+
+
+#undef FSM_STATE_VISIBILITY
+#define FSM_STATE_VISIBILITY static
+
+FSM_STATE_DECL(pick_rest_of_our_oranges);
+FSM_STATE_DECL(unload_oranges);
+FSM_STATE_DECL(pick_opp_oranges);
+FSM_STATE_DECL(opp_corns);
+
+FSM_STATE(pick_our_oranges)
+{
+       switch (FSM_EVENT) {
+       case EV_ENTRY:  SUBFSM_TRANSITION(approach_the_slope, new slope_approach_style(MINE, NEAR_PLAYGROUND_BOUNDARY)); break;
+       case EV_RETURN: FSM_TRANSITION(pick_rest_of_our_oranges); break;
+       default: break;
+       }
+}
+
+FSM_STATE(pick_rest_of_our_oranges)
+{
+       switch (FSM_EVENT) {
+       case EV_ENTRY:  SUBFSM_TRANSITION(approach_the_slope, new slope_approach_style(MINE, NEAR_PLAYGROUND_CENTER)); break;
+       case EV_RETURN: FSM_TRANSITION(unload_oranges); break;
+       default: break;
+       }
+}
+
+FSM_STATE(unload_oranges)
+{
+       switch (FSM_EVENT) {
+       case EV_ENTRY:  SUBFSM_TRANSITION(to_cntainer_and_unld, NULL); break;
+       case EV_RETURN: FSM_TRANSITION(opp_corns); break;
+       default: break;
+       }
+}
+
+FSM_STATE(opp_corns)
+{
+       switch (FSM_EVENT) {
+       case EV_ENTRY:  SUBFSM_TRANSITION(approach_next_corn, NULL); break;
+       case EV_RETURN: FSM_TRANSITION(opp_corns); break;
+       default: break;
+       }
+}