Collin Alexis
Code FRC 2018
Diff: robot_general.cpp
- Revision:
- 0:aebeb20b201a
--- /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;
+}