From 72ffb034aa75afc018bf97a9868620c107e146cb Mon Sep 17 00:00:00 2001
From: petr <silhavik.p@gmail.com>
Date: Sun, 18 Mar 2012 15:36:08 +0100
Subject: [PATCH] robofsm: competition2012

Finished first idea of competition trajectory.

Homologation + collecting the buillons from the nearest totem. After the buillons are collected, they are transported into captain's cabin.
---
 src/robofsm/Makefile.omk       |  6 +--
 src/robofsm/competition2012.cc | 68 +++++++++++++++++++++++++++++++---
 2 files changed, 65 insertions(+), 9 deletions(-)

diff --git a/src/robofsm/Makefile.omk b/src/robofsm/Makefile.omk
index c3f31434..6a513c01 100644
--- a/src/robofsm/Makefile.omk
+++ b/src/robofsm/Makefile.omk
@@ -7,9 +7,9 @@ default_CONFIG = CONFIG_LOCK_CHECKING=n HAVE_PRIO_INHERIT=y
 config_include_HEADERS = robot_config.h
 robot_config_DEFINES = CONFIG_LOCK_CHECKING HAVE_PRIO_INHERIT
 
-#bin_PROGRAMS += competition
-competition_SOURCES = competition.cc	\
-		      common-states.cc
+bin_PROGRAMS += competition
+competition_SOURCES = competition2012.cc#	\
+#		      common-states.cc
 
 bin_PROGRAMS += homologation
 homologation_SOURCES = homologation2012.cc
diff --git a/src/robofsm/competition2012.cc b/src/robofsm/competition2012.cc
index a48f94ba..2c8ab1c0 100644
--- a/src/robofsm/competition2012.cc
+++ b/src/robofsm/competition2012.cc
@@ -49,6 +49,11 @@ FSM_STATE_DECL(leave_bottle);
 FSM_STATE_DECL(goto_totem);
 FSM_STATE_DECL(approach_totem);
 FSM_STATE_DECL(leave_totem);
+
+FSM_STATE_DECL(goto_totem2);
+FSM_STATE_DECL(approach_totem2);
+FSM_STATE_DECL(leave_totem2);
+
 FSM_STATE_DECL(go_home);
 
 FSM_STATE(init)
@@ -280,14 +285,13 @@ FSM_STATE(leave_totem)
 				NO_TURN());
 			break;
 		case EV_MOTION_DONE:
-			FSM_TRANSITION(go_home);
+			FSM_TRANSITION(goto_totem2);
 		default:
 			break;
 	}
 }
 
-
-FSM_STATE(go_home)
+FSM_STATE(goto_totem2)
 {
 	switch(FSM_EVENT) {
 		case EV_ENTRY:
@@ -297,10 +301,62 @@ FSM_STATE(go_home)
 				0.6);
 			robot_trajectory_add_point_trans(
 				0.64,
-				1.0);
+				1.3);
+			robot_trajectory_add_final_point_trans(
+				1.1,
+				1.6,
+				ARRIVE_FROM(DEG2RAD(270),0.1));
+			break;
+		case EV_MOTION_DONE:
+			FSM_TRANSITION(approach_totem2);
+		default:
+			break;
+	}
+}
+
+
+FSM_STATE(approach_totem2)
+{
+	switch(FSM_EVENT) {
+		case EV_ENTRY:
+			robot_trajectory_new(&tcSlow); // new trajectory
+			robot_trajectory_add_final_point_trans(
+				1.1,
+				1.125 + ROBOT_AXIS_TO_FRONT_M + 0.05,
+				ARRIVE_FROM(DEG2RAD(270), 0.10));
+			break;
+		case EV_MOTION_DONE:
+			FSM_TRANSITION(leave_totem2);
+		default:
+			break;
+	}
+}
+
+FSM_STATE(leave_totem2)
+{
+	switch(FSM_EVENT) {
+		case EV_ENTRY:
+			robot_trajectory_new_backward(&tcSlow); // new trajectory
+			robot_trajectory_add_final_point_trans(
+				1.1,
+				1.6,
+				NO_TURN());
+			break;
+		case EV_MOTION_DONE:
+			FSM_TRANSITION(go_home);
+		default:
+			break;
+	}
+}
+
+FSM_STATE(go_home)
+{
+	switch(FSM_EVENT) {
+		case EV_ENTRY:
+			robot_trajectory_new(&tcSlow); // new trajectory
 			robot_trajectory_add_final_point_trans(
-				0.35,
-				1.0,
+				0.5 + ROBOT_AXIS_TO_FRONT_M,
+				1.75,
 				ARRIVE_FROM(DEG2RAD(180),0.1));
 			break;
 		case EV_MOTION_DONE:
-- 
2.39.2