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();
}
