Programme de test
-----------------
- Test de la direction et de la motorisation (rampes)
- Utilisation d'un capteur de distance sur Capt4 ou PC_1
pour régler la vitesse d'avance


-----------------
-----------------
#include "mbed.h"

Serial moduleBT(PC_10,PC_11);  // connection émetteur/récepteur bluetooth

DigitalOut my_led(LED1);
Serial my_pc(USBTX,USBRX);
PwmOut direction(PB_13);    // Servomoteur de direction
PwmOut moteur(PC_9);        // Motorisation / ESC

AnalogIn    capt1(PC_2);
AnalogIn    capt2(PC_3);
AnalogIn    capt3(PC_0);
AnalogIn    capt4(PC_1);
AnalogIn    capt5(PB_0);
AnalogIn    capt6(PA_4);

int angle_roues, vitesse;
char data = 'd';
float distance_c[6] = {0};

////////////////////////////////////
////////////// MAIN ////////////////
///////////////////////////////////

int main() {   
    // TRANSMISSION PC et BT
    my_pc.baud(115200);
    my_pc.printf("Programme Test \r\nVoiture Autonome \r\n6 capteurs\r\n");
    moduleBT.baud(9600);        //vitesse de transmission du capteur BT à la carte
    moduleBT.printf("Programme Test \r\nVoiture Autonome \r\n6 capteurs\r\n");
    
    // Initialisation moteur et direction
    direction.period_ms(20);        //période de réception du servomoteur
    direction.pulsewidth_us(1300);  //première commande du servomoteur -> tout droit
    moteur.period_ms(20);           //période de réception du moteur
    moteur.pulsewidth_us(1500);     // 1500 -> point mort
    wait_us(2000000);
    my_led = 1;
   
    moduleBT.printf("Init Direction\r\n");
    for(angle_roues = 1100; angle_roues < 1500; angle_roues+=10){
        direction.pulsewidth_us(angle_roues);  
        wait_us(100000);
    }
    angle_roues = 1300;
    direction.pulsewidth_us(angle_roues);  
    
    moduleBT.printf("Init Vitesse\r\n");
    moduleBT.printf("\tAvant\r\n");
    for(vitesse = 1500; vitesse < 1600; vitesse+=10){
        moteur.pulsewidth_us(vitesse);  
        wait_us(100000);
    }
    wait_us(1000000);
    moduleBT.printf("\tArrière\r\n");
    vitesse = 1500;
    moteur.pulsewidth_us(vitesse); 
    wait_us(100000);
    vitesse = 1400;
    moteur.pulsewidth_us(vitesse); 
    wait_us(100000);
    vitesse = 1500;
    moteur.pulsewidth_us(vitesse); 
    wait_us(100000);
    
    for(vitesse = 1500; vitesse > 1400; vitesse-=10){
        moteur.pulsewidth_us(vitesse);  
        wait_us(100000);
    } 
    wait_us(1000000);    
    moduleBT.printf("\tStop\r\n");
    vitesse = 1500;
    moteur.pulsewidth_us(vitesse); 
   
    moduleBT.printf("MAIN PROGRAM\r\n");
   
    while(1) {
        my_led = 0;
        distance_c[0] = capt1.read()*3.3;
        distance_c[1] = capt2.read()*3.3;
        distance_c[2] = capt3.read()*3.3;
        distance_c[3] = capt4.read()*3.3;
        distance_c[4] = capt5.read()*3.3;
        distance_c[5] = capt6.read()*3.3;
        
        for(int i = 0; i < 6; i++){
            my_pc.printf("\tD%d = %lf\r\n", i, distance_c[i]);
        }
        my_pc.printf("\r\n");
        // CONNECTION BLUETOOTH : ALLUMER ET DEMARRER LA VOITURE
        if (moduleBT.readable()) {   
            data = moduleBT.getc();     // récupérer un caractère sur la carte venant du capteur BT
            if (data == 'g'){           // g pour go -> démarrer la voiture
                moteur.pulsewidth_us(1700);     // 1600 -> vitesse de démarrage (la plus lente)
                data = '0';
            }
            if (data == 's') {          // s pour stop -> arrêter la voiture
                moteur.pulsewidth_us(1500);
                data = '0';
            }
        }
        if (my_pc.readable()) {   
            data = my_pc.getc();        // récupérer un caractère sur la carte venant du capteur BT
            if (data == 'g'){           // g pour go -> démarrer la voiture
                moteur.pulsewidth_us(1700);     // 1600 -> vitesse de démarrage (la plus lente)
                data = '0';
            }
            if (data == 's') {          // s pour stop -> arrêter la voiture
                moteur.pulsewidth_us(1500);
                data = '0';
            }
        }
        if(data == 'd'){
            float vit_c;
            wait_us(100000);
            if(distance_c[3] > 2){
                vitesse = 1500; // ARRET
            }
            else{
                vit_c = (3.3-distance_c[3])/3.3*200.0;
                vitesse = vit_c + 1500;
            }
            moteur.pulsewidth_us(vitesse);
            my_pc.printf("\tV = %lf / %d\r\n", vit_c, vitesse);
        }
    }
}