From: Filip Jares <filipjares@post.cz>
Date: Sat, 29 May 2010 19:27:49 +0000 (+0200)
Subject: robofsm: jerk faster, go slower through the diagonal, add rush_last_cms for corns
X-Git-Tag: start2011~5
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/3fcb5ac32f3394bba6303de327a40159158259e1

robofsm: jerk faster, go slower through the diagonal, add rush_last_cms for corns
---

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index 431fd989..f117a214 100644
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -356,9 +356,11 @@ FSM_STATE(sledge_down)
 
 FSM_STATE(to_cntainer_and_unld)
 {
+	TrajectoryConstraints tcNotSoFast = trajectoryConstraintsDefault;
+	tcNotSoFast.maxv = 0.5; tcNotSoFast.maxacc = 0.3; tcNotSoFast.maxomega = 2;
 	switch(FSM_EVENT) {
 		case EV_ENTRY:
-			robot_goto_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05), &tcFast);
+			robot_goto_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05), &tcNotSoFast);
 			break;
 		case EV_MOTION_DONE:
 			FSM_TIMER(1000); // FIXME: test this
@@ -384,7 +386,7 @@ FSM_STATE(jerk)
 	switch(FSM_EVENT) {
 		case EV_ENTRY:
 			move_cnt = 0;
-			robot_move_by(-0.05, NO_TURN(), &tcSlow);
+			robot_move_by(-0.05, NO_TURN(), &tcJerk);
 			break;
 		case EV_MOTION_DONE:
 			if (move_cnt == 0) {
@@ -501,7 +503,7 @@ FSM_STATE(rush_the_corn)
 			where_to_go = TURN_AROUND;
 			break;
 		case EV_MOTION_DONE:
-			FSM_TRANSITION(rush_corns_decider);
+			FSM_TRANSITION(rush_last_cms);
 			break;
 		case EV_START:
 		case EV_TIMER:
@@ -515,6 +517,38 @@ FSM_STATE(rush_the_corn)
 	}
 }
 
+FSM_STATE(rush_last_cms)
+{
+	static char move_cnt;
+	switch(FSM_EVENT) {
+		case EV_ENTRY:
+			robot.ignore_hokuyo = true;
+			robot.check_turn_safety = false;
+			move_cnt = 0;
+			robot_move_by(0.08, NO_TURN(), &tcSlow);
+			break;
+		case EV_MOTION_DONE:
+			if (move_cnt == 0) {
+				robot_move_by(-0.08, NO_TURN(), &tcSlow);
+			} else if (move_cnt > 0) {
+				FSM_TRANSITION(rush_corns_decider);
+			}
+			move_cnt++;
+			break;
+		case EV_START:
+		case EV_TIMER:
+		case EV_RETURN:
+		case EV_VIDLE_DONE:
+		case EV_MOTION_ERROR:
+		case EV_SWITCH_STRATEGY:
+			DBG_PRINT_EVENT("unhandled event");
+		case EV_EXIT:
+			robot.ignore_hokuyo = false;
+			robot.check_turn_safety = true;
+			break;
+	}
+}
+
 // used to perform the maneuvre
 FSM_STATE(turn_around)
 {
diff --git a/src/robofsm/common-states.h b/src/robofsm/common-states.h
index 952a40bd..4d683ad8 100644
--- a/src/robofsm/common-states.h
+++ b/src/robofsm/common-states.h
@@ -22,6 +22,7 @@ FSM_STATE_DECL(jerk);
 FSM_STATE_DECL(rush_corns_decider);
 FSM_STATE_DECL(approach_next_corn);
 FSM_STATE_DECL(rush_the_corn);
+FSM_STATE_DECL(rush_last_cms);
 FSM_STATE_DECL(turn_around);
 FSM_STATE_DECL(approach_the_slope);
 
diff --git a/src/robofsm/competition.cc b/src/robofsm/competition.cc
index bc9279b5..8edd27cc 100644
--- a/src/robofsm/competition.cc
+++ b/src/robofsm/competition.cc
@@ -52,8 +52,8 @@ int main()
 	//robot.fsm.motion.transition_callback = move_trans_callback;
 
 	tcJerk = trajectoryConstraintsDefault;
-	tcJerk.maxv = 1;
-	tcJerk.maxacc = 1.5;
+	tcJerk.maxv = 1.5;
+	tcJerk.maxacc = 5;
 	tcJerk.maxomega = 2;
 	tcFast = trajectoryConstraintsDefault;
 	tcFast.maxv = 1;