void print_positions(position_manager_t* pos) { printf("%f\t %f\t %f\t %ld\t %ld\t %d\t \n", fxx_to_double(position_get_x_cm(pos)), fxx_to_double(position_get_y_cm(pos)), fxx_to_double(position_get_angle_deg(pos)), POSX_FPGA, POSY_FPGA, ROT_FPGA); }
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 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; }
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; }
void test_evitement(void) { for(int i = 0;i < 32;i++) { trajectory_goto_arel(&traj, END, 45); while(!trajectory_is_ended(&traj)); } // addTask(&tkm, &returnFire, LOW_PRIORITY, R1); // addTask(&tkm, &throwSpears, LOW_PRIORITY, YELLOW); // addTask(&tkm, &returnFire, LOW_PRIORITY, Y2); // // addTask(&tkm, &returnFire, HIGH_PRIORITY, Y3); // addTask(&tkm, &takeFruitYellow, HIGH_PRIORITY, team); // addTask(&tkm, &putPaint, HIGH_PRIORITY, team); // addTask(&tkm, &findPosition, HIGH_PRIORITY, YELLOWPAINT); // addTask(&tkm, &returnFire, HIGH_PRIORITY, R1); // addTask(&tkm, &throwSpears, HIGH_PRIORITY, RED); // addTask(&tkm, &putFruit, HIGH_PRIORITY, team); // addTask(&tkm, &returnFire, HIGH_PRIORITY, R3); // addTask(&tkm, &takeFruitRed, HIGH_PRIORITY, team); // addTask(&tkm, &putFruit, HIGH_PRIORITY, team); // addTask(&tkm, &returnFire, HIGH_PRIORITY, R2); // asserv_set_vitesse_normal(&asserv); // while (TIRETTE); // printf("begin match \n"); // avoidance_init(); // while(doNextTask(&tkm)); // printf("finish \n"); // throwSpears(RED); // printf("succeed"); // trajectory_goto_d(&traj, END, 10); // while(!trajectory_is_ended(&traj)); // printf("astar test \n"); // set_startCoor(G_LENGTH * 2 + 2); // set_goalCoor(G_LENGTH * 8 + 13); // while(!astarMv() && !actionIsFailed()); // trajectory_goto_d(&traj, END, -100); // while(!trajectory_is_ended(&traj)); while(1); // trajectory_goto_arel(&traj, END, 180); // while(!trajectory_is_ended(&traj)); // trajectory_goto_d(&traj, END, 100); // while(!trajectory_is_ended(&traj)); // double x = fxx_to_double(position_get_y_cm(&pos)); double y = fxx_to_double(position_get_x_cm(&pos)); printf("x : %lf y : %lf\n", x, y); printf("isOut %d obs %d\n", isOutOfGraphe(x, y), isObstacle(x, y)); //go_to_node(x, y,graphe); }
uint8_t returnFire(uint8_t fireNb) { static uint8_t tab[7] = {0}; for(uint8_t i = 1;i < 7;i++) { if(!(tab[i] & DONE)) { avoidFire(i); } } uint16_t stackend = 0; printf("stack %d \n",&stackend - &stack_begin); if(fireNb == R1) { printf("return R1 \n"); double eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 3 + 3); } else { set_goalCoor(G_LENGTH * 3 + 6); asserv_set_vitesse_normal(&asserv); } //put a wall to avoid the wrong way avoidFire(fireNb); 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; //test if it succeed eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 3 + 5); } else { set_goalCoor(G_LENGTH * 3 + 4); } //remove the wall stopAvoid(fireNb); 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; } else if(fireNb == R2) { printf("return R2 \n"); double eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 4 + 2); } else { set_goalCoor(G_LENGTH * 7 + 2); } //put a wall to avoid the wrong way avoidFire(fireNb); 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; //test if it succeed eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 6 + 2); } else { set_goalCoor(G_LENGTH * 5 + 2); } //remove the wall stopAvoid(fireNb); 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; } else if(fireNb == R3) { printf("return R3 \n"); double eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 8 + 6); } else { set_goalCoor(G_LENGTH * 8 + 3); } //put a wall to avoid the wrong way avoidFire(fireNb); 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; //test if it succeed eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 8 + 4); } else { set_goalCoor(G_LENGTH * 8 + 5); } //remove the wall stopAvoid(fireNb); 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; } else if(fireNb == Y1) { printf("return Y1 \n"); double eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 3 + 9); } else { set_goalCoor(G_LENGTH * 3 + 12); } //put a wall to avoid the wrong way avoidFire(fireNb); 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; //test if it succeed eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 3 + 11); } else { set_goalCoor(G_LENGTH * 3 + 10); } //remove the wall stopAvoid(fireNb); 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; } else if(fireNb == Y2) { printf("return Y2 \n"); double eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 7 + 13); } else { set_goalCoor(G_LENGTH * 4 + 13); } //put a wall to avoid the wrong way avoidFire(fireNb); 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; //test if it succeed eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 5 + 13); } else { set_goalCoor(G_LENGTH * 6 + 13); } //remove the wall stopAvoid(fireNb); 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; } else if(fireNb == Y3) { printf("return Y3 \n"); double eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 8 + 12); } else { set_goalCoor(G_LENGTH * 8 + 9); } //put a wall to avoid the wrong way avoidFire(fireNb); 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; //test if it succeed eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 8 + 10); } else { set_goalCoor(G_LENGTH * 8 + 11); } //remove the wall stopAvoid(fireNb); 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; } else { } //if succeed tab[fireNb] += DONE; for(uint8_t i = 1;i < 7;i++) { if(!(tab[i] & DONE)) { stopAvoid(i); } } return DONE; }
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; }