Collin Alexis
/
Code_FRC_2018
Code FRC 2018
robot_general.cpp
- Committer:
- AlexisCollin
- Date:
- 2019-03-21
- Revision:
- 0:aebeb20b201a
File content as of revision 0:aebeb20b201a:
#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; }