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;
        }
    }
}