+ strat_set_acc(5, ACC_ANGLE);
+ strat_set_speed(300, SPEED_ANGLE_SLOW);
+ bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 2000000, 80);
+ 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);
+
+ /* here it is difficult to handle return values, because we
+ * 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);
+ DEBUG(E_USER_STRAT, "deploy support balls");
+ support_balls_deploy();
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP);
+ trajectory_only_a_rel(&mainboard.traj, 2);
+ err = WAIT_COND_OR_TE_TO(0, 0, 2200);
+
+ i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
+ strat_set_acc(3, 3);
+ strat_hardstop();
+ i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK);
+
+ time_wait_ms(1800);
+
+ /* 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);
+ 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);
+ 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);
+
+ /* restore BD coefs */
+ bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
+ bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
+ 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_FAST, SPEED_ANGLE_FAST);
+
+ 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);
+ else
+ trajectory_a_rel(&mainboard.traj, -90);
+ 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));
+ strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);