From: Filip Jares <filipjares@post.cz>
Date: Fri, 30 Apr 2010 12:19:58 +0000 (+0200)
Subject: homologation: experiment with corns rushing (hrnuti)
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/693f906ab20086bae0cadb2c835a6879d6cc991a

homologation: experiment with corns rushing (hrnuti)
---

diff --git a/src/robofsm/eb2010misc.cc b/src/robofsm/eb2010misc.cc
index 66545ba5..62e5a6ab 100644
--- a/src/robofsm/eb2010misc.cc
+++ b/src/robofsm/eb2010misc.cc
@@ -9,7 +9,6 @@
  * Functions to manipulate map, compute distances, choose corns etc.
  ************************************************************************/
 
-
 // returns pointer to next real (non-fake) corn which was not yet collected
 // TODO: note: use this for "short_time_to_end: situation only, otherwise
 // it is better to try to rush more corns at once
@@ -17,21 +16,24 @@ struct corn * choose_next_corn()
 {
 	Point cornPosition;
 
-	double minDistance = 2 * PLAYGROUND_HEIGHT_M; // "infinity"
+	double maxDistance = 0; // 2 * PLAYGROUND_HEIGHT_M; // "infinity"	
 	struct corn *minCorn = NULL, *corn;
 	// robot.corns->corns[NUM_OF_FAKE_CORNS] is first non-fake corn in the robot.corns structure
 	for(corn = &robot.corns->corns[NUM_OF_FAKE_CORNS]; corn < &robot.corns->corns[robot.corns->corns_count]; corn++) {
 		cornPosition.x = corn->position.x;
 		cornPosition.y = corn->position.y;
 		double distance = cornPosition.distanceTo(containerPosition);
+		printf("maxDistance = %.3f; new corn dist = %.3f, it x position = %.3f; %s\n",
+			maxDistance, distance, corn->position.x, (corn->was_collected?"W":"N"));
 		
-		if (distance < minDistance && corn->was_collected == false) {
-			minDistance = distance;
+		if (distance > maxDistance && corn->position.x > 2.2 && corn->was_collected == false) {
+			maxDistance = distance;
 			minCorn = corn;
+			printf("\tchoose it\n");
 		}
 	}
 
-	if (minCorn) printf("\tmin distance was: %.3f  ", minDistance);
+	if (minCorn) printf("\tmin distance was: %.3f  ", maxDistance);
 	return minCorn;
 }
 
@@ -56,8 +58,8 @@ Pos * get_corn_approach_position(struct corn *corn)
 	Point cornPosition(corn->position.x, corn->position.y);
 	double a = approxContainerPosition.angleTo(cornPosition);
 
-	p->x = cornPosition.x + cos(a)*(CORN_NEIGHBOURHOOD_RADIUS_M + 0.1);
-	p->y = cornPosition.y + sin(a)*(CORN_NEIGHBOURHOOD_RADIUS_M + 0.1);
+	p->x = cornPosition.x + cos(a)*(CORN_NEIGHBOURHOOD_RADIUS_M + 0.07);
+	p->y = cornPosition.y + sin(a)*(CORN_NEIGHBOURHOOD_RADIUS_M + 0.07);
 	p->phi = a + M_PI;
 
 	return p;
diff --git a/src/robofsm/homologation.cc b/src/robofsm/homologation.cc
index 7cca8817..b852fe5f 100644
--- a/src/robofsm/homologation.cc
+++ b/src/robofsm/homologation.cc
@@ -221,10 +221,14 @@ FSM_STATE(to_container_diag)
 			robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05));
 			break;
 		case EV_MOTION_DONE:
+			FSM_TIMER(10000);
 			act_vidle(VIDLE_DOWN);
 			break;
-		case EV_START:
 		case EV_TIMER:
+			//act_vidle(VIDLE_UP);
+			//FSM_TRANSITION(experiment);
+			break;
+		case EV_START:
 		case EV_RETURN:
 		case EV_ACTION_DONE:
 		case EV_ACTION_ERROR:
@@ -268,26 +272,43 @@ FSM_STATE(to_container_ortho)
 	}
 }
 
+enum where_to_go {
+	CORN,
+	CONTAINER
+};
+
 static int cnt = 0;
 FSM_STATE(experiment)
 {
+	static enum where_to_go where_to_go;
+	static struct corn *corn_to_get;
 	switch(FSM_EVENT) {
 		case EV_ENTRY: {
 				double x, y, phi;
 				robot_get_est_pos(&x, &y, &phi);
-				printf("experiment: corn cnt: %d, est pos %.3f, %.3f, %.3f\n",
+				printf("experiment: puck cnt: %d, est pos %.3f, %.3f, %.3f\n",
 					cnt, x, y, phi);
-				struct corn * corn = choose_next_corn();
-				Pos *p = get_corn_approach_position(corn /*&robot.corns->corns[cnt++]*/);
-				corn->was_collected = true;
-				//robot_trajectory_new(&tcFast);
-				//robot_trajectory_add_final_point_trans(robot_trajectory_add_final_point_trans(p->x, p->y, TURN(p->phi));
-				robot_goto_trans(p->x, p->y, TURN(p->phi), &tcFast);
-				delete(p);
+				corn_to_get = choose_next_corn();
+				if (corn_to_get) {
+					Pos *p = get_corn_approach_position(corn_to_get);
+					corn_to_get->was_collected = true;
+					//robot_trajectory_new(&tcFast);
+					//robot_trajectory_add_final_point_trans(robot_trajectory_add_final_point_trans(p->x, p->y, TURN(p->phi));
+					robot_goto_trans(p->x, p->y, TURN(p->phi), &tcFast);
+					delete(p);
+				}
+				where_to_go = CONTAINER;
 				break;
 			}
 		case EV_MOTION_DONE:
-			FSM_TRANSITION(experiment);
+			cnt++;
+			if (where_to_go == CORN) {
+				FSM_TRANSITION(experiment);
+			} else {
+				remove_wall_around_corn(corn_to_get);
+				robot_goto_trans(PLAYGROUND_WIDTH_M - 0.4, 0.15, ARRIVE_FROM(DEG2RAD(-90), 0.02), &tcFast);
+				where_to_go = CORN;
+			}
 			break;
 		case EV_START:
 		case EV_TIMER:
@@ -301,8 +322,6 @@ FSM_STATE(experiment)
 	}
 }
 
-
-
 /************************************************************************
  * MAIN FUNCTION
  ************************************************************************/