From: petr <silhavik.p@gmail.com>
Date: Tue, 8 May 2012 10:17:44 +0000 (+0200)
Subject: robofsm: Strategy
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/3aba54ef7544200cd8eeae0bc3eecdd67b01ba68?ds=sidebyside

robofsm: Strategy


Include homologation as strategy into competition machine.
---

diff --git a/src/robofsm/Makefile.omk b/src/robofsm/Makefile.omk
index 7d33d226..7ab36b90 100644
--- 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 = competition2012.cc	\
 		      common-states.cc strategy_get_central_buillon.cc \
-		      strategy_odo_calibration.cc
+		      strategy_homologation.cc strategy_odo_calibration.cc
 
 bin_PROGRAMS += homologation
 homologation_SOURCES = homologation2012.cc
diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index 1975f71a..7db13184 100644
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -423,6 +423,54 @@ FSM_STATE(leave_totem_up)
 	}
 }
 
+/* Homologation states */
+FSM_STATE(homolog_approach_buillon)
+{
+	switch(FSM_EVENT) {
+		case EV_ENTRY:
+			robot_trajectory_new(&tcSlow); // new trajectory
+			robot_trajectory_add_point_trans(
+				0.65,
+				PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
+			robot_trajectory_add_point_trans(
+				0.65,
+				1.3);
+			robot_trajectory_add_final_point_trans(
+				0.5,
+				1.1,
+				NO_TURN());
+			break;
+		case EV_MOTION_DONE:
+		case EV_TIMER:
+			FSM_TRANSITION(homolog_push_bottle);
+			break;
+		default:
+			break;
+	}
+}
+
+FSM_STATE(homolog_push_bottle)
+{
+	switch(FSM_EVENT) {
+		case EV_ENTRY:
+			robot_trajectory_new(&tcSlow); // new trajectory
+			robot_trajectory_add_point_trans(
+				0.64,
+				0.7);
+			robot_trajectory_add_final_point_trans(
+				0.64 + 0.08,
+				ROBOT_AXIS_TO_FRONT_M + 0.05,
+				ARRIVE_FROM(DEG2RAD(270), 0.10));
+			break;
+		case EV_MOTION_DONE:
+			SUBFSM_RET(NULL);
+			break;
+		default:
+			break;
+	}
+}
+
+
 /* State for odometry calibration */
 
 FSM_STATE(go_back_for_cal)
diff --git a/src/robofsm/common-states.h b/src/robofsm/common-states.h
index 7dc1c4d0..029684e8 100644
--- a/src/robofsm/common-states.h
+++ b/src/robofsm/common-states.h
@@ -11,7 +11,7 @@ extern double totem_x, totem_y;
 extern bool up;
 /* strategy FSM */
 FSM_STATE_DECL(get_central_buillon_first);
-FSM_STATE_DECL(ignore_central_buillon);
+FSM_STATE_DECL(homolog_wait_for_start);
 FSM_STATE_DECL(calibrate);
 
 /* Strategy catch buillon in center */
@@ -47,6 +47,11 @@ FSM_STATE_DECL(place_up_buillon);
 
 FSM_STATE_DECL(push_second_bottle);
 */
+/* HOMOLOGATION states */
+/* movement states - buillon */
+FSM_STATE_DECL(homolog_approach_buillon);
+/* Pushing the bottle */
+FSM_STATE_DECL(homolog_push_bottle);
 void start_entry();
 void start_timer();
 void start_go();
diff --git a/src/robofsm/strategy_get_central_buillon.cc b/src/robofsm/strategy_get_central_buillon.cc
index d5012c7c..d82a961a 100644
--- a/src/robofsm/strategy_get_central_buillon.cc
+++ b/src/robofsm/strategy_get_central_buillon.cc
@@ -12,11 +12,11 @@ FSM_STATE(get_central_buillon_first)
 	switch (FSM_EVENT) {
 		case EV_ENTRY:
 			start_entry();
-#ifdef COMPETITION
+//#ifdef COMPETITION
 			ul_logmsg("waiting for start\n");
 			FSM_TIMER(1000);
 			break;
-#endif
+//#endif
 		case EV_START:
 			start_go();
 			FSM_TRANSITION(pick_central_buillon);
@@ -29,7 +29,7 @@ FSM_STATE(get_central_buillon_first)
 			start_exit();
 			break;
 		case EV_SWITCH_STRATEGY:
-			FSM_TRANSITION(calibrate);
+			FSM_TRANSITION(homolog_wait_for_start);
 			break;
 		default:;
 	}
diff --git a/src/robofsm/strategy_homologation.cc b/src/robofsm/strategy_homologation.cc
new file mode 100644
index 00000000..bea54155
--- /dev/null
+++ b/src/robofsm/strategy_homologation.cc
@@ -0,0 +1,37 @@
+#include "common-states.h"
+#include "robot.h"
+#include <ul_log.h>
+
+
+UL_LOG_CUST(ulogd_strategy_homologation); /* Log domain name = ulogd + name of the file */
+
+
+FSM_STATE(homolog_wait_for_start)
+{
+	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();
+			SUBFSM_TRANSITION(homolog_approach_buillon, NULL);
+			break;
+		case EV_TIMER:
+			FSM_TIMER(1000);
+			start_timer();
+			break;
+		case EV_EXIT:
+			start_exit();
+			break;
+		case EV_SWITCH_STRATEGY:
+			FSM_TRANSITION(calibrate);
+			break;
+		case EV_RETURN:
+			break;
+		default:;
+	}
+}
\ No newline at end of file