/** Main initialisation. */
static void
main_init (void)
{
#ifndef HOST
/* Disable watchdog (enabled in bootloader). */
MCUSR &= ~(1 << WDRF);
wdt_disable ();
#endif
/* Serial port */
uart0_init ();
/* Enable interrupts */
sei ();
/* Main timer */
timer_init ();
timer_wait ();
/* TWI communications */
asserv_init ();
mimot_init ();
twi_master_init ();
/* IO modules. */
pwm_init ();
contact_init ();
codebar_init ();
usdist_init ();
/* AI modules. */
clamp_init ();
logistic_init ();
path_init ();
/* Initialization done. */
proto_send0 ('z');
}
//////////////////////////////////////
// Main
/////////////////////////////////////
int main(void)
{
//int a = 0;
/////////////////////////////////////
///TODO//////////////////////////////
/////////////////////////////////////
//
//participer a la coupe
/////////////////////////////////////
/////////////////////////////////////
//Initialisations////////////////////
/////////////////////////////////////
uart_init();
time_init(128);
printf("fdevopen \n");
fdevopen(uart0_send,uart0_recv, 1);
// Affichage uart: tip -s 9600 -l /dev/ttyS0
// prog usb :
// avarice -j /dev/ttyUSB0 --erase --program -f ./main.hex
adc_init();
DDRE |= 0x00;
PORTE |= 0x10;//active le pull up
//initialisation du fpga
sbi(XMCRA,SRW11);
sbi(XMCRA,SRW00);
sbi(MCUCR,SRE);
wait_ms(1000);
//on reset le fpga
U_RESET = 0x42;
sbi(PORTB,0);
wait_ms(100);
//on relache le reset
cbi(PORTB,0);
sbi(PORTE,3);
wait_ms(100);
//printf("stack begin :%d \n", &a);
//init des interruptions
printf("init i2c \n");
scheduler_init();
position_init(&pos);
asserv_init(&asserv,&pos);
trajectory_init(&traj,&pos,&asserv);
i2cm_init();
//autorise les interruptions
sei();
//infinite loop!!!
//get_enc_value();
//init communication
#ifdef ENABLE_MECA
printf("init com \n");
initCom();
printf("end init com \n");
#endif
//init A*
initObstacle();
//init méca
#ifdef ENABLE_MECA
mecaCom(TIROIR_FERMER);
mecaCom(PEIGNE_FERMER);
#endif
team = getTeam();
while(MAXIME);//boucle anti_maxime
// while(1)
// {
// printf("gp2r = %d ,gp2l = %d ,gp2m = %d \n",adc_get_value(ADC_REF_AVCC | MUX_ADC0),adc_get_value(ADC_REF_AVCC | MUX_ADC1),adc_get_value(ADC_REF_AVCC | MUX_ADC2));
// }
//avoidance_init();
//antipatinage_init();
// homologation();
// while(1);
// test_evitement();
// while(1);
if(team == RED)
{
disableSpinning();
findPosition(team);
enableSpinning();
trajectory_goto_d(&traj, END, 20);
trajectory_goto_a(&traj, END, 55);
while(!trajectory_is_ended(&traj));
//
// trajectory_goto_a(&traj, END, 90);
//
// trajectory_goto_a(&traj, END, 0);
// while(!trajectory_is_ended(&traj));
asserv_set_vitesse_normal(&asserv);
initTaskManager(&tkm);
//addTask(&tkm, &returnFire, LOW_PRIORITY, R1);
addTask(&tkm, &throwSpears, LOW_PRIORITY, RED);
addTask(&tkm, &putPaint, HIGH_PRIORITY, team);
// addTask(&tkm, &returnFire, LOW_PRIORITY, R3);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, R2);
addTask(&tkm, &takeFruitRed, HIGH_PRIORITY, team);
// addTask(&tkm, &takeFruitYellow, HIGH_PRIORITY, team);
// addTask(&tkm, &findPosition, HIGH_PRIORITY, REDPAINT);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, Y1);
//addTask(&tkm, &throwSpears, HIGH_PRIORITY, YELLOW);
addTask(&tkm, &putFruit, HIGH_PRIORITY, team);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, Y3);
// addTask(&tkm, &takeFruitYellow, HIGH_PRIORITY, team);
// addTask(&tkm, &putFruit, HIGH_PRIORITY, team);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, Y2);
while (TIRETTE);
printf("begin match \n");
avoidance_init();
while(doNextTask(&tkm));
printf("finish \n");
}
else // Team jaune
{
disableSpinning();
findPosition(team);
enableSpinning();
trajectory_goto_d(&traj, END, 20);
trajectory_goto_a(&traj, END, -55);
while(!trajectory_is_ended(&traj));
// addTask(&tkm, &returnFire, LOW_PRIORITY, R1);
addTask(&tkm, &returnFire, LOW_PRIORITY, Y2);
addTask(&tkm, &throwSpears, LOW_PRIORITY, YELLOW);
addTask(&tkm, &putPaint, HIGH_PRIORITY, team);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, Y3);
// addTask(&tkm, &takeFruitRed, HIGH_PRIORITY, team);
addTask(&tkm, &takeFruitYellow, HIGH_PRIORITY, team);
// addTask(&tkm, &returnFire, LOW_PRIORITY, R1);
addTask(&tkm, &putFruit, HIGH_PRIORITY, team);
// addTask(&tkm, &findPosition, HIGH_PRIORITY, YELLOWPAINT);
//addTask(&tkm, &returnFire, HIGH_PRIORITY, R1);
//addTask(&tkm, &throwSpears, HIGH_PRIORITY, RED);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, R3);
// addTask(&tkm, &takeFruitRed, HIGH_PRIORITY, team);
// addTask(&tkm, &putFruit, HIGH_PRIORITY, team);
// addTask(&tkm, &returnFire, HIGH_PRIORITY, R2);
while (TIRETTE);
printf("begin match \n");
avoidance_init();
while(doNextTask(&tkm));
printf("finish \n");
// // attend que la tirette soit retirée
// printf("begin match \n");
// printf("start pos: x %lf pos y %lf \n",position_get_x_cm(&pos),position_get_y_cm(&pos));
}
printf("time: %ld \n",time_get_s());
while(1);//attention boucle infinie
}
//////////////////////////////////////
// Main
/////////////////////////////////////
int main(void)
{
/////////////////////////////////////
//Initialisations////////////////////
/////////////////////////////////////
uart_init();
time_init(128);
printf("fdevopen \n");
fdevopen(uart0_send,uart0_recv, 1);
printf("test uart \n");
adc_init();
DDRE |= 0x00;
PORTE |= 0x10; //active le pull up
//initialisation du fpga
sbi(XMCRA,SRW11);
sbi(XMCRA,SRW00);
sbi(MCUCR,SRE);
wait_ms(1000);
//on reset le fpga
//U_RESET = 0x42;
RESET_FPGA = 1 ;
DDRB |= (1<<0);
sbi(PORTB,0);
wait_ms(100);
//on relache le reset
cbi(PORTB,0);
sbi(PORTE,3);
wait_ms(100);
RESET = (1<<1) | (1<<4) |(1<<7);
wait_ms(10);
RESET = 0 ;
RELATION = 15750;
MOT_R=0;
MOT_L=0;
//init des interruptions
printf("init i2c \n");
scheduler_init();
position_init(&pos);
asserv_init(&asserv,&pos);
trajectory_init(&traj,&pos,&asserv);
//autorise les interruptions
sei();
// while(1)
// {
// // printf("CLICKERS = %d \t %d \t %d \t \n",CLICKER_LEFT,CLICKER_MIDDLE,CLICKER_RIGHT);
// printf("GP2 = %hu \t%hu \t%hu \t%hu\n", read_adc0(),read_adc1(),read_adc2(),read_adc3());
// }
asserv_set_vitesse_fast(&asserv);
find_position_from(GREENSTART);
// SERVO1 = 1000;
// SERVO2 = 1000;
// SERVO3 = 1000;
// SERVO4 = 1000;
// SERVO5 = 1000;
// SERVO6 = 1000;
// SERVO7 = 1000;
// SERVO8 = 1000;
// SERVO9 = 1000;
// SERVO10 = 1000;
// SERVO11 = 1000;
// SERVO12 = 1000;
// SERVO13 = 1000;
// SERVO14 = 1000;
// SERVO15 = 1000;
// SERVO16 = 1000;
// while(1){
// // printf("PORTE=%d, PINE=%d \n", PORTE, PINE);
// //printf("%hu\t%hu\t%hu\t%hu\n", read_adc0(),read_adc1(),read_adc2(),read_adc3());
// printf("%d=PINE & mask=%d %d %d %d \n", PINE, PINE &(1<<PE4),PINE &(1<<PE5), PINE &(1<<PE6), PINE &(1<<PE7));
// // printf("%d=PORTE & mask=%d %d %d %d \n \n \n", PORTE, PORTE &(1<<PE1),PORTE &(1<<PE2), PORTE &(1<<PE3), PORTE &(1<<PE4) );
// // printf("PINE >> pin=%d %d %d %d \n\n\n", PORTE >>PE1, PORTE >>PE2, PORTE >>PE3, PORTE >>PE4 );
// //printf("PINE >> PEx=%d %d %d %d \n", PINE & 0x10, PINE & 0x20, PINE & 0x30, PINE & 0x40);
// wait_ms(1000);
// }
while(1);
}
