ER2

Dependencies:   mbed SRF05

Files at this revision

API Documentation at this revision

Comitter:
dorian06
Date:
Wed Jun 02 07:21:07 2021 +0000
Parent:
1:06c976cc11fb
Commit message:
N1N0

Changed in this revision

SRF05.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 06c976cc11fb -r 354d20277612 SRF05.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SRF05.lib	Wed Jun 02 07:21:07 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/simon/code/SRF05/#e758665e072c
diff -r 06c976cc11fb -r 354d20277612 main.cpp
--- 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