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 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); } }
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 calibrateGP2(void) { //////////////////////////////////////// //Code de Recuperation des valeurs des gp2 avant //////////////////////////////////////// //////////////////////////////////////// //Placer le robot face a un mur //////////////////////////////////////// //trajectory_goto_d(&traj, END, -10); //while(!trajectory_is_ended(&traj)); for (int i = 0; i < 25; ++i) { wait_ms(500); printf("dist = %d cm",i*5); printf("gp2 right %d gp2 left %d gp2 middle %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)); trajectory_goto_d(&traj, END, -5); while(!trajectory_is_ended(&traj)); } return DONE; }
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; }