Moteurs
Dependencies: Encoder_Nucleo_16_bits PwmIn mbed
Fork of Automate by
main.cpp
- Committer:
- DOREL
- Date:
- 2017-06-09
- Revision:
- 8:ad8b64ca548d
- Parent:
- 6:88b4805d33e1
- Child:
- 9:168226ff8f76
File content as of revision 8:ad8b64ca548d:
/** Main Test Board * * \brief Programme de tests pour le robot NCR 2017 * \author H. Angelis * \version alpha_1 * \date 15/05/17 * */ #include "include_define_typedeflibrary.h" #include "function_library.h" int main() { int I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, MOTG_check = -1, MOTD_check = -1*/; Byte PIXY_rCCObjet = 0, PIXY_rNMObjet = 0; int PIXY_objet; int SERVO_pulseWidth = 400, SERVO_max = 1400, SERVO_min = 400; T_SERVODIR SERVO_dir = S_monte; char MENU_choix = 0; char BOUSSOLE_status[1] = {0}; char I2C_registerValue[4]; double BOUSSOLE_periode; double CAP_I2C, CAP_PWM; double CNY1_val, CNY2_val, CNY3_val, Vbat_val; double SD2_dist, LD2_dist; int MOTG_evol = 1, MOTD_evol = 1; double MOTG_duty = 0.5, MOTD_duty = 0.5; //Variables Automate int etat_actuel = 0, etat_futur = 0, etat_precedent = 0; //Need etat_precedent ? //Variables test capteurs int ball_found = 0, ball_forward = 0, ball_captured = 0, face_ennemy_side = 0, face_our_side = 0; int wall = 0, white_line_crossed = 0, return_done = 0, rotation_done; //Rename white_line_crossed //Variables int orientation_camp_adversaire, orientation_mon_camp; times.reset(); times.start(); // Initialisation des interruptions tick.attach(&tickTime, 0.001); BP.rise (&BPevent); Echo1.rise (&echo1Rise); Echo2.rise (&echo2Rise); Echo3.rise (&echo3Rise); Echo1.fall (&echo1Fall); Echo2.fall (&echo2Fall); Echo3.fall (&echo3Fall); PWMG.rise (&PWM_motGRise); PWMD.rise (&PWM_motDRise); Pixy.attach (&getPixyByte); BP.enable_irq(); IG.enable_irq(); Echo1.enable_irq(); Echo2.enable_irq(); Echo3.enable_irq(); PWMG.enable_irq(); PWMD.enable_irq(); // Initialisation des périphériques // Bus I2C Bus_I2C.frequency (100000); // PWM des moteurs Pwm_MG.period_us(50); Pwm_MD.period_us(50); En = 0; // Bus SPI SPIG.format (16,1); SPIG.frequency (1000000); SPID.format (16,1); SPID.frequency (1000000); SS = 1; // Led Led2 = 0; Servo.period_ms (20); Servo.pulsewidth_us(200); while(1) { //capteurs_read(); //Lecture tous les capteurs CAP_PWM = ((PWMB.pulsewidth()*1000)-1)*10; Pc.printf ("\r PWM = %4.1lf", CAP_PWM); //capteurs_test(); //Teste les valeurs des capteurs et change des variables booleenne etat_actuel = etat_futur; switch(etat_actuel) { case START: etat_futur = STOP_RETURN; break; case SEEK_BALL: if(ball_found) etat_futur = TURN_TO_BALL; if(!ball_found) etat_futur = SEEK_ROTATION; break; case SEEK_ROTATION: if(rotation_done) etat_futur = SEEK_BALL; case TURN_TO_BALL: if(ball_forward) etat_futur = GO_TO_BALL; break; case GO_TO_BALL: if(!ball_forward) etat_futur = TURN_TO_BALL; if(ball_captured & face_ennemy_side) etat_futur = BALL_LAUNCHING; if(ball_captured & !face_ennemy_side) etat_futur = CAPTURE_AND_TURN; break; case CAPTURE_AND_TURN: if(face_ennemy_side) etat_futur = RELEASE_CAPTURE; break; case WALL_CAPTURE_AND_TURN: if(!wall & face_ennemy_side) etat_futur = RELEASE_CAPTURE; break; case RELEASE_CAPTURE: etat_futur = BALL_LAUNCHING; break; case BALL_LAUNCHING: if(wall) etat_futur = WALL_CAPTURE_AND_TURN ; if(white_line_crossed) etat_futur = STOP_BALL_LAUNCHING; break; case STOP_BALL_LAUNCHING: //Add tempo ? etat_futur = TURN_TO_BASE; break; case TURN_TO_BASE: if(face_our_side) etat_futur = RETURN; break; case RETURN: //return_done is a tempo ? if(return_done) etat_futur = STOP_RETURN; else if(wall) etat_futur = WALL_RETURN; break; case WALL_RETURN: if(!wall) etat_futur = STOP_RETURN; break; case STOP_RETURN: etat_futur = SEEK_BALL; break; default : etat_futur = STOP_RETURN; break; } switch(etat_actuel) { case START: orientation_camp_adversaire = (int)CAP_PWM; orientation_mon_camp =(((int)CAP_PWM+180)%360); break; case SEEK_BALL: break; case TURN_TO_BALL: break; case GO_TO_BALL: //Avancer droit (ralentir a la fin) break; case CAPTURE_AND_TURN: //Baisser cage //Tourner //-------------------->Separer etapes ? break; case RELEASE_CAPTURE: //Lever cage break; case BALL_LAUNCHING: //Avancer droit a fond break; case STOP_BALL_LAUNCHING: //Arreter break; case TURN_TO_BASE: //Faire demi tour break; case RETURN: //Avancer pendant x secondes break; case STOP_RETURN: //S'arreter break; default : break; } } }