From: zer0 <zer0@carbon.local>
Date: Tue, 11 May 2010 22:06:54 +0000 (+0200)
Subject: real tests
X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=f1fab48d45873bc23dccbae5db393afd2942570c;p=aversive.git

real tests
---

diff --git a/projects/microb2010/cobboard/main.c b/projects/microb2010/cobboard/main.c
index d01a92e..ad496ca 100755
--- a/projects/microb2010/cobboard/main.c
+++ b/projects/microb2010/cobboard/main.c
@@ -211,10 +211,10 @@ int main(void)
 	PWM_NG_TIMER_16BITS_INIT(4, TIMER_16_MODE_PWM_10,
 				 TIMER4_PRESCALER_DIV_1);
 
-	PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED,
-		      &PORTD, 4);
-	PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED |
-		      PWM_NG_MODE_SIGN_INVERTED, &PORTD, 5);
+	PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED |
+		      PWM_NG_MODE_SIGN_INVERTED, &PORTD, 4);
+	PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED,
+		      &PORTD, 5);
 	PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED,
 		      &PORTD, 6);
 	PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED |
diff --git a/projects/microb2010/cobboard/main.h b/projects/microb2010/cobboard/main.h
index d539a46..80469ea 100755
--- a/projects/microb2010/cobboard/main.h
+++ b/projects/microb2010/cobboard/main.h
@@ -48,16 +48,16 @@
 #define BRAKE_ON()      do { PORTJ |= 0xF0; } while(0)
 #define BRAKE_OFF()     do { PORTJ &= 0x0F; } while(0)
 
-#define LEFT_SPICKLE_ENCODER   ((void *)0)
-#define RIGHT_SPICKLE_ENCODER  ((void *)1)
+#define RIGHT_SPICKLE_ENCODER  ((void *)0)
+#define LEFT_SPICKLE_ENCODER   ((void *)1)
 #define SHOVEL_ENCODER         ((void *)2)
 
 #define SERVO_DOOR_PWM         ((void *)&gen.servo2)
 #define SERVO_CARRY_L_PWM      ((void *)&gen.servo1)
 #define SERVO_CARRY_R_PWM      ((void *)&gen.servo3)
 
-#define LEFT_SPICKLE_PWM       ((void *)&gen.pwm1_4A)
-#define RIGHT_SPICKLE_PWM      ((void *)&gen.pwm2_4B)
+#define RIGHT_SPICKLE_PWM      ((void *)&gen.pwm1_4A)
+#define LEFT_SPICKLE_PWM       ((void *)&gen.pwm2_4B)
 #define SHOVEL_PWM             ((void *)&gen.pwm3_1A)
 
 /** ERROR NUMS */
diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c
index 0bd04b5..e0e6a1b 100644
--- a/projects/microb2010/mainboard/commands_traj.c
+++ b/projects/microb2010/mainboard/commands_traj.c
@@ -831,7 +831,7 @@ static void auto_position(void)
 		goto intr;
 	strat_reset_pos(ROBOT_WIDTH/2 + 100,
 			COLOR_Y(ROBOT_HALF_LENGTH_FRONT),
-			COLOR_A(-90));
+			COLOR_A(-90) + ROBOT_ANGLE_FRONT);
 	strat_hardstop();
 
 	trajectory_d_rel(&mainboard.traj, -180);
@@ -851,7 +851,7 @@ static void auto_position(void)
 		goto intr;
 	strat_reset_pos(ROBOT_HALF_LENGTH_FRONT,
 			DO_NOT_SET_POS,
-			180);
+			180 + ROBOT_ANGLE_FRONT);
 	strat_hardstop();
 
 	trajectory_d_rel(&mainboard.traj, -170);
diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c
index 896fa16..0700469 100644
--- a/projects/microb2010/mainboard/cs.c
+++ b/projects/microb2010/mainboard/cs.c
@@ -223,7 +223,7 @@ void microb_cs_init(void)
 	position_init(&mainboard.pos);
 	position_set_physical_params(&mainboard.pos, VIRTUAL_TRACK_MM, DIST_IMP_MM);
 	position_set_related_robot_system(&mainboard.pos, &mainboard.rs);
-	//position_set_centrifugal_coef(&mainboard.pos, 0.000025);
+	position_set_centrifugal_coef(&mainboard.pos, 0.000016);
 	position_use_ext(&mainboard.pos);
 
 	/* TRAJECTORY MANAGER */
diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h
index 54e03b0..03bc688 100755
--- a/projects/microb2010/mainboard/main.h
+++ b/projects/microb2010/mainboard/main.h
@@ -83,6 +83,12 @@
 #define ROBOT_HALF_LENGTH_REAR 120
 #define ROBOT_WIDTH 320
 
+#ifdef HOST_VERSION
+#define ROBOT_ANGLE_FRONT 0.
+#else
+#define ROBOT_ANGLE_FRONT 0.75 // 0.27
+#endif
+
 /* it is a 1024 imps -> 4096 because we see 1/4 period
  * and diameter: 55mm -> perimeter 134mm
  * dist_imp_mm = 4096/134 x 10 -> 304 */
diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c
index dc59581..64562f5 100644
--- a/projects/microb2010/mainboard/strat.c
+++ b/projects/microb2010/mainboard/strat.c
@@ -112,12 +112,14 @@ void strat_conf_dump(const char *caller)
 		return;
 
 	printf_P(PSTR("-- conf --\r\n"));
-	printf_P(PSTR("our_orange = %s\n"),
+	printf_P(PSTR("our_orange = %s\r\n"),
 		 (strat_conf.flags & STRAT_CONF_OUR_ORANGE) ? "y":"n");
-	printf_P(PSTR("wait_obstacle = %s\n"),
+	printf_P(PSTR("wait_obstacle = %s\r\n"),
 		 (strat_conf.flags & STRAT_CONF_WAIT_OBSTACLE) ? "y":"n");
-	printf_P(PSTR("opp_orange = %d\n"), strat_conf.opp_orange);
-	printf_P(PSTR("orphan_tomato = %d\n"), strat_conf.orphan_tomato);
+	printf_P(PSTR("straight begin = %s\r\n"),
+		 (strat_conf.flags & STRAT_CONF_STRAIGHT_BEGIN) ? "y":"n");
+	printf_P(PSTR("opp_orange = %d\r\n"), strat_conf.opp_orange);
+	printf_P(PSTR("orphan_tomato = %d\r\n"), strat_conf.orphan_tomato);
 }
 
 void strat_event_enable(void)
@@ -144,6 +146,8 @@ void strat_init(void)
 	time_reset();
 	interrupt_traj_reset();
 
+	i2c_cobboard_deploy(I2C_LEFT_SIDE);
+	i2c_cobboard_deploy(I2C_RIGHT_SIDE);
 	i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
 	i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
 
@@ -239,13 +243,12 @@ static void check_corn(void)
 	ballboard.rcob = I2C_COB_NONE;
 	IRQ_UNLOCK(flags);
 
-	/* XXX take opponent position into account */
-
 	/* check present cobs once per second */
 	cur_time = time_get_s();
 	if (cur_time != prev_check_time) {
 		uint8_t i;
 
+		/* only useful for simu */
 		for (i = 0; i < CORN_NB; i++) {
 			if (strat_db.corn_table[i]->present == 1 &&
 			    strat_db.corn_table[i]->time_removed != -1 &&
@@ -297,6 +300,8 @@ static void check_corn(void)
 #ifdef HOST_VERSION
 					cobboard.cob_count ++;
 					printf("add cob %d\n", lidx);
+#else
+					strat_db.corn_table[lidx]->present = 0;
 #endif
 				}
 			}
@@ -332,6 +337,8 @@ static void check_corn(void)
 #ifdef HOST_VERSION
 					cobboard.cob_count ++;
 					printf("add cob %d\n", ridx);
+#else
+					strat_db.corn_table[ridx]->present = 0;
 #endif
 				}
 			}
@@ -511,28 +518,29 @@ static uint8_t strat_eject(void)
 #endif
 	}
 
-	/* half turn */
-	trajectory_a_abs(&mainboard.traj, COLOR_A(-110));
-	err = wait_traj_end(END_INTR|END_TRAJ);
-	if (!TRAJ_SUCCESS(err))
-		goto fail;
+	if (get_cob_count() > 0) {
+		/* half turn */
+		trajectory_a_abs(&mainboard.traj, COLOR_A(-110));
+		err = wait_traj_end(END_INTR|END_TRAJ);
+		if (!TRAJ_SUCCESS(err))
+			goto fail;
 
-	/* cob ejection */
-	trajectory_d_rel(&mainboard.traj, -70);
-	err = wait_traj_end(END_INTR|END_TRAJ);
-	if (!TRAJ_SUCCESS(err))
-		goto fail;
+		/* cob ejection */
+		trajectory_d_rel(&mainboard.traj, -70);
+		err = wait_traj_end(END_INTR|END_TRAJ);
+		if (!TRAJ_SUCCESS(err))
+			goto fail;
 
-	if (get_cob_count() > 0) {
 		i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT);
 		time_wait_ms(2000);
-	}
 
-	/* cob ejection */
-	trajectory_d_rel(&mainboard.traj, 70);
-	err = wait_traj_end(END_INTR|END_TRAJ);
-	if (!TRAJ_SUCCESS(err))
-		goto fail;
+
+		/* cob ejection */
+		trajectory_d_rel(&mainboard.traj, 70);
+		err = wait_traj_end(END_INTR|END_TRAJ);
+		if (!TRAJ_SUCCESS(err))
+			goto fail;
+	}
 
 	strat_db_dump(__FUNCTION__);
 	err = END_TRAJ;
@@ -550,7 +558,8 @@ static uint8_t strat_beginning(uint8_t do_initturn)
 	strat_set_acc(ACC_DIST, ACC_ANGLE);
 
 	if (do_initturn) {
-		strat_set_speed(600, 60); /* OK */
+		//strat_set_speed(600, 60);
+		strat_set_speed(450, 50);
 		trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20));
 		err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
 					    TRAJ_FLAGS_STD);
@@ -594,7 +603,7 @@ static uint8_t strat_beginning2(uint8_t do_initturn)
 	strat_set_acc(ACC_DIST, ACC_ANGLE);
 
 	if (do_initturn) {
-		strat_set_speed(600, 90); /* OK */
+		strat_set_speed(600, 95); /* OK */
 		trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(-40));
 		err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
 					    TRAJ_FLAGS_STD);
@@ -751,7 +760,7 @@ uint8_t get_orphan_tomatoes(void)
 	if (ABS(a) < 10)
 		strat_reset_pos(AREA_X - ROBOT_HALF_LENGTH_FRONT,
 				DO_NOT_SET_POS,
-				COLOR_A(0));
+				COLOR_A(0) + ROBOT_ANGLE_FRONT);
 
 	strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_FAST);
 	trajectory_d_rel(&mainboard.traj, -250);
@@ -810,6 +819,7 @@ uint8_t prepare_hill(uint8_t orange_color, int16_t posx)
 	uint8_t our_color = get_color();
 	uint8_t err;
 
+	strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
 	if (orange_color == I2C_COLOR_YELLOW)
 		starty = HILL_POSY_YELLOW;
 	else
@@ -866,7 +876,6 @@ uint8_t run_to_the_hills(uint8_t orange_color)
 	bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 2000000, 80);
 	bd_set_speed_threshold(&mainboard.distance.bd, 10);
 	support_balls_pack();
-	i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_PREP_FORK);
 
 	/* decrease angle gains */
 	pid_set_gains(&mainboard.angle.pid, 200, 0, 2000);
@@ -875,10 +884,16 @@ uint8_t run_to_the_hills(uint8_t orange_color)
 	 * are on the hill */
 	i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
 	trajectory_d_rel(&mainboard.traj, HILL_LEN);
-	err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) >
-				    HILL_POSX_BALLS_DOWN1,
-				    TRAJ_FLAGS_SMALL_DIST);
+	if (orange_color == our_color)
+		err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) >
+					    HILL_POSX_BALLS_DOWN1,
+					    TRAJ_FLAGS_SMALL_DIST);
+	else
+		err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
+					    AREA_X - HILL_POSX_BALLS_DOWN1,
+					    TRAJ_FLAGS_SMALL_DIST);
 	DEBUG(E_USER_STRAT, "deploy support balls");
+	i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_PREP_FORK);
 	support_balls_deploy();
 	i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP);
 	trajectory_only_a_rel(&mainboard.traj, 2);
@@ -894,26 +909,38 @@ uint8_t run_to_the_hills(uint8_t orange_color)
 	/* reach top, go down */
 	trajectory_d_rel(&mainboard.traj, -HILL_LEN);
 
-	err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
-				    HILL_POSX_BALLS_DOWN2,
-				    TRAJ_FLAGS_SMALL_DIST);
+	if (orange_color == our_color)
+		err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
+					    HILL_POSX_BALLS_DOWN2,
+					    TRAJ_FLAGS_SMALL_DIST);
+	else
+		err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) >
+					    AREA_X - HILL_POSX_BALLS_DOWN2,
+					    TRAJ_FLAGS_SMALL_DIST);
 	DEBUG(E_USER_STRAT, "pack support balls");
 	support_balls_pack();
-	err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
-				    HILL_POSX_BALLS_DOWN3,
-				    TRAJ_FLAGS_SMALL_DIST);
+	if (orange_color == our_color)
+		err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
+					    HILL_POSX_BALLS_DOWN3,
+					    TRAJ_FLAGS_SMALL_DIST);
+	else
+		err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) >
+					    AREA_X - HILL_POSX_BALLS_DOWN3,
+					    TRAJ_FLAGS_SMALL_DIST);
+
 	DEBUG(E_USER_STRAT, "deploy support balls");
 	strat_set_acc(ad, aa);
 	strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 	support_balls_deploy();
-	err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-	i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
 
 	/* wait to be near the wall */
-	err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < 200,
-				    TRAJ_FLAGS_SMALL_DIST);
-
-	i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+	if (orange_color == our_color)
+		err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < 300,
+					    TRAJ_FLAGS_SMALL_DIST);
+	else
+		err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) >
+					    AREA_X - 300,
+					    TRAJ_FLAGS_SMALL_DIST);
 
 	/* restore BD coefs */
 	bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
@@ -921,33 +948,42 @@ uint8_t run_to_the_hills(uint8_t orange_color)
 	bd_set_speed_threshold(&mainboard.distance.bd, 60);
 
 	/* calibrate position on the wall */
-	strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
-	err = strat_calib(-400, TRAJ_FLAGS_SMALL_DIST);
-	if (orange_color == our_color)
-		strat_reset_pos(ROBOT_HALF_LENGTH_REAR,
-			     DO_NOT_SET_POS, COLOR_A(0));
-	else
-		strat_reset_pos(AREA_X - ROBOT_HALF_LENGTH_REAR,
-			     DO_NOT_SET_POS, COLOR_A(180));
+	strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_SLOW);
+
+	i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+
+	trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
+	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+	time_wait_ms(200);
+
+	err = strat_calib(400, TRAJ_FLAGS_SMALL_DIST);
+	strat_reset_pos(DO_NOT_SET_POS,
+			COLOR_Y(ROBOT_HALF_LENGTH_FRONT),
+			COLOR_A(-90) + ROBOT_ANGLE_FRONT);
 	strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 
-	trajectory_d_rel(&mainboard.traj, 250);
+	trajectory_d_rel(&mainboard.traj, -250);
 	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
-	if (orange_color == I2C_COLOR_YELLOW)
-		trajectory_a_rel(&mainboard.traj, 90);
+	if (orange_color == our_color)
+		trajectory_a_abs(&mainboard.traj, 180);
 	else
-		trajectory_a_rel(&mainboard.traj, -90);
+		trajectory_a_abs(&mainboard.traj, 0);
 	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 	time_wait_ms(200);
 
 	strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
-	err = strat_calib(-400, TRAJ_FLAGS_SMALL_DIST);
-	strat_reset_pos(DO_NOT_SET_POS,
-			COLOR_Y(ROBOT_HALF_LENGTH_REAR),
-			COLOR_A(90));
+	err = strat_calib(400, TRAJ_FLAGS_SMALL_DIST);
+	if (orange_color == our_color)
+		strat_reset_pos(ROBOT_HALF_LENGTH_FRONT,
+				DO_NOT_SET_POS,
+				180 + ROBOT_ANGLE_FRONT);
+	else
+		strat_reset_pos(AREA_X - ROBOT_HALF_LENGTH_FRONT,
+				DO_NOT_SET_POS,
+				0 + ROBOT_ANGLE_FRONT);
 	strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 
-	trajectory_d_rel(&mainboard.traj, 250);
+	trajectory_d_rel(&mainboard.traj, -250);
 	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 
 	/* revert acceleration and speed */
diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h
index e8572fc..ea2a6ec 100644
--- a/projects/microb2010/mainboard/strat.h
+++ b/projects/microb2010/mainboard/strat.h
@@ -131,7 +131,7 @@
 #define TRAJ_FLAGS_SMALL_DIST (END_TRAJ|END_BLOCKING|END_INTR)
 
 /* default acc */
-#define ACC_DIST  16.
+#define ACC_DIST  12.
 #define ACC_ANGLE 16.
 
 /* default speeds */
diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c
index 8716486..d68f5a6 100644
--- a/projects/microb2010/mainboard/strat_base.c
+++ b/projects/microb2010/mainboard/strat_base.c
@@ -143,18 +143,18 @@ uint8_t strat_goto_xy_force(int16_t x, int16_t y)
 }
 
 /* reset position */
-void strat_reset_pos(int16_t x, int16_t y, int16_t a)
+void strat_reset_pos(int16_t x, int16_t y, double a)
 {
-	int16_t posx = position_get_x_s16(&mainboard.pos);
-	int16_t posy = position_get_y_s16(&mainboard.pos);
-	int16_t posa = position_get_a_deg_s16(&mainboard.pos);
+	double posx = position_get_x_double(&mainboard.pos);
+	double posy = position_get_y_double(&mainboard.pos);
+	double posa = position_get_a_rad_double(&mainboard.pos);
 
 	if (x == DO_NOT_SET_POS)
 		x = posx;
 	if (y == DO_NOT_SET_POS)
 		y = posy;
 	if (a == DO_NOT_SET_POS)
-		a = posa;
+		a = DEG(posa);
 
 	/* some issues with blocking on simulator */
 #ifdef HOST_VERSION
@@ -217,7 +217,7 @@ uint8_t strat_calib(int16_t dist, uint8_t flags)
 	trajectory_d_rel(&mainboard.traj, dist);
 	pid_set_maximums(&mainboard.distance.pid, max_in, max_i, 4095);
 	pid_set_gains(&mainboard.angle.pid, 1, 0, 0);
-	time_wait_ms(300);
+	time_wait_ms(500);
 	strat_hardstop();
 
 #ifdef HOST_VERSION
diff --git a/projects/microb2010/mainboard/strat_base.h b/projects/microb2010/mainboard/strat_base.h
index bcabf2b..efc9af4 100644
--- a/projects/microb2010/mainboard/strat_base.h
+++ b/projects/microb2010/mainboard/strat_base.h
@@ -45,7 +45,7 @@ void strat_hardstop(void);
 #define DO_NOT_SET_POS -1000
 /* Reset position. If arg == DO_NOT_SET_POS, don't update value for
  * it. */
-void strat_reset_pos(int16_t x, int16_t y, int16_t a);
+void strat_reset_pos(int16_t x, int16_t y, double a);
 
 /* decrease gain on angle PID, and go forward until we reach the
  * border. */