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; }
void beacon_calc(void *dummy) { static uint8_t a=0; static int32_t local_rising, local_falling; static int32_t middle; static float size = 0; int32_t local_angle; int32_t local_dist; int32_t local_count_diff_rising ; int32_t local_count_diff_falling ; int32_t local_beacon_coeff; int32_t result_x=0; int32_t result_y=0; int32_t temp=0; int32_t edge=0; //int32_t total_size=0; uint8_t flags; //uint8_t i; int8_t local_valid; if(a) LED4_ON(); else LED4_OFF(); a = !a; if (falling == -1){ /* 0.5 second timeout */ if (invalid_count < 25) invalid_count++; else { IRQ_LOCK(flags); beacon.opponent_x = I2C_OPPONENT_NOT_THERE; IRQ_UNLOCK(flags); } return; } invalid_count = 0; IRQ_LOCK(flags); local_valid = valid_beacon; local_count_diff_rising = count_diff_rising; local_count_diff_falling = count_diff_falling ; local_rising = rising; local_falling = falling; local_beacon_coeff = beacon_coeff; IRQ_UNLOCK(flags); if (local_valid){ invalid_count = 0; //BEACON_DEBUG("rising= %ld\t",local_rising); //BEACON_DEBUG("falling= %ld\r\n",local_falling); /* recalculate number of pulse by adding the value of the counter, then put value back into motor's round range */ local_rising = ((local_rising + (local_count_diff_rising * local_beacon_coeff) / COEFF_MULT)) %(BEACON_STEP_TOUR); local_falling = ((local_falling + (local_count_diff_falling * local_beacon_coeff) / COEFF_MULT)) %(BEACON_STEP_TOUR); //BEACON_DEBUG("rising1= %ld\t",local_rising); //BEACON_DEBUG("falling1= %ld\r\n",local_falling); //BEACON_DEBUG("count diff rising= %ld\t",local_count_diff_rising); //BEACON_DEBUG("count diff falling= %ld\r\n",local_count_diff_falling); /* if around 360 deg, rising > falling, so invert both and recalculate size and middle */ if(local_falling < local_rising){ temp = local_rising; local_rising = local_falling; local_falling = temp; size = BEACON_STEP_TOUR - local_falling + local_rising; middle = (local_falling + ((int32_t)(size)/2) + BEACON_STEP_TOUR) %(BEACON_STEP_TOUR); edge = local_falling; } /* else rising > falling */ else{ size = local_falling - local_rising; middle = local_rising + (size / 2); edge = local_rising; } //for(i=BEACON_MAX_SAMPLE-1;i>0;i--){ // beacon_sample_size[i] = beacon_sample_size[i-1]; // total_size += beacon_sample_size[i]; //} //beacon_sample_size[0] = size; //total_size += size; //total_size /= BEACON_MAX_SAMPLE; //BEACON_DEBUG("rising2= %ld\t",local_rising); //BEACON_DEBUG("falling2= %ld\r\n",local_falling); /* BEACON_DEBUG("size= %ld %ld\t",size, total_size); */ BEACON_DEBUG("size= %f\r\n",size); //BEACON_DEBUG("middle= %ld\r\n",middle); local_angle = get_angle(middle,0); BEACON_NOTICE("opponent angle= %ld\t",local_angle); local_dist = get_dist(size); BEACON_NOTICE("opponent dist= %ld\r\n",local_dist); beacon_angle_dist_to_x_y(local_angle, local_dist, &result_x, &result_y); IRQ_LOCK(flags); beacon.opponent_x = result_x; beacon.opponent_y = result_y; beacon.opponent_angle = local_angle; beacon.opponent_dist = local_dist; /* for I2C test */ //beacon.opponent_x = OPPONENT_POS_X; //beacon.opponent_y = OPPONENT_POS_Y; IRQ_UNLOCK(flags); BEACON_NOTICE("opponent x= %ld\t",beacon.opponent_x); BEACON_NOTICE("opponent y= %ld\r\n\n",beacon.opponent_y); } else { BEACON_NOTICE("non valid\r\n\n"); } falling = -1; }
void led4_off() { LED4_ENABLE(); LED4_OFF(); }