Diff: main.cpp

Revision:

2:354d20277612

Parent:

1:06c976cc11fb

--- a/main.cpp	Sun Jan 24 18:46:56 2021 +0000
+++ b/main.cpp	Wed Jun 02 07:21:07 2021 +0000
@@ -1,15 +1,171 @@
 #include "mbed.h"
-                //Déclaration des variables globales
-DigitalOut myled1(LED1) ;            
+#include "SRF05.h"
+#define PI 3.14159265358979323846264338327950288419716939937510582
+#define periode 20
+#define Temps 1.0 //temps sur le quel on mesure le nombre d'encodeur passé
+#define rayon 0.021
+#define crans 12
+#define voulu 25.0
+SRF05 srf(p18, p17);
+//Déclaration des variables globales
+DigitalOut myled1(LED1) ;
+DigitalOut myled2(LED2) ;
+DigitalOut myled3(LED3) ;
+DigitalOut myled4(LED4) ;
+DigitalIn Bp1(p30); //gauche
+DigitalIn Bp2(p29);    //droit
+AnalogIn capt1(p20);    //droit
+AnalogIn capt2(p19);    //gauche
+InterruptIn Encodeur_AG(p10);
+InterruptIn Encodeur_AD(p9);
+InterruptIn Encodeur_BG(p7);
+InterruptIn Encodeur_BD(p6);
+PwmOut mot1(p23);
+PwmOut mot2(p22);
+DigitalOut sens_droit(p11);
+DigitalOut sens_gauche(p12);
+AnalogIn Vbat(p15);
+float dist;
+int sens=1;
+int cpt=0;
+int vitesse1=30;
+int vitesse2;
+int mot;
+int ticker;
+float vitessemsAG;  //vitesse en m/s en fonction des encodeurs
+float vitessemsAD;
+float vitesseradsAG; // vitesse en ras/s en foction des encodeurs
+float vitesseradsAD;
+int cpt1=0,cpt2=0; // compteur pour la mesure de la vitessse
+Ticker T1;                        // ticker pour la mesure de la vitesse
+float Bat;
+void sens_vitesse(int,int);   // gestin du sens et de la vitesse et du moteur 1 periode entre [-100;100] puis le moteur 1=droit 2=gauche
+void gestion_cpt_AG(void);    //mesure de la vitesse sue les different encodeurs
+void gestion_cpt_AD(void);    //"                                              "
+void calculvitesse(void);
+void position(float); // distance voulu
+int main()      //Début du main
+{
+    Encodeur_AG.mode(PullNone);         // pas de resistance interne
+    Encodeur_AD.mode(PullNone);
+    Encodeur_AG.rise(&gestion_cpt_AG); // front montant
+    Encodeur_AD.rise(&gestion_cpt_AD);
+    Bp1.mode(PullUp);         //Déclaration des variables locales au main
+    Bp2.mode(PullUp);
+    mot1.period_us(100);    //periode des moteurs
+    mot2.period_us(100);
+    T1.attach(&calculvitesse, Temps);
 
 
-int main() {    //Début du main
-
-                //Déclaration des variables locales au main
+    while(1) {  //Boucle infinie
+        int BpG;
+        /*myled1=1;
+        myled2=0;
+        myled3=0;
+        myled4=0;
+        wait(0.5);
+        myled1=0;
+        myled2=1;
+        myled3=0;
+        myled4=0;
+        wait(0.5);
+        myled1=0;
+        myled2=0;
+        myled3=1;
+        myled4=0;
+        wait(0.5);
+        myled1=0;
+        myled2=0;
+        myled3=0;
+        myled4=1;
+        wait(0.5);*/
+        BpG=Bp1.read();
+        Bat=(Vbat.read()*100)/7.4; //valeur de la batterie
+        /*printf("batt:%g \t",Bat);
+        printf("Bg: %d \t Bd: %d \t",Bp1.read(),Bp2.read());
+        printf("Cd: %.02f \t Cg:%.02f \t",capt1.read(),capt2.read());
+        printf("EAg:%d \t EAd:%d \t",Encodeur_AG.read(),Encodeur_AD.read());
+        printf("EBg:%d \t EBd:%d \n\r",Encodeur_BG.read(),Encodeur_BD.read());
+        wait(0.5);*/
+        if (ticker==1) {
+            //printf("vitesse mot Gauche:%g\n\r",vitessemsAG);
+            //printf("vitesse mot Droite:%g\n\r",vitessemsAD);
+            ticker=0;
+        }
+        dist= srf.read();
+    if(dist>voulu) {
+        sens_vitesse(30,1);
+        sens_vitesse(30,2);
+    } else if(dist< voulu) {
+        sens_vitesse(-30,1);
+        sens_vitesse(-30,2);
+    } else if((dist<23) && (dist>27)) {
+        sens_vitesse(0,1);
+        sens_vitesse(0,2);
+    }
+    }
+    
+    
+}
 
-    while(1) {  //Boucle infinie
+void sens_vitesse(int Periode,int mot)
+{
+    int sens;
+    if (Periode<=0) {       // determination du sens 1 si peride inf a 0
+        sens=1;
+        Periode=abs(Periode); // valeurs abs de la periode donc en positif
+    } else {
+        sens=0;
+    }
+    if((mot==1)&&(sens==1)) {
+        sens_gauche.write(1);
+        mot1.pulsewidth_us(100-Periode); // 100-periode car sinon vitesse est de la difference entre 100 et la periode
 
-        printf("Afficher ici votre message\n\r");
-        
+    } else if((mot==1)&& (sens==0)) {
+        sens_gauche.write(0);
+        mot1.pulsewidth_us(Periode);
+    }
+
+    if((mot==2)&&(sens==1)) {
+        sens_droit.write(1);
+        mot2.pulsewidth_us(100-Periode); // 100-periode car sinon vitesse est de la difference entre 100 et la periode
+    } else if((mot==2)&& (sens==0)) {
+        sens_droit.write(0);
+        mot2.pulsewidth_us(Periode);
     }
 }
+
+void gestion_cpt_AG(void)
+{
+    cpt1++;   // incrementation de +1 a chaque passage
+}
+void gestion_cpt_AD(void)
+{
+    cpt2++;   // incrementation de +1 a chaque passage
+}
+
+void calculvitesse(void)
+{
+    vitessemsAG = (cpt1*((2*PI*rayon)/crans))/Temps; //(compteur * ((perimetre)/ nombre de crans))/Temps
+    vitesseradsAG = vitessemsAG/rayon; // m/s div par rayon donc = rad/s
+    vitessemsAD = (cpt2*((2*PI*rayon)/crans))/Temps; //(compteur * ((perimetre)/ nombre de crans))/Temps
+    vitesseradsAD = vitessemsAD/rayon; // m/s div par rayon donc = rad/s
+    ticker=1;
+    cpt1=0;                     // remise du compteur a 0
+    cpt2=0;                     // remise du compteur a 0
+}
+
+/*void position(float voulu)
+{
+    dist= srf.read();
+    if(dist>voulu) {
+        sens_vitesse(30,1);
+        sens_vitesse(30,2);
+    } else if(dist< voulu) {
+        sens_vitesse(-30,1);
+        sens_vitesse(-30,2);
+    } else if((dist<23) && (dist>27)) {
+        sens_vitesse(0,1);
+        sens_vitesse(0,2);
+    }
+}*/
\ No newline at end of file