예제 #1
0
void strat_autopos(int16_t x, int16_t y, int16_t a, int16_t epaisseurRobot)
{

    robot.is_aligning = 1;

    // Pour se recaler, on met le robot en regulation angulaire, on reduit la vitesse et l'acceleration
    // On diminue la sensibilite on augmente la constante de temps de detection du bloquage

    bd_set_thresholds(&robot.distance_bd,  5000, 2);
    trajectory_set_speed(&robot.traj, 100, 100);
    robot.mode = BOARD_MODE_DISTANCE_ONLY;

    // On recule jusqu'a� qu'on ait touche un mur
    trajectory_d_rel(&robot.traj, (double) -2000);
    wait_traj_end(END_BLOCKING);

    trajectory_hardstop(&robot.traj);
    bd_reset(&robot.distance_bd);
    bd_reset(&robot.angle_bd);
    robot.mode = BOARD_MODE_ANGLE_DISTANCE;

    position_set(&robot.pos, epaisseurRobot, 0, COLOR_A(0.));
    /* On se mets a la bonne position en x. */
    trajectory_d_rel(&robot.traj, 80);
    wait_traj_end(END_TRAJ);

    /* On se tourne face a la paroi en Y. */
    trajectory_only_a_abs(&robot.traj, COLOR_A(90));
    wait_traj_end(END_TRAJ);

    /* On recule jusqu'a avoir touche le bord. */

    trajectory_d_rel(&robot.traj, (double) -2000);
    wait_traj_end(END_BLOCKING);

    bd_reset(&robot.distance_bd);
    bd_reset(&robot.angle_bd);

    /* On reregle la position. */
    robot.pos.pos_d.y = COLOR_Y(epaisseurRobot);
    robot.pos.pos_s16.y = COLOR_Y(epaisseurRobot);


    trajectory_d_rel(&robot.traj, 80);
    wait_traj_end(END_TRAJ);

    trajectory_goto_forward_xy_abs(&robot.traj, x, COLOR_Y(y));
    wait_traj_end(END_TRAJ);

    /* Pour finir on s'occuppe de l'angle. */
    trajectory_a_abs(&robot.traj, COLOR_A((double)a));
    wait_traj_end(END_TRAJ);

    /* On remet le robot dans son etat initial. */
    robot.mode = BOARD_MODE_ANGLE_DISTANCE;
    robot.is_aligning = 0;

    /* On se met en place a la position demandee. */
    trajectory_set_speed(&robot.traj, speed_mm2imp(&robot.traj, 300), speed_rd2imp(&robot.traj, 2.5));
}
예제 #2
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;
}
예제 #3
0
파일: main.c 프로젝트: onitake/aversive
int main(void)
{
#ifndef HOST_VERSION
	/* brake */
	BRAKE_DDR();
	BRAKE_OFF();

	/* CPLD reset on PG3 */
	DDRG |= 1<<3;
	PORTG &= ~(1<<3); /* implicit */

	/* LEDS */
	DDRJ |= 0x0c;
	DDRL = 0xc0;
	LED1_OFF();
	LED2_OFF();
	LED3_OFF();
	LED4_OFF();
#endif

	memset(&gen, 0, sizeof(gen));
	memset(&mainboard, 0, sizeof(mainboard));
	mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
		DO_POS | DO_POWER | DO_BD | DO_ERRBLOCKING;
	ballboard.lcob = I2C_COB_NONE;
	ballboard.rcob = I2C_COB_NONE;

	/* UART */
	uart_init();
	uart_register_rx_event(CMDLINE_UART, emergency);
#ifndef HOST_VERSION
#if CMDLINE_UART == 3
 	fdevopen(uart3_dev_send, uart3_dev_recv);
#elif CMDLINE_UART == 1
 	fdevopen(uart1_dev_send, uart1_dev_recv);
#endif

	/* check eeprom to avoid to run the bad program */
	if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) !=
	    EEPROM_MAGIC_MAINBOARD) {
		int c;
		sei();
		printf_P(PSTR("Bad eeprom value ('f' to force)\r\n"));
		c = uart_recv(CMDLINE_UART);
		if (c == 'f')
			eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_MAINBOARD);
		wait_ms(100);
		bootloader();
	}
#endif /* ! HOST_VERSION */

	/* LOGS */
	error_register_emerg(mylog);
	error_register_error(mylog);
	error_register_warning(mylog);
	error_register_notice(mylog);
	error_register_debug(mylog);

#ifndef HOST_VERSION
	/* SPI + ENCODERS */
	encoders_spi_init(); /* this will also init spi hardware */

	/* I2C */
	i2c_init(I2C_MODE_MASTER, I2C_MAINBOARD_ADDR);
	i2c_protocol_init();
	i2c_register_recv_event(i2c_recvevent);
	i2c_register_send_event(i2c_sendevent);

	/* TIMER */
	timer_init();
	timer0_register_OV_intr(main_timer_interrupt);

	/* PWM */
	PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10,
				 TIMER1_PRESCALER_DIV_1);
	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.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED,
		      &PORTD, 6);
	PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED,
		      &PORTD, 7);


	/* servos */
	PWM_NG_TIMER_16BITS_INIT(3, TIMER_16_MODE_PWM_10,
				 TIMER1_PRESCALER_DIV_256);
	PWM_NG_INIT16(&gen.servo1, 3, C, 10, PWM_NG_MODE_NORMAL,
		      NULL, 0);
	PWM_NG_TIMER_16BITS_INIT(5, TIMER_16_MODE_PWM_10,
				 TIMER1_PRESCALER_DIV_256);
	PWM_NG_INIT16(&gen.servo2, 5, A, 10, PWM_NG_MODE_NORMAL,
		      NULL, 0);
	PWM_NG_INIT16(&gen.servo3, 5, B, 10, PWM_NG_MODE_NORMAL,
		      NULL, 0);
	PWM_NG_INIT16(&gen.servo4, 5, C, 10, PWM_NG_MODE_NORMAL,
		      NULL, 0);
	support_balls_deploy(); /* init pwm for servos */
#endif /* !HOST_VERSION */

	/* SCHEDULER */
	scheduler_init();
#ifdef HOST_VERSION
	hostsim_init();
	robotsim_init();
#endif

#ifndef HOST_VERSION
	scheduler_add_periodical_event_priority(do_led_blink, NULL,
						100000L / SCHEDULER_UNIT,
						LED_PRIO);
#endif /* !HOST_VERSION */

	/* all cs management */
	microb_cs_init();

	/* TIME */
	time_init(TIME_PRIO);

	/* sensors, will also init hardware adc */
	sensor_init();

#ifndef HOST_VERSION
	/* start i2c slave polling */
	scheduler_add_periodical_event_priority(i2c_poll_slaves, NULL,
						8000L / SCHEDULER_UNIT, I2C_POLL_PRIO);
#endif /* !HOST_VERSION */

	/* strat */
 	gen.logs[0] = E_USER_STRAT;
 	gen.log_level = 5;

	/* strat-related event */
	scheduler_add_periodical_event_priority(strat_event, NULL,
						25000L / SCHEDULER_UNIT,
						STRAT_PRIO);

#ifndef HOST_VERSION
	/* eeprom time monitor */
	scheduler_add_periodical_event_priority(do_time_monitor, NULL,
						1000000L / SCHEDULER_UNIT,
						EEPROM_TIME_PRIO);
#endif /* !HOST_VERSION */

	sei();

	strat_db_init();

	printf_P(PSTR("\r\n"));
	printf_P(PSTR("Respect et robustesse.\r\n"));
#ifndef HOST_VERSION
	{
		uint16_t seconds;
		seconds = eeprom_read_word(EEPROM_TIME_ADDRESS);
		printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60);
	}
#endif

#ifdef HOST_VERSION
	strat_reset_pos(400, COLOR_Y(400), COLOR_A(-90));
#endif

	cmdline_interact();

	return 0;
}