void homologation(void)
{
disableSpinning();
findPosition(team);
trajectory_goto_d(&traj, END, 18);
if(team == RED)
{
trajectory_goto_a(&traj, END, 45);
}
else
{
trajectory_goto_a(&traj, END, -45);
}
while(!trajectory_is_ended(&traj));
enableSpinning();
asserv_set_vitesse_normal(&asserv);
while(TIRETTE);
avoidance_init();
if(team == RED)
{
returnFire(Y1);
}
else
{
returnFire(R1);
}
}
uint8_t putPaint(uint8_t side)
{
printf("putPaint \n");
double eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
asserv_set_vitesse_normal(&asserv);
if(side == RED)
{
set_goalCoor(G_LENGTH * 3 + 7);
}
else
{
set_goalCoor(G_LENGTH * 3 + 8);
}
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
if(actionIsFailed())return FAILED;
printf("angle %lf \n",position_get_angle_deg(&pos));
trajectory_goto_a(&traj, END, 180);
trajectory_goto_d(&traj, END, 10);
while(!trajectory_is_ended(&traj));
trajectory_goto_a(&traj, END, 0);
asserv_set_vitesse_normal(&asserv);
if(actionIsFailed())return FAILED;
disableAvoidance();
printf("angle %lf \n",position_get_angle_deg(&pos));
trajectory_goto_d(&traj, END, -50);
while(!trajectory_is_ended(&traj));
trajectory_goto_d(&traj, END, 10);
while(!trajectory_is_ended(&traj));
// if(team == RED)
// {
// findPosition(REDPAINT);
// }
// else
// {
// findPosition(YELLOWPAINT);
// }
enableAvoidance();
return DONE;
}
void trajectory_goto_reculon_xy(trajectory_manager_t *t, trajectory_order_when_t chen, double x, double y)//Nouvelle fonction pour se déplacer plus simplement à reculon
{
//y = y * get_couleur_depart();
double x_current = position_get_x_cm(t->pm);
double y_current = position_get_y_cm(t->pm);//*get_couleur_depart();
//printf("%lf\t", x - x_current);
double angle = 180 + 180 * atan2(y - y_current, x - x_current) / M_PI;
//printf("%lf\n", angle);
double dist = -1 * sqrt((x - x_current)*(x - x_current) + (y - y_current)*(y - y_current));
trajectory_goto_a(t, END, angle); // relatif ou pas?
trajectory_goto_d(t, END, dist);
}
uint8_t putFruit(uint8_t side)
{
asserv_set_vitesse_normal(&asserv);
printf("begin take fruit");
double eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
printf("start pos: %d eps %lf \n", position_get_coor_eps(&pos, &eps),eps);
static uint8_t k = 0;
if(team == RED)
{
set_goalCoor(G_LENGTH * 7 + 11);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
double eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
printf("start pos: %d eps %lf \n", position_get_coor_eps(&pos, &eps),eps);
set_goalCoor(G_LENGTH * 3 + 11);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
actionSucceed();
while(!astarMv() && !actionIsFailed());
if(actionIsFailed())return FAILED;
trajectory_goto_a(&traj, END, 0);
while(!trajectory_is_ended(&traj));
if(actionIsFailed())return FAILED;
disableSpinning();
trajectory_goto_d(&traj, END, -40);
enableSpinning();
while(!trajectory_is_ended(&traj));
// {
// if(REPOSITIONING)
// {
// trajectory_reinit(&traj);
// asserv_stop(&asserv);
// }
// }
enableSpinning();
//se recaller?
throwSpears(YELLOW);
trajectory_goto_d(&traj, END, -4);
while(!trajectory_is_ended(&traj));
if(k)
{
trajectory_goto_a(&traj, END, -90);
}
else
{
trajectory_goto_a(&traj, END, -90);
}
disableAvoidance();
//trajectory_goto_d(&traj, END, -40);
while(!trajectory_is_ended(&traj));
enableAvoidance();
//if(actionIsFailed())return FAILED;
mecaCom(TIROIR_DEVERSER);
wait_ms(2000);
mecaCom(TIROIR_FERMER);
//trajectory_goto_d(&traj, END, -60);
//trajectory_goto_a(&traj, END, 0);
//trajectory_goto_d(&traj, END,15);
//while(!trajectory_is_ended(&traj));
}
else
{
set_goalCoor(G_LENGTH * 7 + 3);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
double eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
printf("start pos: %d eps %lf \n", position_get_coor_eps(&pos, &eps),eps);
set_goalCoor(G_LENGTH * 4 + 3);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
actionSucceed();
while(!astarMv() && !actionIsFailed());
if(actionIsFailed())return FAILED;
trajectory_goto_a(&traj, END, 180);
trajectory_goto_d(&traj, END, 10);
trajectory_goto_a(&traj, END, 0);
while(!trajectory_is_ended(&traj));
if(actionIsFailed())return FAILED;
trajectory_goto_d(&traj, END, -40);
disableSpinning();
enableSpinning();// a tester
while(!trajectory_is_ended(&traj));
// {
// if(REPOSITIONING)
// {
// trajectory_reinit(&traj);
// asserv_stop(&asserv);
// }
// }
enableSpinning();
throwSpears(RED);
trajectory_goto_d(&traj, END, -5);
while(!trajectory_is_ended(&traj));
if(k)
{
trajectory_goto_a(&traj, END, -90);
}
else
{
trajectory_goto_a(&traj, END, -90);
}
disableAvoidance();
while(!trajectory_is_ended(&traj));
enableAvoidance();
// if(actionIsFailed())return FAILED;
mecaCom(TIROIR_DEVERSER);
wait_ms(2000);
mecaCom(TIROIR_FERMER);
// if(k)
// {
// trajectory_goto_a(&traj, END, 0);
// trajectory_goto_d(&traj, END, 12);
// }
// else
// {
// trajectory_goto_a(&traj, END, 90);
// k = 1;
// //trajectory_goto_d(&traj, END, 0);
// }
// while(!trajectory_is_ended(&traj));
}
return DONE;
}
uint8_t takeFruitYellow(uint8_t type)
{
double eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
printf("start pos: %d eps %lf \n",position_get_coor_eps(&pos, &eps),eps);
set_goalCoor(G_LENGTH * 8 + 9);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
if(actionIsFailed())return FAILED;
printf("take the fruit");
mecaCom(PEIGNE_OUVERT);
mecaCom(TIROIR_OUVERT);
trajectory_goto_a(&traj, END, 0);
if (team == RED)
{
trajectory_goto_d(&traj, END, 4);
}
else
{
trajectory_goto_d(&traj, END, 10);
}
trajectory_goto_a(&traj, END, 90);
while(!trajectory_is_ended(&traj));
if(actionIsFailed())return FAILED;
asserv_set_vitesse_normal(&asserv);
trajectory_goto_d(&traj, END, 80);
trajectory_goto_a(&traj, END, 135);
if(team == RED)
{
trajectory_goto_d(&traj, END, 11 * 1.414);
}
else
{
trajectory_goto_d(&traj, END, 11 * 1.414);
}
// trajectory_goto_a(&traj, END, 90);
// while(!trajectory_is_ended(&traj));
// trajectory_goto_d(&traj, END, 12);
// while(!trajectory_is_ended(&traj));
trajectory_goto_a(&traj, END, 180);
trajectory_goto_d(&traj, END, 55);
trajectory_goto_a(&traj, END, -90);
if(team == RED)
{
trajectory_goto_d(&traj, END, 11 * 1.414);
}
else
{
trajectory_goto_d(&traj, END, 12 * 1.414);
}
while(!trajectory_is_ended(&traj));
mecaCom(PEIGNE_FERMER);
asserv_set_vitesse_normal(&asserv);
if(actionIsFailed())return FAILED;
return DONE;
}
uint8_t findPosition(uint8_t type)
{
disableSpinning();
if(type == REDSTART)
{
asserv_set_vitesse_low(&asserv);
trajectory_goto_d(&traj, END, -200);
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);//dist centre = 12.2
while(!trajectory_is_ended(&traj));
trajectory_goto_arel(&traj, END, -90.0);
while(!trajectory_is_ended(&traj));
trajectory_goto_d(&traj, END, -200);
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);
while(!trajectory_is_ended(&traj));
//asserv_set_distance()
cli();
printf("pos asserv a : %lu %lu \n",U_ASSERV_ANGLE,U_PM_ANGLE);
position_set_xya_cm_deg(&pos,fxx_from_double(20.0),fxx_from_double(20.0), fxx_from_double(0.0));
wait_ms(100);
printf("pos x : %ld pos y %ld \n",pos.x,pos.y);
printf("pos sum x : %lf pos y %lf \n",pos.sum_x,pos.sum_y);
asserv_stop(&asserv);//attention bug bizare avant
asserv_set_vitesse_fast(&asserv);
}
else if(type == YELLOWSTART)// team == YELLOW
{
asserv_set_vitesse_low(&asserv);
trajectory_goto_d(&traj, END, -200);
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);//dist centre = 12.2
while(!trajectory_is_ended(&traj));
trajectory_goto_arel(&traj, END, 90.0);
while(!trajectory_is_ended(&traj));
trajectory_goto_d(&traj, END, -200);
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);
while(!trajectory_is_ended(&traj));
cli();
printf("pos asserv a : %lu %lu \n",U_ASSERV_ANGLE,U_PM_ANGLE);
position_set_xya_cm_deg(&pos,fxx_from_double(20.0),fxx_from_double(280.0), fxx_from_double(0.0));
asserv_stop(&asserv);
asserv_set_vitesse_fast(&asserv);
}
else if(type == REDPAINT)
{
// asserv_set_vitesse_low(&asserv);
// double eps = 0;
// set_startCoor(position_get_coor_eps(&pos, &eps));
// printf("start pos: %d eps %lf \n",position_get_coor_eps(&pos, &eps),eps);
// set_goalCoor(G_LENGTH * 1 + 7);
// if (eps > EPSILON)
// {
// go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
// }
// while(!astarMv() && !actionIsFailed());
// if(actionIsFailed())return DONE;
asserv_set_vitesse_normal(&asserv);
trajectory_goto_a(&traj, END, 90);
trajectory_goto_d(&traj, END, -40);
disableSpinning();
enableSpinning();// a tester
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
enableSpinning();
//if(actionIsFailed())return DONE;
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);
//while(!trajectory_is_ended(&traj));
// if(actionIsFailed())return DONE;
cli();
printf("pos asserv a : %lu %lu \n",U_ASSERV_ANGLE,U_PM_ANGLE);
position_set_xya_cm_deg(&pos,fxx_from_double(20.0),fxx_from_double(160.0), fxx_from_double(90.0));
asserv_stop(&asserv);
asserv_set_vitesse_normal(&asserv);
}
else if(type == YELLOWPAINT)
{
// double eps = 0;
// set_startCoor(position_get_coor_eps(&pos, &eps));
// printf("start pos: %d eps %lf \n",position_get_coor_eps(&pos, &eps),eps);
// set_goalCoor(G_LENGTH * 1 + 8);
// if (eps > EPSILON)
// {
// go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
// }
// while(!astarMv() && !actionIsFailed());
// if(actionIsFailed())return DONE;
asserv_set_vitesse_normal(&asserv);
trajectory_goto_a(&traj, END, -90);
trajectory_goto_d(&traj, END, -40);
disableSpinning();
enableSpinning();// a tester
while(!trajectory_is_ended(&traj))
{
if(REPOSITIONING)
{
trajectory_reinit(&traj);
asserv_stop(&asserv);
}
}
enableSpinning();
//if(actionIsFailed())return DONE;
trajectory_goto_d(&traj, END, 20 - DIST_CENTRE);
while(!trajectory_is_ended(&traj));
//if(actionIsFailed())return DONE;
cli();
printf("pos asserv a : %lu %lu \n",U_ASSERV_ANGLE,U_PM_ANGLE);
position_set_xya_cm_deg(&pos,fxx_from_double(20.0),fxx_from_double(160.0), fxx_from_double(-90.0));
asserv_stop(&asserv);
asserv_set_vitesse_normal(&asserv);
}
//printf("pos asserv a : %lu %lu \n",U_ASSERV_ANGLE,U_PM_ANGLE);
quadramp_reset(&asserv);
control_reset(&asserv);
pid_reset(&asserv);
diff_reset(&asserv);
printf("pos x : %ld pos y %ld \n",pos.x,pos.y);
printf("pos sum x : %lf pos y %lf \n",pos.sum_x,pos.sum_y);
sei();
enableSpinning();
return DONE;
}
//////////////////////////////////////
// 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
}
uint8_t throwSpears(uint8_t side)
{
if(side == RED)
{
if(team == RED)
{
double eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
set_goalCoor(G_LENGTH * 3 + 3);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv());
trajectory_goto_a(&traj, END, 90);
trajectory_goto_d(&traj, END, 11);
while(!trajectory_is_ended(&traj));
disableAvoidance();//test
trajectory_goto_a(&traj, END, -180);
trajectory_goto_d(&traj, END, 14.8);
while(!trajectory_is_ended(&traj));
mecaCom(LANCE_BALLE_AV);
trajectory_goto_d(&traj, END, -14.8);
while(!trajectory_is_ended(&traj));
enableAvoidance();//test
}
else
{
// double eps = 0;
// set_startCoor(position_get_coor_eps(&pos,&eps));
// set_goalCoor(G_LENGTH * 3 + 4);
// if (eps > EPSILON)
// {
// go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
// }
// while(!astarMv() && !actionIsFailed());
// trajectory_goto_a(&traj,END,0);
// trajectory_goto_d(&traj, END, -16.8);
// while(!trajectory_is_ended(&traj));
mecaCom(LANCE_BALLE_AR);
// trajectory_goto_d(&traj, END, 16.8);
// while(!trajectory_is_ended(&traj));
}
}
else
{
if(team == RED)
{
// double eps = 0;
// set_startCoor(position_get_coor_eps(&pos,&eps));
// set_goalCoor(G_LENGTH * 3 + 11);
// if (eps > EPSILON)
// {
// go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
// }
// while(!astarMv());
// trajectory_goto_a(&traj, END, 0);
// trajectory_goto_d(&traj, END, -16.8);
// while(!trajectory_is_ended(&traj));
mecaCom(LANCE_BALLE_AR);
// trajectory_goto_d(&traj, END, 16.8);
// while(!trajectory_is_ended(&traj));
}
else
{
double eps = 0;
set_startCoor(position_get_coor_eps(&pos,&eps));
set_goalCoor(G_LENGTH * 3 + 12);
if (eps > EPSILON)
{
go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos)));
}
while(!astarMv() && !actionIsFailed());
trajectory_goto_a(&traj,END,-90);
trajectory_goto_d(&traj, END, 11);
while(!trajectory_is_ended(&traj));
disableAvoidance();//test
trajectory_goto_a(&traj,END,180);
trajectory_goto_d(&traj, END, 14.8);
while(!trajectory_is_ended(&traj));
mecaCom(LANCE_BALLE_AV);
trajectory_goto_d(&traj, END, -14.8);
while(!trajectory_is_ended(&traj));
enableAvoidance();//test
}
}
return DONE;
}
