File content as of revision 26:4670968fbc63:

// Nom du fichier : debugPC.cpp
#include "pins.h"

// Variables globales
Serial pc(USBTX, USBRX);
Ticker Ticker_affUS;

bool aff_US[6];
bool aff_cd[4];
bool aff_odo[3];
bool m_dis = true;    // 0 : START // 1 : STOP
bool m_avance = false;   // 0 : BACKWARD // 1 : FORWARD
bool mtrt_sens = false;
int cmdType=0;
long comptG_saved = 0, comptD_saved = 0; // Sauvegarde nombre de tics codeurs

void serialIT() // avec Tera Term
{
    //pc.printf("\n\rserialIT on\n\r");
    static int i=0;
    static char buffer[10]=""; // Tableau qui contient la chaine de caractère rentrée dans le terminal.
    static char cmd[Lcmd]=""; // Variable qui retient que les premiers caractères qui représentent la commande.

    char tampon = pc.getc();

    if((tampon >= 48 && tampon <=57) || (tampon>=97 && tampon<=122) || tampon==13) {
        buffer[i]=tampon; // Ajout du caractère dans le tableau buffer
        pc.putc(buffer[i]); // Réécriture sur le terminal du caractère envoyé
        i++;
    }

    if(buffer[i-1]=='\r') { // Attente un appui sur la touche entrée
        i=0;
        copieTab(buffer,cmd); // Sauvegarde la commande dans le tableau cmd

        switch(cmdType) {
            case 1 : // Commande test angle
                StringToAngle(cmd);
                cmdType=0;
                break;
            case 2 : // Commande test vitesse
                StringToVitesse(cmd);
                cmdType=0;
                break;
            case 3 : // Commande distance a parcourir
                StringToDist(cmd);
                cmdType=0;
                break;
            default : // Commande par défaut
                cmdChoice(cmd);
        }
    }
}

void copieTab(char *tab1,char *tab2)   // Fonction qui recopie un tableau dans un autre
{
    //pc.printf("\n\rcopieTab on\n\r");
    //pc.printf("\n\r");
    for(int j=0; j<Lcmd; j++) {
        tab2[j]=tab1[j];
        //pc.printf("%c",tab2[j]);
    }
    //pc.printf("\n\r");
}

void cmdChoice(char *cmd)   // Fonction qui permet de choisir de l'activation d'une commande
{
    const char *options[]= {
        "help", //0
        "usao", //1
        "us1o", //2
        "us2o", //3
        "us3o", //4
        "us4o", //5
        "us5o", //6
        "us6o", //7
        "tdrv", //8
        "cdon", //9
        "cdga", //10
        "cdgb", //11
        "tagl", //12
        "mten", //13
        "mtdr", //14
        "mtvt", //15
        "mtrt", //16
        "cofr", //17
        "cofa", //18
        "parc", //19
        "odom", //20
        0
    };

    long option=-1;

    for (long a=0; options[a] && option<0; a++) {
        if (!strcmp(cmd,options[a]) || strcmp(cmd,options[a])==1) { // strcmp(cmd,options[a])==1 permet de contourner le problème, à revoir !!!
            option=a;
        }
        //pc.printf("res = %d\r\n",strcmp(cmd,options[a]));
    }

    switch (option) {
        case 0:     //help
            // pc
            pc.printf("\n\n\r###HELP###\n\r");
            pc.printf("usao : Affichage resultats capteurs a ultrasons\n\r");
            pc.printf("usxo : Affichage resultat capteur ultrasons x\n\r");
            pc.printf("tdrv : Appelle une fonction test\n\r");
            pc.printf("cdon : Affichage resultats codeurs\n\r");
            pc.printf("cdgx : Affichage resultats codeur gauche phase x (a ou b)\n\r");
            pc.printf("tagl : Tourne le robot sur lui-meme avec un angle a preciser\r\n");
            pc.printf("mten : Activation/Desactivation des moteurs\r\n");
            pc.printf("mtdr : Changement de de sens de rotation des moteurs\r\n");
            pc.printf("mtvt : Changement de la vitesse de rotation (vitesse a preciser)\r\n");
            pc.printf("mtrt : Sens de rotation des moteurs opposes\r\n");
            pc.printf("cofr : Enregistrement nb tics et reset\r\n");
            pc.printf("cofa : Affichage nb tics\r\n");
            pc.printf("parc : Distance a parcourir\r\n");
            pc.printf("odom : Afficher les valeurs pour l'odometrie\r\n");
            pc.printf("\n\r");
            // bt
            bt.printf("\n\n\r###HELP###\n\r");
            bt.printf("usao : Affichage resultats capteurs a ultrasons\n\r");
            bt.printf("usxo : Affichage resultat capteur ultrasons x\n\r");
            bt.printf("tdrv : Appelle une fonction test\n\r");
            bt.printf("cdon : Affichage resultats codeurs\n\r");
            bt.printf("cdgx : Affichage resultats codeur gauche phase x (a ou b)\n\r");
            bt.printf("tagl : Tourne le robot sur lui-meme avec un angle a preciser\r\n");
            bt.printf("mten : Activation/Desactivation des moteurs\r\n");
            bt.printf("mtdr : Changement de de sens de rotation des moteurs\r\n");
            bt.printf("mtvt : Changement de la vitesse de rotation (vitesse a preciser)\r\n");
            bt.printf("mtrt : Sens de rotation des moteurs opposes\r\n");
            bt.printf("cofr : Enregistrement nb tics et reset\r\n");
            bt.printf("cofa : Affichage nb tics\r\n");
            bt.printf("parc : Distance a parcourir\r\n");
            bt.printf("odom : Afficher les valeurs pour l'odometrie\r\n");
            bt.printf("\n\r");
            break;
        case 1:     //usao
            pc.printf("Capt US ALL ON/OFF\n\r");
            bt.printf("Capt US ALL ON/OFF\n\r");
            aff_US[0]=!aff_US[0];
            aff_US[1]=!aff_US[1];
            aff_US[2]=!aff_US[2];
            aff_US[3]=!aff_US[3];
            aff_US[4]=!aff_US[4];
            aff_US[5]=!aff_US[5];
            break;
        case 2:     //us1o
            pc.printf("Capt US 1 ON/OFF\n\r");
            bt.printf("Capt US 1 ON/OFF\n\r");
            aff_US[0]=!aff_US[0];
            break;
        case 3:     //us2o
            pc.printf("Capt US 2 ON/OFF\n\r");
            bt.printf("Capt US 2 ON/OFF\n\r");
            aff_US[1]=!aff_US[1];
            break;
        case 4:     //us3o
            pc.printf("Capt US 3 ON/OFF\n\r");
            bt.printf("Capt US 3 ON/OFF\n\r");
            aff_US[2]=!aff_US[2];
            break;
        case 5:     //us4o
            pc.printf("Capt US 4 ON/OFF\n\r");
            bt.printf("Capt US 4 ON/OFF\n\r");
            aff_US[3]=!aff_US[3];
            break;
        case 6:     //us5o
            pc.printf("Capt US 5 ON/OFF\n\r");
            bt.printf("Capt US 5 ON/OFF\n\r");
            aff_US[4]=!aff_US[4];
            break;
        case 7:     //us6o
            pc.printf("Capt US 6 ON/OFF\n\r");
            bt.printf("Capt US 6 ON/OFF\n\r");
            aff_US[5]=!aff_US[5];
            break;
        case 8:     //tdrv
            pc.printf("Fonction test_drv()\n\r");
            bt.printf("Fonction test_drv()\n\r");
            //test_drv();
            //comptG = 0;
            //comptD = 0;
            //test1();
            //consigneOrientation = (180*3.1415)/180;
            //xC = (double)0;
            //yC = (double)100;
            //fnc=1;

            /*
            indice++;
            fnc = objEtape[indice];
            xC = objX[indice];
            yC = objY[indice];
            mot_en();
            */
            break;
        case 9:     //cdon
            pc.printf("Results ALL Encoders ON/OFF\n\r");
            bt.printf("Results ALL Encoders ON/OFF\n\r");
            aff_cd[0]=!aff_cd[0];
            aff_cd[1]=!aff_cd[1];
            break;
        case 10:    //cdga
            pc.printf("Results Encoder Left A ON/OFF\n\r");
            bt.printf("Results Encoder Left A ON/OFF\n\r");
            aff_cd[0]=!aff_cd[0];
            break;
        case 11:    //cdgb
            pc.printf("Results Encoder Left B ON/OFF\n\r");
            bt.printf("Results Encoder Left B ON/OFF\n\r");
            aff_cd[1]=!aff_cd[1];
            break;
        case 12:    //tagl
            pc.printf("Cmd Angle robot\r\n");
            pc.printf("Angle (entre 0 et 360 degres sous la forme xxx): ");
            bt.printf("Cmd Angle robot\r\n");
            bt.printf("Angle (entre 0 et 360 degres sous la forme xxx): ");
            cmdType=1;
            break;
        case 13:    // mten
            if(m_dis) {
                mot_en();
                m_dis=!m_dis;
                pc.printf("Motors Enable\r\n");
                bt.printf("Motors Enable\r\n");
            } else {
                mot_dis();
                m_dis=!m_dis;
                pc.printf("Motors Disable\r\n");
                bt.printf("Motors Disable\r\n");
            }
            break;
        case 14:    // mtdr
            if(m_avance) {
                pc.printf("Changement de direction : En avant\r\n");
                bt.printf("Changement de direction : En avant\r\n");
                motGauche_fwd();
                motDroite_fwd();
                m_avance=!m_avance;
            } else {
                pc.printf("Changement de direction : En arriere\r\n");
                bt.printf("Changement de direction : En arriere\r\n");
                motGauche_bck();
                motDroite_bck();
                m_avance=!m_avance;
            }
            break;
        case 15:    // mtvt
            pc.printf("Cmd Vitesse robot\r\n");
            pc.printf("Vitesse en mm/s (sous la forme xxxx): ");
            bt.printf("Cmd Vitesse robot\r\n");
            bt.printf("Vitesse en mm/s (sous la forme xxxx): ");
            cmdType=2;
            break;
        case 16:    // mtrt
            pc.printf("Rotation du robot - Changement de sens");
            bt.printf("Rotation du robot - Changement de sens");
            mtrt_sens = !mtrt_sens;
            if(mtrt_sens) {
                motGauche_fwd();
                motDroite_bck();
            } else {
                motGauche_bck();
                motDroite_fwd();
            }
            break;
        case 17 :
            comptG_saved = comptG;
            comptD_saved = comptD;
            comptG=0;
            comptD=0;
            pc.printf("comptG et comptD sauvegardes\r\n");
            bt.printf("comptG et comptD sauvegardes\r\n");
            break;
        case 18 :
            pc.printf("comptG_saved = %ld\r\n",comptG_saved);
            pc.printf("comptD_saved = %ld\r\n",comptD_saved);
            bt.printf("comptG_saved = %ld\r\n",comptG_saved);
            bt.printf("comptD_saved = %ld\r\n",comptD_saved);
            break;
        case 19 : //
            bt.printf("Distance a parcourir : ");
            cmdType=3;
            break;
        case 20: // odom
            pc.printf("Odometrie :\n\r");
            pc.printf("x = %lf\n\r", x);
            pc.printf("y = %lf\n\r", y);
            pc.printf("O = %lf\n\r", O);
            pc.printf("consigneAngl = %lf\n\r", consigneOrientation);
            pc.printf("consigneDist = %lf\n\r", distanceCible);
            pc.printf("indice = %d\n\r", indice);
            //pc.printf("dist 1 = %d\n\r", ttt[0]);
            //pc.printf("dist 2 = %d\n\r", ttt[1]);
            //pc.printf("dist 6 = %d\n\r", ttt[5]);
            pc.printf("---\n\r");
            //pc.printf("dist 3 = %d\n\r", ttt[2]);
            //pc.printf("dist 4 = %d\n\r", ttt[3]);
            //pc.printf("dist 5 = %d\n\r", ttt[4]);

            bt.printf("Odometrie :\n\r");
            bt.printf("x = %lf\n\r", x);
            bt.printf("y = %lf\n\r", y);
            bt.printf("O = %lf\n\r", O);
            bt.printf("consigneAngl = %lf\n\r", consigneOrientation);
            bt.printf("consigneDist = %lf\n\r", distanceCible);
            bt.printf("indice = %d\n\r", indice);
            bt.printf("dist 1 = %lf\n\r", distt[0]);
            bt.printf("dist 2 = %lf\n\r", distt[1]);
            bt.printf("dist 6 = %lf\n\r", distt[5]);
            bt.printf("---\n\r");
            bt.printf("dist 3 = %lf\n\r", distt[2]);
            bt.printf("dist 4 = %lf\n\r", distt[3]);
            bt.printf("dist 5 = %lf\n\r", distt[4]);
            bt.printf("distanceMem = %lf\n\r", distanceMem);
            bt.printf("distancePlus = %lf\n\r", distancePlus);
            break;
        default:
            pc.printf("Commande invalide\n\r");
            bt.printf("Commande invalide\n\r");
    }
}

void StringToAngle(char *cmd)
{
    int cmd2 = 0;
    cmd2 += (cmd[0]-48)*100;
    cmd2 += (cmd[1]-48)*10;
    cmd2 += (cmd[2]-48)*1;

    if (cmd2>=0 && cmd2<=360) {
        consigneOrientation = (cmd2*3.1415)/180;
        motGauche_bck();
        motDroite_fwd();
        mot_en();
        bt.printf("\r\nCommande envoyee au robot : %lf\r\n",consigneOrientation);
    } else {
        pc.printf("\r\nAngle incorrect\r\n\n");
        bt.printf("\r\nAngle incorrect\r\n\n");
        cmd2 = 0;
    }
}

void StringToDist(char *cmd)
{
    int cmd4 = 0;
    cmd4 += (cmd[0]-48)*1000;
    cmd4 += (cmd[1]-48)*100;
    cmd4 += (cmd[2]-48)*10;
    cmd4 += (cmd[3]-48)*1;

    if (cmd4>=0 && cmd4<=9999) {
        //consigneDistance = cmd4;
        //pc.printf("\r\nCommande envoyee au robot : %lf\r\n",consigneDistance);
        //bt.printf("\r\nCommande envoyee au robot : %lf\r\n",consigneDistance);

    } else {
        pc.printf("\r\nDistance incorrecte\r\n\n");
        bt.printf("\r\nDistance incorrecte\r\n\n");
        cmd4 = 0;
    }
}

void StringToVitesse(char *cmd)
{
    for(int y=0; y<Lcmd ; y++) {
        bt.printf("%d ",cmd[y]);
    }
    bt.printf("\r\n");

    float cmd3 = 0;
    cmd3 += (cmd[0]-48)*1000;
    cmd3 += (cmd[1]-48)*100;
    cmd3 += (cmd[2]-48)*10;
    cmd3 += (cmd[3]-48)*1;

    if (cmd3>=0 && cmd3<=9999) {
        cmd3 = cmd3 / 1000;
        pc.printf("\r\nCommande envoyee au robot : %.3f m/s\r\n",cmd3);
        bt.printf("\r\nCommande envoyee au robot : %.3f m/s\r\n",cmd3);
        drvGauche.moveLinSpeed(cmd3);
        drvDroite.moveLinSpeed(cmd3);
    } else {
        pc.printf("\r\nVitesse incorrect\r\n\n");
        bt.printf("\r\nVitesse incorrect\r\n\n");
        cmd3 = 0;
    }
}

void affUltrasons()
{
    // Données bruts

    /*
    // pc
    if(aff_US[0]) pc.printf("Tps US1 = %5.0f uS\n\r", us_diff[0]);
    if(aff_US[1]) pc.printf("Tps US2 = %5.0f uS\n\r", us_diff[1]);
    if(aff_US[2]) pc.printf("Tps US3 = %5.0f uS\n\r", us_diff[2]);
    if(aff_US[3]) pc.printf("Tps US4 = %5.0f uS\n\r", us_diff[3]);
    if(aff_US[4]) pc.printf("Tps US5 = %5.0f uS\n\r", us_diff[4]);
    if(aff_US[5]) pc.printf("Tps US6 = %5.0f uS\n\r", us_diff[5]);
    if(aff_US[0]||aff_US[1]||aff_US[2]||aff_US[3]||aff_US[4]||aff_US[5]) pc.printf("\n\r");

    // bt
    if(aff_US[0]) bt.printf("Tps US1 = %5.0f uS\n\r", us_diff[0]);
    if(aff_US[1]) bt.printf("Tps US2 = %5.0f uS\n\r", us_diff[1]);
    if(aff_US[2]) bt.printf("Tps US3 = %5.0f uS\n\r", us_diff[2]);
    if(aff_US[3]) bt.printf("Tps US4 = %5.0f uS\n\r", us_diff[3]);
    if(aff_US[4]) bt.printf("Tps US5 = %5.0f uS\n\r", us_diff[4]);
    if(aff_US[5]) bt.printf("Tps US6 = %5.0f uS\n\r", us_diff[5]);
    if(aff_US[0]||aff_US[1]||aff_US[2]||aff_US[3]||aff_US[4]||aff_US[5]) bt.printf("\n\r");
    */

    
    // Distances

    // pc
    /*
    pc.printf("US[1] = %lf\n\r", us_diff[0]);
    pc.printf("US[2] = %lf\n\r", us_diff[1]);
    pc.printf("US[3] = %lf\n\r", us_diff[2]);
    pc.printf("US[4] = %lf\n\r", us_diff[3]);
    pc.printf("US[5] = %lf\n\r", us_diff[4]);
    pc.printf("US[6] = %lf\n\r", us_diff[5]);
    pc.printf("\n\r");
    */
    
    ///*
    pc.printf("distance[1] = %lf\n\r", distt[0]);
    pc.printf("distance[2] = %lf\n\r", distt[1]);
    pc.printf("distance[3] = %lf\n\r", distt[2]);
    pc.printf("distance[4] = %lf\n\r", distt[3]);
    pc.printf("distance[5] = %lf\n\r", distt[4]);
    pc.printf("distance[6] = %lf\n\r", distt[5]);
    pc.printf("\n\r");
    //*/

    // bt
    
    bt.printf("distance[1] = %lf\n\r", distt[0]);
    bt.printf("distance[2] = %lf\n\r", distt[1]);
    bt.printf("distance[3] = %lf\n\r", distt[2]);
    bt.printf("distance[4] = %lf\n\r", distt[3]);
    bt.printf("distance[5] = %lf\n\r", distt[4]);
    bt.printf("distance[6] = %lf\n\r", distt[5]);
    bt.printf("\n\r");
    
}

void affCodeurs()
{
    if(aff_cd[0]) pc.printf("comptG = %d\n\r", comptG);
    if(aff_cd[1]) pc.printf("comptD = %d\n\r", comptD);

    if(aff_cd[0]) bt.printf("comptG = %d\n\r", comptG);
    if(aff_cd[1]) bt.printf("comptD = %d\n\r", comptD);
}

/*
void affOdo()
{
    if(aff_odo[0]) bt.printf("x = %f\n\r", xB);
    if(aff_odo[1]) bt.printf("y = %f\n\r", yB);
    if(aff_odo[2]) bt.printf("phi = %f\n\r", phiB*180/_PI_);
}
*/