X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_base.c;h=1f3b6e5380ff4bd69ed4bb2a0c12ff6ed8acf81e;hp=2c527780c7bf9ecafe8d67709144f6ed8ec82c7b;hb=4e7801883ed4076cb14b63a0571467747894c0f8;hpb=fa8546ea39c7442ad3bf5a822a72a2b50a41045d

diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c
index 2c52778..1f3b6e5 100644
--- a/projects/microb2010/mainboard/strat_base.c
+++ b/projects/microb2010/mainboard/strat_base.c
@@ -1,6 +1,6 @@
-/*  
+/*
  *  Copyright Droids Corporation, Microb Technology (2009)
- * 
+ *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
  *  the Free Software Foundation; either version 2 of the License, or
@@ -101,7 +101,7 @@ const char *get_err(uint8_t err)
 	return "END_UNKNOWN";
 }
 
-void strat_hardstop(void) 
+void strat_hardstop(void)
 {
 	trajectory_hardstop(&mainboard.traj);
 	pid_reset(&mainboard.angle.pid);
@@ -140,7 +140,7 @@ uint8_t strat_goto_xy_force(int16_t x, int16_t y)
 	return END_BLOCKING;
 }
 
-/* reset position */ 
+/* reset position */
 void strat_reset_pos(int16_t x, int16_t y, int16_t a)
 {
 	int16_t posx = position_get_x_s16(&mainboard.pos);
@@ -162,7 +162,7 @@ void strat_reset_pos(int16_t x, int16_t y, int16_t a)
 	DEBUG(E_USER_STRAT, "pos resetted", __FUNCTION__);
 }
 
-/* 
+/*
  * decrease gain on angle PID, and go forward until we reach the
  * border.
  */
@@ -171,12 +171,17 @@ uint8_t strat_calib(int16_t dist, uint8_t flags)
 	int32_t p = pid_get_gain_P(&mainboard.angle.pid);
 	int32_t i = pid_get_gain_I(&mainboard.angle.pid);
 	int32_t d = pid_get_gain_D(&mainboard.angle.pid);
+	int32_t max_in = pid_get_max_in(&mainboard.angle.pid);
+	int32_t max_i = pid_get_max_I(&mainboard.angle.pid);
+	int32_t max_out = pid_get_max_out(&mainboard.angle.pid);
 	uint8_t err;
 
-	pid_set_gains(&mainboard.angle.pid, 150, 0, 2000);
+	pid_set_maximums(&mainboard.distance.pid, 0, 20000, 1000);
+	pid_set_gains(&mainboard.angle.pid, 200, 0, 2000);
 	trajectory_d_rel(&mainboard.traj, dist);
 	err = wait_traj_end(flags);
 	pid_set_gains(&mainboard.angle.pid, p, i, d);
+	pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out);
 	return err;
 }
 
@@ -190,7 +195,7 @@ static void strat_update_traj_speed(void)
 	a = strat_speed_a;
 	if (strat_limit_speed_a && a > strat_limit_speed_a)
 		a = strat_limit_speed_a;
-	
+
 	trajectory_set_speed(&mainboard.traj, d, a);
 }
 
@@ -272,14 +277,16 @@ void strat_limit_speed(void)
 
 /* start the strat */
 void strat_start(void)
-{ 
-	int8_t i;
+{
 	uint8_t err;
 
 	strat_preinit();
 
+#ifndef HOST_VERSION
 	/* if start sw not plugged */
 	if (sensor_get(S_START_SWITCH)) {
+		int8_t i;
+
 		printf_P(PSTR("No start switch, press a key or plug it\r\n"));
 
 		/* while start sw not plugged */
@@ -294,16 +301,18 @@ void strat_start(void)
 			break;
 		}
 	}
-	
+
 	/* if start sw plugged */
 	if (!sensor_get(S_START_SWITCH)) {
 		printf_P(PSTR("Ready, unplug start switch to start\r\n"));
 		/* while start sw plugged */
 		while (!sensor_get(S_START_SWITCH));
 	}
+#endif
 
 	strat_init();
 	err = strat_main();
+	printf("coucou\n");
 	NOTICE(E_USER_STRAT, "Finished !! returned %s", get_err(err));
 	strat_exit();
 }
@@ -311,6 +320,7 @@ void strat_start(void)
 /* return true if we have to brake due to an obstacle */
 uint8_t strat_obstacle(void)
 {
+#if 0
 	int16_t x_rel, y_rel;
 	int16_t opp_x, opp_y, opp_d, opp_a;
 
@@ -328,6 +338,25 @@ uint8_t strat_obstacle(void)
 	opponent_obstacle.y = opp_y;
 	opponent_obstacle.d = opp_d;
 	opponent_obstacle.a = opp_a;
+#else /* belgium cup only */
+	int16_t x_rel, y_rel;
+	int16_t opp_d, opp_a;
+	double opp_x, opp_y;
+
+#ifdef HOST_VERSION
+	if (time_get_s() >= 12 && time_get_s() <= 30)
+		return 1;
+#endif
+	if (!sensor_get(S_RCOB_WHITE))
+		return 0;
+
+	opp_a = 0;
+	opp_d = 300;
+
+	rel_da_to_abs_xy(opp_d, RAD(opp_a), &opp_x, &opp_y);
+	if (!is_in_area(opp_x, opp_y, 250))
+		return 0;
+#endif
 
 	/* sensor are temporarily disabled */
 	if (sensor_obstacle_is_disabled())
@@ -344,14 +373,14 @@ uint8_t strat_obstacle(void)
 	/* opponent is in front of us */
 	if (mainboard.speed_d > 0 && (opp_a > 325 || opp_a < 35)) {
 		DEBUG(E_USER_STRAT, "opponent front d=%d, a=%d "
-		      "xrel=%d yrel=%d (speed_d=%d)", 
+		      "xrel=%d yrel=%d (speed_d=%d)",
 		      opp_d, opp_a, x_rel, y_rel, mainboard.speed_d);
 		sensor_obstacle_disable();
 		return 1;
 	}
 	/* opponent is behind us */
 	if (mainboard.speed_d < 0 && (opp_a < 215 && opp_a > 145)) {
-		DEBUG(E_USER_STRAT, "opponent behind d=%d, a=%d xrel=%d yrel=%d", 
+		DEBUG(E_USER_STRAT, "opponent behind d=%d, a=%d xrel=%d yrel=%d",
 		      opp_d, opp_a, x_rel, y_rel);
 		sensor_obstacle_disable();
 		return 1;
@@ -371,7 +400,7 @@ void interrupt_traj_reset(void)
 }
 
 uint8_t test_traj_end(uint8_t why)
-{ 
+{
 	uint16_t cur_timer;
 	point_t robot_pt;
 
@@ -389,26 +418,26 @@ uint8_t test_traj_end(uint8_t why)
 	}
 
 	if ((why & END_INTR) && traj_intr) {
-		interrupt_traj_reset();		
+		interrupt_traj_reset();
 		return END_INTR;
 	}
 
 	if ((why & END_TRAJ) && trajectory_finished(&mainboard.traj))
 		return END_TRAJ;
-	
+
 	/* we are near the destination point (depends on current
 	 * speed) AND the robot is in the area bounding box. */
 	if (why & END_NEAR) {
-		int16_t d_near = 100;	
-		
+		int16_t d_near = 100;
+
 		if (mainboard.speed_d > 2000)
 			d_near = 150;
-		
+
 		if (trajectory_in_window(&mainboard.traj, d_near, RAD(5.0)) &&
 		    is_in_boundingbox(&robot_pt))
 			return END_NEAR;
 	}
-	
+
 	if ((why & END_BLOCKING) && bd_get(&mainboard.angle.bd)) {
 		strat_hardstop();
 		return END_BLOCKING;
@@ -443,7 +472,7 @@ uint8_t __wait_traj_end_debug(uint8_t why, uint16_t line)
 			      line, opp_x, opp_y, opp_d, opp_a);
 	}
 	else {
-		DEBUG(E_USER_STRAT, "Got %s at line %d", 
+		DEBUG(E_USER_STRAT, "Got %s at line %d",
 		      get_err(ret), line);
 	}
 	return ret;