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)); }
/* * 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; }
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; }