Ejemplo n.º 1
0
/*
 * get last 2 columns
 * must be called after the first temple building
 */
uint8_t strat_static_columns_pass2(void)
{
	uint16_t old_spdd, old_spda;
	uint8_t side, err, next_mode;

	DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);

	strat_get_speed(&old_spdd, &old_spda);

	if (get_color() == I2C_COLOR_RED)
		side = I2C_RIGHT_SIDE;
	else
		side = I2C_LEFT_SIDE;

	if (strat_infos.conf.flags & STRAT_CONF_STORE_STATIC2)
		next_mode = I2C_MECHBOARD_MODE_STORE;
	else
		next_mode = I2C_MECHBOARD_MODE_HARVEST;

	switch (strat_infos.s_cols.configuration) {

	/* configuration 1: 4 cols on line 0 */
	case 1:
		if (strat_infos.s_cols.flags & STATIC_COL_LINE1_DONE) {
			/* go on line 2 */

			strat_set_speed(2000, 700);
			trajectory_d_a_rel(&mainboard.traj, -450, COLOR_A(35));
			err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
			if (!TRAJ_SUCCESS(err))
				ERROUT(err);
			
			i2c_mechboard_mode_prepare_pickup_next(side, 
							       next_mode);

			strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
			trajectory_goto_forward_xy_abs(&mainboard.traj,
						       LINE2_X, 
						       COLOR_Y(400));
			err = WAIT_COND_OR_TRAJ_END(get_column_count() == 2,
						    TRAJ_FLAGS_NO_NEAR);
			if (!TRAJ_SUCCESS(err))
				ERROUT(err);
		}
		else {
			/* go on line 1 */
			strat_set_speed(2000, 700);
			trajectory_d_a_rel(&mainboard.traj, -650, COLOR_A(55));
			err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
			if (!TRAJ_SUCCESS(err))
				ERROUT(err);
			
			i2c_mechboard_mode_prepare_pickup_next(side, 
							       next_mode);

			strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);

			err = goto_and_avoid_forward(LINE1_X, 
						     COLOR_Y(400),
						     TRAJ_FLAGS_NO_NEAR,
						     TRAJ_FLAGS_NO_NEAR);
			if (!TRAJ_SUCCESS(err))
				ERROUT(err);
		}

		ERROUT(END_TRAJ);
		break;

	/* configuration 2: 2 cols on line 0,
	   all other colums are on line 1 */
	case 2:
		/* go on line 1 */
		strat_set_speed(2000, 700);
		trajectory_d_a_rel(&mainboard.traj, -410, COLOR_A(-20));
		err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
		if (!TRAJ_SUCCESS(err))
			ERROUT(err);
			
		i2c_mechboard_mode_prepare_pickup_next(side, 
						       next_mode);

		strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);

		err = goto_and_avoid_forward(COL10_X, COLOR_Y(400),
					     TRAJ_FLAGS_NO_NEAR,
					     TRAJ_FLAGS_NO_NEAR);
		if (!TRAJ_SUCCESS(err))
			ERROUT(err);
		
		ERROUT(END_TRAJ);
		break;

	/* configuration 3: 2 cols on line 0,
	   all other colums are on line 2 */
	case 3:
		/* go on line 2 */
		strat_set_speed(2000, 700);
		trajectory_d_a_rel(&mainboard.traj, -150, COLOR_A(-30));
		err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
		if (!TRAJ_SUCCESS(err))
			ERROUT(err);
			
		i2c_mechboard_mode_prepare_pickup_next(side, 
						       next_mode);

		strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);

		trajectory_goto_forward_xy_abs(&mainboard.traj,
					       LINE2_X, 
					       COLOR_Y(400));
		err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
		if (!TRAJ_SUCCESS(err))
			ERROUT(err);

		ERROUT(END_TRAJ);
		break;

	/* configuration 4: 2 cols on line 0,
	   2 on line 1, 2 on line 2 */
	case 4:
		/* go on line 1 */
		strat_set_speed(600, 2000);
		trajectory_d_a_rel(&mainboard.traj, -BIG_DIST,
				   COLOR_A(-135));
		err = WAIT_COND_OR_TRAJ_END(y_is_more_than(900),
					    TRAJ_FLAGS_STD);
		if (TRAJ_SUCCESS(err)) /* we should not reach end */
			ERROUT(END_ERROR);
		else if (err)
			ERROUT(err);

		DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
		i2c_mechboard_mode_prepare_pickup_next(side, 
						       next_mode);

		strat_set_speed(2000, 2000);
		trajectory_d_rel(&mainboard.traj, -BIG_DIST);
		err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1100),
					    TRAJ_FLAGS_STD);
		if (TRAJ_SUCCESS(err)) /* we should not reach end */
			ERROUT(END_ERROR);
		else if (err)
			ERROUT(err);
		
		DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
		trajectory_d_a_rel(&mainboard.traj, -600, COLOR_A(40));
		err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
		if (!TRAJ_SUCCESS(err))
			ERROUT(err);
			
		DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
		strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
		err = goto_and_avoid_forward(LINE1_X, 
					     COLOR_Y(400),
					     TRAJ_FLAGS_NO_NEAR,
					     TRAJ_FLAGS_NO_NEAR);
		ERROUT(END_TRAJ);
		break;

	default:
		break;
	}
	
	/* should not reach this point */
	ERROUT(END_ERROR);

 end:
	strat_set_speed(old_spdd, old_spda);
	return err;
}
Ejemplo n.º 2
0
/*
 * must be called from start area.
 * get 4 static columns and build a temple on the disc
 */
uint8_t strat_static_columns(uint8_t configuration)
{
	uint8_t err;
	uint8_t col1_present = 0, col4_present = 0;
	uint16_t old_spdd, old_spda;

	DEBUG(E_USER_STRAT, "%s(%d)", __FUNCTION__, configuration);

	strat_get_speed(&old_spdd, &old_spda);

	/* calibrate scanner */
	i2c_sensorboard_scanner_calib();

	i2c_mechboard_mode_harvest();

	/* go straight. total distance is less than 5 meters */
	strat_set_speed(1000, 1000);
	trajectory_d_rel(&mainboard.traj, BIG_DIST);

	/* when y > 50, break */
	err = WAIT_COND_OR_TRAJ_END(y_is_more_than(500), TRAJ_FLAGS_STD);
	if (TRAJ_SUCCESS(err)) /* we should not reach end */
		ERROUT(END_ERROR);
	else if (err)
		ERROUT(err);

	/* turn to 90° abs while going forward */
	DEBUG(E_USER_STRAT, "turn now");
	strat_set_speed(1000, 350);
	trajectory_only_a_abs(&mainboard.traj, COLOR_A(90));

	/* when y > 100, check the presence of column 4 */
	err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1000), TRAJ_FLAGS_STD);
	if (TRAJ_SUCCESS(err)) /* we should not reach end */
		ERROUT(END_ERROR);
	else if (err)
		ERROUT(err);
	if (get_color() == I2C_COLOR_RED && sensor_get(S_COLUMN_RIGHT))
		col4_present = 1;
	if (get_color() == I2C_COLOR_GREEN && sensor_get(S_COLUMN_LEFT))
		col4_present = 1;

	/* when y > 120, check the presence of column 1 */
	err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1200), TRAJ_FLAGS_STD);
	if (TRAJ_SUCCESS(err)) /* we should not reach end */
		ERROUT(END_ERROR);
	else if (err)
		ERROUT(err);
	if (get_color() == I2C_COLOR_RED && sensor_get(S_COLUMN_RIGHT))
		col1_present = 1;
	if (get_color() == I2C_COLOR_GREEN && sensor_get(S_COLUMN_LEFT))
		col1_present = 1;

	/* when y > 130, break */
	err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1300), TRAJ_FLAGS_STD);
	if (TRAJ_SUCCESS(err)) /* we should not reach end */
		ERROUT(END_ERROR);
	else if (err)
		ERROUT(err);

	strat_infos.s_cols.flags |= STATIC_COL_LINE0_DONE;

	DEBUG(E_USER_STRAT, "col4=%d col1=%d", col4_present, col1_present);
	DEBUG(E_USER_STRAT, "have %d cols", get_column_count());

	if (configuration == 0) {
		if (get_column_count() > 2) {
			configuration = 1;
			if (col4_present || col1_present) {
				strat_infos.s_cols.flags |= 
					STATIC_COL_LINE2_DONE;
			}
			else {
				strat_infos.s_cols.flags |= 
					STATIC_COL_LINE1_DONE;
			}
		}

		/* only 2 colums on the first line */
		else {
			/* all other colums are on line 1 */
			if (col4_present && col1_present) {
				configuration = 2;
				strat_infos.s_cols.flags |= 
					STATIC_COL_LINE2_DONE;
			}

			/* only 2 columns on line 1, so there are also
			 * 2 on line 2 */
			else if (col4_present || col1_present) {
				configuration = 4;
				strat_infos.s_cols.flags |= 
					STATIC_COL_LINE2_DONE;
			}

			/* all other columns are on line 2 */
			else {
				configuration = 3;
				strat_infos.s_cols.flags |= 
					STATIC_COL_LINE1_DONE;
			}
		}
	}

	strat_infos.s_cols.configuration = configuration;
	DEBUG(E_USER_STRAT, "use configuration %d", configuration);

	if (configuration == 1) {
		/* we already got 4 columns, go to the disc directly */

		strat_set_speed(1500, 900);
		trajectory_only_a_abs(&mainboard.traj, COLOR_A(0));
		err = WAIT_COND_OR_TRAJ_END(x_is_more_than(1100), TRAJ_FLAGS_STD);

		if (TRAJ_SUCCESS(err)) /* we should not reach end */
			ERROUT(END_ERROR);
		else if (err)
			ERROUT(err);
	}
	else if (configuration == 2 /* go from line 0 to line 1 */) {
		strat_set_speed(800, 1000);
		/* relative is needed here */
		trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180));
		err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1300), TRAJ_FLAGS_STD);
		if (TRAJ_SUCCESS(err)) /* we should not reach end */
			ERROUT(END_ERROR);
		else if (err)
			ERROUT(err);
		strat_set_speed(1000, 600);
		err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1100),
					    TRAJ_FLAGS_STD);
		if (TRAJ_SUCCESS(err)) /* we should not reach end */
			ERROUT(END_ERROR);
		else if (err)
			ERROUT(err);
	}
	else if (configuration == 3 /* go from line 0 to line 2 and there is 4 columns
		    on line 2*/) {
		strat_set_speed(1000, 600);
		/* relative is needed here */
		trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180));
		err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1110), TRAJ_FLAGS_STD);
		if (TRAJ_SUCCESS(err)) /* we should not reach end */
			ERROUT(END_ERROR);
		else if (err)
			ERROUT(err);
	} 	
	else if (configuration == 4 /* go from line 0 to line 2 and there is 2 columns
		      on line 2 */) {
		strat_set_speed(1000, 600);
		/* relative is needed here */
		trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180));
		err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(600), TRAJ_FLAGS_STD);
		if (TRAJ_SUCCESS(err)) /* we should not reach end */
			ERROUT(END_ERROR);
		else if (err)
			ERROUT(err);
	}
	else {
		trajectory_stop(&mainboard.traj);
	}

	ERROUT(END_TRAJ);

 end:
	strat_set_speed(old_spdd, old_spda);
	return err;
}
Ejemplo n.º 3
0
int strat_goto_avoid(int x, int y, int flags)
{
    int i, len, ret;
    int retry_count;
    int opp_x, opp_y;
    poly_t *pol_opp;
    point_t *p;
    oa_init();

    /* The robot will try 3 times before giving up. */
    for(retry_count=0;retry_count<3;retry_count++) {

        /* Creates one polygon for each opponent robot. */
        for(i=0;i<robot.beacon.nb_beacon;i++) {
            pol_opp = oa_new_poly(4);

            strat_da_rel_to_xy_abs(robot.beacon.beacon[i].direction, robot.beacon.beacon[i].distance*10,
                   &opp_x, &opp_y);

            NOTICE(0, "Op is at %d;%d", opp_x, opp_y);
            create_opp_polygon(pol_opp, 800, 0);

            /* Checks if the arrival point is in an opponent. */
            if(is_point_in_poly(pol_opp, x, y)) {
                WARNING(0, "Destination point is in opponent.");
                return END_ERROR;
            }
        }

        /* Sets starting and ending point of the path. */
        oa_start_end_points(position_get_x_s16(&robot.pos), position_get_x_s16(&robot.pos), x, y);

        /* Computes the path */
        len = oa_process();

        /* Checks if a path was found. */
        if(len == 0) {
            WARNING(0, "Cannot find a suitable path.");
            return END_ERROR;
        }

        p = oa_get_path();
        /* For all the points in the path. */
        for(i=0;i<len;i++) {
            /* Goes to the point. */
            trajectory_goto_forward_xy_abs(&robot.traj, p->x, p->y);

            /* Waits for the completion of the trajectory. */
            ret = wait_traj_end(flags);

            /* If we were blocked or met an obstacle, we will retry. */
            if(ret == END_BLOCKING || ret == END_OBSTACLE) {
                WARNING(0, "Retry");
                break; // we retry once more
            } else if(!TRAJ_SUCCESS(ret)) {
                /* If it was an other error, we simply abort and return it. */
                WARNING(0, "Unknown error code : %d", ret);
                return ret;
            }

            /* Increments pointer to load next point. */
            p++;
        }

        /* If we reached last point, no need to retry. */
        if(ret == END_TRAJ)
            return END_TRAJ;
    }

    /* If we reach here, it means 3 try were not enough. */
    return END_ERROR;
}