File content as of revision 9:168226ff8f76:

/** 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;

    //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 moteurs
    
    int md_pwm = 0;
    int mg_pwm = 0;
    int md_sens = 0;
    int mg_sens = 0;

    //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);
    
    //Moteurs
    En = 1;    
    
    while(1) {    
        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      SD1_val, SD2_val, LD1_val, LD2_val, CNY1_val, CNY2_val, CNY3_val, Vbat_val;
        double      SD1_dist, SD2_dist, LD1_dist, LD2_dist;     
        
        
        //capteurs_read(); //Lecture tous les capteurs
        CAP_PWM = ((PWMB.pulsewidth()*1000)-1)*10;
        
        //capteurs_test(); //Teste les valeurs des capteurs et change des variables booleenne 
        face_ennemy_side = (((int)CAP_PWM > (orientation_camp_adversaire - 10))&((int)CAP_PWM < (orientation_camp_adversaire + 10))); 
        
        //Actions moteurs
        Pwm_MD = md_pwm;
        Pwm_MG = mg_pwm;
        SensD = md_sens;
        SensG = mg_sens;
        
        //Test
        
        motor_command(100, 100, *mg_pwm, *md_pwm, *mg_sens, *md_sens);
        
        //Action cage
        
        //Automate
        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;
        }
    }
}