Collin Alexis
/
Code_FRC_2018
Code FRC 2018
Revision 0:aebeb20b201a, committed 2019-03-21
- Comitter:
- AlexisCollin
- Date:
- Thu Mar 21 14:59:13 2019 +0000
- Commit message:
- code de l'an dernier
Changed in this revision
diff -r 000000000000 -r aebeb20b201a mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Mar 21 14:59:13 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file
diff -r 000000000000 -r aebeb20b201a robot_general.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/robot_general.cpp Thu Mar 21 14:59:13 2019 +0000 @@ -0,0 +1,722 @@ +#include "robot_general.h" + +DigitalIn Daniels_in(PA_15); + +//DigitalIn UBp(USER_BUTTON); +InterruptIn UBp_it(USER_BUTTON); +Timeout ubp_to; + +//Serial dbug(USBTX, USBRX, 115200); +Serial dbug(PA_0, PA_1, 38400); + +Timer timer_generale; + +int flagUBp = 0, flagDaniels = 0, match_en_cours = 0; +int flagEv = 0, flagAv = 1, flagDpl = 0, flag_auto_suiv = 0; +int sensrotation=-1; + +short pos_balle[10][4];int nb_balle = 0, balle_retrouve = 0; + + + +unsigned int temps_ecoule = 0; + +void isr_UBp();void user_bp_ar(); + +enum etat_visee_balle_T {init_visee_balle, fin_vir_rapide, debut_vir_lent, fin_vir_lent, avancer_20,debut_vir_lent2, fin_vir_lent2,avancer,rotation, lancer, fin_visee_balle, error_visee_balle, D, ED, Perdu, debut_vir_retour, fin_vir_rapide_odo,M, balle_disparue, recalage, recalage3, recalage2, retour}; +enum etat_visee_balle_T etat_visee_balle = init_visee_balle , etat_visee_balle_prec = Perdu; +int timer_visee_balle=0; +void automate_visee_balle(); + +enum etat_pied_ballon_T {init_pied_ballon_dierct, avance_x_DT_pb, rotation_ballon_x_pb, reculer_ballon_x_pb, recalage_pied_pb,fin_pied_ballon, error_pied_ballon}; +enum etat_pied_ballon_T etat_pied_ballon = init_pied_ballon_dierct , etat_pied_ballon_prec = fin_pied_ballon; +int timer_pied_ballon = 0; +void automate_pied_ballon(); + +enum etat_macro_auto_T {init_macro_auto}; +enum etat_macro_auto_T etat_macro_auto = init_macro_auto , etat_macro_auto_prec = init_macro_auto; +int timer_macro_auto = 0; +void automate_macro_auto(); + +enum etat_tirer_balle_complet_T {init_tirer_balle_complet, pied_ballon_tbc, visee_balle_tbc, fin_tirer_balle_complet, error_tirer_balle_complet}; +enum etat_tirer_balle_complet_T etat_tirer_balle_complet = init_tirer_balle_complet , etat_tirer_balle_complet_prec = init_tirer_balle_complet; +int timer_tirer_balle_complet = 0; +void automate_tirer_balle_complet(); + +enum etat_3balles_odo_T {init_tirer_balle_complet, pied_ballon_tbc, visee_balle_tbc, fin_tirer_balle_complet, error_tirer_balle_complet}; +enum etat_3balles_odo_T etat_3balles_odo = init_tirer_balle_complet , etat_3balles_odo = init_tirer_balle_complet; +int timer_3balles_odo = 0; +void automate_3balles_odo(); + + +void automate_base(); +int etat_auto = 0, etat_auto_prec = 0; +int etat_auto_base = 0, etat_auto_base_prec = 0; + +void if_UBp(); +void wait_debut_de_partie(); + + + +int main() +{ + dbug.printf("\nSETUP "); + can_init(); + UBp_it.rise(&isr_UBp); + //SetOdometrie(26,500,55,0); + bonus_findepartie(0); + + cmd_servo = 0; + ejection_continue = 1; + ejection_active = 1; + temps_ecoule = 0; + match_en_cours = 0; + + //BendRadius(139, 900, -1,0); + //GoStraight(3800,0,0,0); + dbug.printf("END SETUP\n"); + while(!match_en_cours){ + trait_can(); + wait_debut_de_partie();} + + timer_generale.start(); + //Rotate(3600); + //BendRadius(139, -900, -1,0); + //while(1); + //dbug.printf("%d\n", flagDaniels); + + while(1) + { + if(temps_ecoule > DUREE_MATCH_TIC) + { + match_en_cours = 0; + SendRawId(ASSERVISSEMENT_STOP); + ejection_active = 0; + automate_ejecteur(); + wait(6); + bonus_findepartie(1); + while(1); + } + else if(timer_generale.read_us() > TIKER_GENERALE_US) + {timer_generale.reset();temps_ecoule++; + trait_can(); + f_mesure(); + pos_disquette(); + + //if_UBp(); + //automate_base(); + automate_pied_ballon(); + automate_visee_balle(); + + automate_ejecteur(); + if(((temps_ecoule%500) == 0))// execute chaques secondes + { + dbug.printf("Pos x:%d, y:%d, te:%d, Tecoule:%d\n", pos[0], pos[1], pos[2],temps_ecoule*TIKER_GENERALE_US/1000000); + //dbug.printf("Dis D:%f, G:%f, min:%f\n", DTD_avg_Ex, DTG_avg_Ex, DT_avg_min_Ex); + } + } + + } +} + +void automate_macro_auto() +{ + timer_macro_auto++; + switch(etat_macro_auto) + { + case init_macro_auto: + etat_macro_auto = init_macro_auto; timer_macro_auto = 0; + break; + } +} + +void automate_3balles_odo() +{ + timer_3balles_odot++; + switch(etat_3balles_odo) + { + case 1: + ejection_active = 1; + ejection_continue = 1; + GoStraight(1805,0,0,0); + etat_3balles_odo = 2; timer_3balles_odot = 0; + break; + + case 2 : + if(flagFinDpl) + {flagFinDpl = 0; + flagDaniels = 1; + Rotate(-930);//rotation acoté de la balle + etat_3balles_odo = pied_ballon_tbc; timer_3balles_odot = 0; + }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; + break; + + case 3 : + if(flagFinDpl) + {flagFinDpl = 0; + ejection_continue = 0; + GoStraight(1500,0,0,0);//on va vers la deuxieme balle + etat_3balles_odo = 4; timer_3balles_odot = 0; + }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; + break; + + case 4 : + if(flagFinDpl) + {flagFinDpl = 0; + Rotate(940);//deuxieme balle gobé ,on tourne + etat_3balles_odo = 5; timer_3balles_odot = 0; + }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; + break; + + case 5 : + if(flagFinDpl) + {flagFinDpl = 0; + envoyer_balle = 1;ejection_continue = 1;//tir la deuxieme balle + flagDaniels = 1; + Rotate(-940);//ici implementer le deuxieme ciblage laser + etat_3balles_odo = 6; timer_3balles_odot = 0; + }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; + break; + + case 6: + if(flagFinDpl) + {flagFinDpl = 0; + ejection_continue = 0; + GoStraight(1500,0,0,0); + etat_3balles_odo = 7; timer_3balles_odot = 0; + }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; + break; + + case 7: + if(flagFinDpl) + {flagFinDpl = 0; + Rotate(930); + etat_3balles_odo = 8; timer_3balles_odot = 0; + }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; + break; + + case 8: + if(flagFinDpl) + {flagFinDpl = 0; + envoyer_balle = 1;ejection_continue = 1;//envoie de la troisième balle + etat_3balles_odo = 9; timer_3balles_odot = 0; + sensrotation=1;//on est a gauche sur le terrain ,donc on cible vers la droite + }else if(timer_3balles_odot>TIMEOUT_ERROR_TIC) etat_3balles_odo = 10; + break; + + case 9: + break; + + case 10: + break; + } +} + + + +void automate_tirer_balle_complet() +{ + timer_tirer_balle_complet++; + switch(etat_tirer_balle_complet) + { + case init_tirer_balle_complet: + sensrotation = -sensrotation; + etat_pied_ballon = init_pied_ballon_dierct; + etat_tirer_balle_complet = pied_ballon_tbc; timer_tirer_balle_complet = 0; + break; + + case pied_ballon_tbc: + automate_pied_ballon(); + if(etat_pied_ballon == fin_pied_ballon) + { + etat_visee_balle = init_visee_balle; + etat_tirer_balle_complet = visee_balle_tbc; timer_tirer_balle_complet = 0; + } + break; + + case visee_balle_tbc: + automate_visee_balle(); + if(etat_visee_balle == fin_visee_balle) + { + //etat_visee_balle = init_visee_balle; + etat_tirer_balle_complet = fin_tirer_balle_complet; timer_tirer_balle_complet = 0; + } + else if(etat_visee_balle == error_visee_balle) + { + //etat_visee_balle = init_visee_balle; + etat_tirer_balle_complet = error_tirer_balle_complet; timer_tirer_balle_complet = 0; + } + break; + + case fin_tirer_balle_complet: + break; + + case error_tirer_balle_complet: + break; + + + } +} + +void automate_pied_ballon() +{ + timer_pied_ballon++; + switch(etat_pied_ballon) + { + case init_pied_ballon_dierct: + short y_temp; + if(sensrotation==1)y_temp = 300; + else y_temp = 3700; + GoToPosition(3500, y_temp, 0, 0); + etat_pied_ballon = avance_x_DT_pb; timer_pied_ballon = 0; + break; + + case avance_x_DT_pb: + if(flagFinDpl) + {flagFinDpl = 0; + GoStraight(3050-DTA_avg_Ex,0,0,0); + //SetOdometrie(26,pos[0],y_temp,sensrotation*900); + etat_pied_ballon = rotation_ballon_x_pb; timer_pied_ballon = 0; + }else if(timer_pied_ballon>TIMEOUT_ERROR_TIC) etat_pied_ballon = error_pied_ballon; + break; + + case rotation_ballon_x_pb: + if(flagFinDpl) + {flagFinDpl = 0; + if(DTA_avg_Ex<3070 && DTA_avg_Ex>3020){ + Rotate(sensrotation*900); + etat_pied_ballon = reculer_ballon_x_pb; timer_pied_ballon = 0; + } + }else if(timer_pied_ballon>TIMEOUT_ERROR_TIC) etat_pied_ballon = error_pied_ballon; + break; + + case reculer_ballon_x_pb: + if(flagFinDpl) + {flagFinDpl = 0; + GoStraight(-250,0,0,0); + etat_pied_ballon = recalage_pied_pb; timer_pied_ballon = 0; + }else if(timer_pied_ballon>TIMEOUT_ERROR_TIC) etat_pied_ballon = error_pied_ballon; + break; + + case recalage_pied_pb: + if(flagFinDpl) + {flagFinDpl = 0; + short y_temp; + if(sensrotation==1)y_temp = 55; + else y_temp = 3945; + SetOdometrie(26,3050,y_temp,sensrotation*900); + etat_pied_ballon = fin_pied_ballon; timer_pied_ballon = 0; + }else if(timer_pied_ballon>TIMEOUT_ERROR_TIC) etat_pied_ballon = error_pied_ballon; + break; + + case fin_pied_ballon: + break; + + case error_pied_ballon: + break; + } + if(etat_pied_ballon != etat_pied_ballon_prec)dbug.printf("Etat visee_balle : %d\n",etat_pied_ballon); + etat_pied_ballon_prec = etat_pied_ballon; +} + + + +void automate_visee_balle() +{ + timer_visee_balle++; + + switch(etat_visee_balle) + { + case init_visee_balle ://--------------------------------------0 + SendSpeed(80, 600); + Rotate(sensrotation*900); + etat_visee_balle = fin_vir_rapide; + timer_visee_balle=0; + break; + + case fin_vir_rapide : //--------------------------------------1 + if(balleg || balled ) + {flagFinDpl = 0; + dbug.printf("balle 1-----1\n"); + pos_balle[0][2] = pos[2]; + SendRawId(ASSERVISSEMENT_STOP); + timer_visee_balle = 0; + etat_visee_balle = debut_vir_lent; + } else if(flagFinDpl || timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; + break; + + case debut_vir_lent://--------------------------------------2 + if(flagFinDpl) + {flagFinDpl = 0; + dbug.printf("balle 2-----2\n"); + SendSpeed(18, 300); + Rotate((pos_balle[0][2]-pos[2])*2); + etat_visee_balle = fin_vir_lent; + }else if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; + break; + + case balle_disparue: + break; + + case fin_vir_lent://--------------------------------------3 + if(balleg && balled) + {flagFinDpl = 0; + pos_balle[0][2] = pos[2]; + SendRawId(ASSERVISSEMENT_STOP); + SendSpeed(100, 600); + if(DT_avg_min_Ex<2000)etat_visee_balle = avancer; + else etat_visee_balle = avancer_20; + timer_visee_balle = 0; + }else{ + etat_visee_balle = debut_vir_lent; + flagFinDpl = 1; + } + break; + + + case avancer_20://--------------------------------------4 + if(flagFinDpl) + {flagFinDpl = 0; + GoStraight(DT_avg_min_Ex-D_ROULEAU_AXE-500,0,0,0); + etat_visee_balle = debut_vir_lent2; + timer_visee_balle = 0; + }else if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; + break; + + case debut_vir_lent2://--------------------------------------5 + if(flagFinDpl) + {flagFinDpl = 0; + + SendSpeed(18, 300); + + if(!balleg && !balled) + { + etat_visee_balle = error_visee_balle; + } + else{ + if(!balleg && balled) + { + BendRadius(139, sensrotation*30, sensrotation,0); + etat_visee_balle = fin_vir_lent2; + } + else if(balleg && !balled) + { + BendRadius(139, 30, sensrotation,0); + etat_visee_balle = fin_vir_lent2; + } + else if(balleg && balled) + { + flagFinDpl = 1; + SendSpeed(100, 600); + etat_visee_balle = avancer; + timer_visee_balle = 0; + } + } + }else if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; + break; + + case fin_vir_lent2://--------------------------------------6 + if(balleg && balled) + {flagFinDpl = 0; + SendRawId(ASSERVISSEMENT_STOP); + SendSpeed(100, 600); + etat_visee_balle = avancer; + timer_visee_balle = 0; + }else{flagFinDpl = 1; + etat_visee_balle = debut_vir_lent2; + } + break; + + + case avancer://--------------------------------------7 + if(flagFinDpl) + {flagFinDpl = 0; + ejection_continue = 0; + GoStraight(DT_avg_min_Ex-(D_ROULEAU_AXE),0,0,0); + etat_visee_balle = rotation; + } + if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; + break; + + case rotation://--------------------------------------8 + if(flagFinDpl) + {flagFinDpl = 0; + if(balle_gobbee) + { + short dTeta = -pos[2]; + Rotate(dTeta); + etat_visee_balle = lancer; + timer_visee_balle = 0; + }else etat_visee_balle = error_visee_balle; + } + break; + + case lancer: + if(flagFinDpl) + {flagFinDpl = 0; + envoyer_balle = 1; + ejection_continue = 1; + etat_visee_balle = fin_visee_balle; + + } if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; + break; + + case fin_visee_balle: + ejection_continue = 1; + break; + case error_visee_balle: + ejection_continue = 1; + break; + + case retour : + GoToPosition(300, 0, 0, 0); + etat_visee_balle = recalage; + timer_visee_balle = 0; + break; + + case recalage : + if(flagFinDpl) + {flagFinDpl = 0; + GoStraight(-295,0,0,0); + etat_visee_balle = recalage2; + timer_visee_balle = 0; + }if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; + break; + case recalage2 : + if(flagFinDpl) + {flagFinDpl = 0; + GoStraight(30,2,50,0); + etat_visee_balle = recalage2; + timer_visee_balle = 0; + }if(timer_visee_balle>TIMEOUT_ERROR_TIC) etat_visee_balle = error_visee_balle; + break; + case recalage3 : + if(flagFinDpl) + {flagFinDpl = 0; + etat_visee_balle = init_visee_balle; + } + break; + } + if(etat_visee_balle != etat_visee_balle_prec)dbug.printf("Etat visee_balle : %d\n",etat_visee_balle); + etat_visee_balle_prec = etat_visee_balle; +} + +void automate_base() +{ + switch(etat_auto_base) + { + case 0 : + temps_ecoule = 0; + etat_auto_base = 1; + break; + + case 1 : + ejection_active = 1; + ejection_continue = 1; + GoStraight(1805,0,0,0); + etat_auto_base=2; + break; + + case 2 : + if(flagFinDpl) + {flagFinDpl = 0; + flagDaniels = 1; + Rotate(-850);//rotation acoté de la balle + etat_auto_base=201; + + } + break; + + case 201 : + if(flagFinDpl) + {flagFinDpl = 0; + sensrotation=-1; + etat_visee_balle = init_visee_balle; + etat_auto_base=202; + } + break; + + case 202 : + automate_visee_balle(); //ciblage vers la gauche + if(etat_visee_balle == init_visee_balle)//on attend d'etre sur la balle + { + etat_visee_balle = init_visee_balle; + etat_auto_base = 3; + } + break; +//---------------------FIN--------------------------- + + case 3 : + ejection_continue = 0; + GoStraight(1500,0,0,0);//on va vers la deuxieme balle + etat_auto_base=4; + break; + + case 4 : + if(flagFinDpl) + {flagFinDpl = 0; + Rotate(900);//deuxieme balle gobé ,on tourne + etat_auto_base=5; + } + break; + + case 5 : + if(flagFinDpl) + {flagFinDpl = 0; + envoyer_balle = 1;//tir la deuxieme balle + flagDaniels = 1; + Rotate(-850);//ici implementer le deuxieme ciblage laser + etat_auto_base=601; + } + break; + + case 601 : + if(flagFinDpl) + {flagFinDpl = 0; + sensrotation=-1; + etat_visee_balle = init_visee_balle; + etat_auto_base=602; + } + break; + + case 602 : + automate_visee_balle(); //ciblage vers la gauche + if(etat_visee_balle == 4)//on attend d'etre sur la balle + { + etat_visee_balle = init_visee_balle; + etat_auto_base = 7; + } + break; + + case 7: + ejection_continue = 0; + GoStraight(1500,0,0,0); + etat_auto_base=8; + break; + + case 8 : + if(flagFinDpl) + {flagFinDpl = 0; + Rotate(900); + etat_auto_base=9; + } + break; + + case 9 : + if(flagFinDpl) + {flagFinDpl = 0; + envoyer_balle = 1;//envoie de la troisième balle + etat_auto_base=10; + sensrotation=1;//on est a gauche sur le terrain ,donc on cible vers la droite + } + break; + + case 10: + GoStraight(1500,0,0,0); + etat_auto_base=11; + break; + + case 11 : + if(flagFinDpl) + {flagFinDpl = 0; + Rotate(900); + //cmd_servo = 2; + etat_auto_base=12; + } + break; + + case 12 : + if(flagFinDpl) + {flagFinDpl = 0; + GoStraight(100,2,0,0); + cmd_servo = 2; + etat_auto_base=13; + } + break; + case 13 : + if(flagFinDpl) + {flagFinDpl = 0; + GoStraight(3800,0,0,0); + cmd_servo = 1; + etat_auto_base=14; + } + break; + + case 14 : + if(flagFinDpl) + {flagFinDpl = 0; + GoStraight(-500,0,0,0); + etat_auto_base=15; + } + break; + + case 15 : + if(flagFinDpl) + {flagFinDpl = 0; + Rotate(1800); + cmd_servo = 0; + etat_auto_base=16; + sensrotation=1; + } + break; + case 16 : + + break; + + + + case 100 : + etat_visee_balle = init_visee_balle;//On passe dans l'automate de ciblage + etat_auto_base=101; + break; + + case 101 : + automate_visee_balle(); //ciblage vers la gauche + if(etat_visee_balle == init_visee_balle)//on attend d'etre sur la balle + { + etat_visee_balle = init_visee_balle; + etat_auto_base = 102; + } + break; + + case 102 : + if(flagFinDpl) + {flagFinDpl = 0; + ejection_continue = 0; + GoStraight(DTD_avg_Ex,0,0,0); + etat_auto_base=103; + } + break; + + case 103 : + if(flagFinDpl) + {flagFinDpl = 0; + Rotate(-pos[2]); + etat_auto_base=100; + } + break; + default : + + break; + + } + if(etat_auto_base != etat_auto_base_prec)dbug.printf("Etat auto_base: %d\n",etat_auto_base); + etat_auto_base_prec = etat_auto_base; +} + +void wait_debut_de_partie() +{ + if(!Daniels_in) + { + dbug.printf("-----DEBUT DE PARTIE------\n"); + match_en_cours = 1; + } +} + +void user_bp_ar(){UBp_it.enable_irq();} +void isr_UBp() +{ + UBp_it.disable_irq();dbug.printf("ubp\n"); + ubp_to.attach(&user_bp_ar, 0.3); + flagUBp = 1; +}
diff -r 000000000000 -r aebeb20b201a robot_general.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/robot_general.h Thu Mar 21 14:59:13 2019 +0000 @@ -0,0 +1,25 @@ +#ifndef ROBOT_GEN_H +#define ROBOT_GEN_H + +#include "DT35_TOR.h" +#include "CAN_asser.h" +#include "ejecteur.h" +#include "Disquette.h" + +#define TIKER_GENERALE_US 2000 + +#define DUREE_MATCH_SEC 87 +#define DUREE_MATCH_TIC DUREE_MATCH_SEC*1000000/TIKER_GENERALE_US + +#define DELTA_SECU_SEC 5 +#define DELTA_SECU_TIC DELTA_SECU_SEC*1000000/TIKER_GENERALE_US + +#define TEMP_CERVE_BALLON_SEC 5 +#define TEMP_CERVE_BALLON_TIC TEMP_CERVE_BALLON_SEC*1000000/TIKER_GENERALE_US + +#define TIMEOUT_ERROR_SEC 10 +#define TIMEOUT_ERROR_TIC TIMEOUT_ERROR_SEC*1000000/TIKER_GENERALE_US + +extern Serial dbug; + +#endif \ No newline at end of file