/* 
 * piloter deux moteurs à courant continu à distance 
 * Auteurs : TAKTAK Rami, AZAIZ Malek, KHITOUS Hamza & TOUJANI Adem 
 */

#include "mbed.h"
#include <stdint.h>

//moteur à gauche = ROUE 1
PwmOut moteur1(D8);                 // controle de la roue 1 
DigitalOut moteur1_direction(D6);   // sens de rotation de la roue 1 

//moteur à droite = ROUE 2 
PwmOut moteur2(D9);                 // controle de la roue 2 
DigitalOut moteur2_direction(D3);   // sens de rotation de la roue 2 

Serial pc(USBTX, USBRX);            // affichage sur ordinateur
Serial carte1(PC_10, PC_11);        // affichage sur ordinateur

/*
 *--------------------------------------------------------------------------------------------------------------------------------------------------------------
 *PILOTAGE DES MOTEURS:
 */

void marche_arriere(float rc){ 
    moteur1_direction = 0 ;
    moteur2_direction = 1 ;
        
    moteur1.write(rc);
    moteur2.write(rc);
    
    wait(1) ;
        
    moteur1.write(0);
    moteur2.write(0);
    
    }   

void marche_avant(float rc){ 
  
        moteur1_direction = 1 ;  
        moteur2_direction = 0 ;
        
        moteur1.write(rc);
        moteur2.write(rc);
        
        wait(1) ;
        
        moteur1.write(0);
        moteur2.write(0);
        
    }  

void turn_R(float rc) //( tourner de 90° à droite pour une consigne de 30 rad/s)
{       moteur1_direction = 1;
        moteur1.write(rc);
        moteur2.write(0);
        
        moteur2_direction = 1;
        
        wait(0.14);  // disance de l'arc  = pi/2 * R(=25cm , rayon de la roue)
        moteur1.write(0); 
        }

void turn_L(float rc) //( tourner de 90° à gauche pour une consigne de 30 rad/s)
{       moteur1_direction = 1;
        moteur2_direction = 0;
        
        moteur1.write(0);
        moteur2.write(rc);
        
        wait(0.14);  // disance de l'arc  = pi/2 *R(=25cm , rayon de la roue)  
        moteur2.write(0); 
        }

/*
 *-------------------------------------------------------------------------------------------------------------------------------------------------------------
 *  TRANSMISSION:
 */

/// Transmission :
#include "mbed.h"
#include <string>

Serial carte2(PC_10, PC_11);
//Serial pc(USBTX, USBRX);
int j;

char commande1[]="A";
char commande2[]="D";
char commande3[]="R";
char commande4[]="G";

//float chaine_to_float(char chaine[] ) ;

int main() {

    carte2.baud(19200);
    while(1) {
        for(j=0;j<sizeof(commande3);j++){
        carte2.putc(commande3[j]);
        //pc.printf(commande1);
        wait(1);
        }
        //wait(0.2);
       }
      
 }


/// Reception :
#include "mbed.h"

Serial carte1(PC_10, PC_11);
Serial pc(USBTX, USBRX);

int main() {
    char data ;
    carte1.baud(19200);
   
    while(1) {
        if (carte1.readable())
        {
            data=carte1.getc();
            pc.putc(data);
            //pc.prrintf(reception);
            }
        }
}



/*
 *-------------------------------------------------------------------------------------------------------------------------------------------------------------
 *  ASSERVISSEMENT:
 */

float resolution=2*3.1415/12 ;
QEI encoder1(D10, D11, NC, 48, QEI::X4_ENCODING);
QEI encoder2(D0 , D1 , NC, 48, QEI::X4_ENCODING);

float v_ang1(){                                     // fonction qui calcule la vitesse angulaire du premier moteur
    int p0, p1 ;                                    
    float v ;                                     
    p0=encoder1.getPulses();                        // position de l'incrémant à t 
    wait_ms(10) ;
    p1 = encoder1.getPulses();                      // position de l'incrémant à t+dt avec  dt = 10 ms 
    v=(p1-p0)*resolution/(0.01*19.225) ;            // calcul de la vitesse angulaire 
    return(v); 
    } 
    
float v_ang2(){                                    // fonction qui calcule la vitesse angulaire du deuxieme moteur
    int p0, p1 ;
    float v ;
    p0=encoder2.getPulses();                       // position de l'incrémant à t 
    wait_ms(10) ;
    p1 = encoder2.getPulses();                     // position de l'incrémant à t+dt avec  dt = 10 ms
    v=(p1-p0)*resolution/(0.01*19.225) ;           // calcul de la vitesse angulaire
    return(v); 
    } 

// La boucle d'asservissement en vitesse

void boucle_regul(void){
    float v1;
    float v2;
    float erreur=0.0;                                                   // erreur proportionnelle du moteur 1 initiale 
    float Ierreur=0.0;                                                  // erreur intégrale du moteur 1 initiale  ( erreur accumulée)
    float erreur2=0.0;                                                  // erreur proportionnelle du moteur 1 initiale   
    float Ierreur2=0.0;                                                 // erreur intégrale du moteur 2 initiale  ( erreur accumulée)
 
    float Kp = 1.0/19.225;
    float KiTe = 0.0001;
   
    Ierreur = Ierreur + erreur;                                         // calcul de l'erreur accumulée du moteur 1 
    Ierreur2= Ierreur2+ erreur2;                                        // calcul de l'erreur accumulée du moteur 2
    
    v1=v_ang1();                                                        // vitesse angulaire réelle du moteur 1 
    v2=v_ang2();                                                        // vitesse angulaire réelle du moteur 2
    
    erreur = consigne - v1;                                             // erreur du moteur 1 
    erreur2= consigne - v2;                                             // erreur du moteur 2
    
    moteur_1(Kp* erreur + KiTe *Ierreur );                              // la nouvelle commande u(t) appliquée au moteur 1  
    moteur_2(Kp* erreur2 + KiTe *Ierreur2 );                            // la nouvelle commande u(t) appliquée au moteur 2  
    
    //pc.printf("c1 => %lf \r\n",consigne);                             // affichage de la consigne 
    //pc.printf("v1 => %lf \r\n",v1);                                   // affichage de vitesse angulaire actuelle du moteur 1
    //pc.printf("rc1 => %lf \r\n\n",Kp * erreur + KiTe *Ierreur);       // affichage de la commande appliquée au moteur 1 
    
    }


/*
 *-------------------------------------------------------------------------------------------------------------------------------------------------------------
 *  DEMONSTRATION:
 */


// fonction qui traduit la lettre reçuie en une commande à applioquer au robot 
void marche( char direction, float rc ){
    if (direction=='F'){                        // F = forward 
        marche_avant(rc);}
        
    if (direction=='B'){                        // B = backward 
        marche_avant(rc);}
        
    if (direction=='R'){                        // R = right
        turn_R(rc);}
        
    if (direction=='L'){                        // L = left 
        turn_L(rc);}
    }

    
int main() {
    double rc;
    char data ;
    
    rc =0.35;
    carte1.baud(19200);
     
    moteur1.period_ms(10);       
    moteur2.period_ms(10);
    
    
    while(1) {
        if (carte1.readable())
        {
            data=carte1.getc();    // récupération de la lettre de commande 
            marche(data, rc ) ;    // commande associée à la lettre reçue
            pc.putc(data);         // affiche sur tera term la lettre reçue
           
            }
        }
     
    
    }