Revision 8:24a3fa0f912a, committed 2017-06-07

Comitter:

vermaelen

Date:

Wed Jun 07 16:14:27 2017 +0000

Parent:

6:5f7df5c74a77

Commit message:

V1

Changed in this revision

diff -r 5f7df5c74a77 -r 24a3fa0f912a fct.cpp
--- a/fct.cpp	Wed May 31 08:48:23 2017 +0000
+++ b/fct.cpp	Wed Jun 07 16:14:27 2017 +0000
@@ -1,177 +1,2 @@
 #include "mbed.h"
 #include "fct.h"
-void contournement()
-{
-    sensMG.write(0);
-    sensMD.write(0);
-    cmdD=35;
-    cmdG=15;
-    MD.pulsewidth(vitesse(cmdD));
-    MG.pulsewidth(vitesse(cmdG));
-
-}
-void stopMotor()
-{
-    sensMG.write(0);
-    sensMD.write(0);
-    MG.pulsewidth(vitesse(0));
-    MD.pulsewidth(vitesse(0));
-}
-void suivi_mur()
-{
-    E3=E2;
-    E2=E1;
-    E1=E0;
-    E0=US2-US1;
-    if((E0+E1)>0) {
-        cmdD=VMOY;
-        cmdG=VMOY-Kp_ecart*(E0+E1)-Kp_dist*(US2-45);
-    } else {
-        cmdD=VMOY+Kp_ecart*(E0+E1)+Kp_dist*(US2-45);
-        cmdG=VMOY;
-    }
-
-    MD.pulsewidth(vitesse(cmdD));
-    MG.pulsewidth(vitesse(cmdG));
-
-    wait(0.001);
-}
-void rotation_horaire()
-{
-    sensMG.write(0);
-    sensMD.write(1);
-    MG.pulsewidth(vitesse(30));
-    MD.pulsewidth(vitesse(30));
-}
-void init()
-{
-    sensMG.write(0);
-    sensMD.write(0);
-    MG.period(PERIOD);
-    MD.period(PERIOD);
-    MG.pulsewidth(vitesse(0));
-    MD.pulsewidth(vitesse(0));
-    tic1.attach(&fcttrig,0.035);
-    tic2.attach(&mesAN,0.01);
-    echo.rise(&start);
-    echo.fall(&stop);
-}
-void mesAN()
-{
-    if(flag4==0) {
-        AN1_av=AN1;
-    }
-    AN1=0.82*a/(3.3*AnaG.read()-b);
-    if(((AN1-AN1_av)>40)||((AN1-AN1_av)<-40)) {
-        float temp=AN1;
-        AN1=AN1_av;
-        AN1_av=temp;
-        flag4=1;
-    } else {
-        flag4=0;
-    }
-    if(AN1<0||AN1>150)AN1=150;
-
-
-    if(flag5==0) {
-        AN2_av=AN2;
-    }
-    AN2=0.82*a/(3.3*AnaAV.read()-b);
-
-    if(((AN2-AN2_av)>40)||((AN2-AN2_av)<-40)) {
-        float temp=AN2;
-        AN2=AN2_av;
-        AN2_av=temp;
-        flag5=1;
-    } else {
-        flag5=0;
-    }
-    if(AN2<0||AN2>150)AN2=150;
-}
-
-void fcttrig()
-{
-    switch(drap) {
-        case 1 :
-            trigger2.write(1);
-            wait_us(10);
-            trigger2.write(0);
-            drap=2;
-            break;
-        case 2 :
-            trigger3.write(1);
-            wait_us(10);
-            trigger3.write(0);
-            drap=3;
-            break;
-        case 3 :
-            trigger1.write(1);
-            wait_us(10);
-            trigger1.write(0);
-            drap=1;
-            break;
-    }
-
-}
-void start()
-{
-    temp.reset();
-    temp.start();
-}
-void stop()
-{
-    temp.stop();
-    switch(drap) {
-        case 1 :
-            if(flag3==0) {
-                US3_av=US3;
-            }
-            US3=temp.read_us()/58.31;
-            if(((US3-US3_av)>50)||((US3-US3_av)<-50)) {
-                float temp=US3;
-                US3=US3_av;
-                US3_av=temp;
-                flag3=1;
-            } else {
-                flag3=0;
-            }
-            if(US3<0||US3>150)US3=150;
-            break;
-        case 2 :
-            if(flag2==0) {
-                US2_av=US2;
-            }
-            US2=temp.read_us()/58.31;
-            if(((US2-US2_av)>50)||((US2-US2_av)<-50)) {
-                float temp=US2;
-                US2=US2_av;
-                US2_av=temp;
-                flag2=1;
-            } else {
-                flag2=0;
-            }
-            if(US2<0||US2>150)US2=150;
-            break;
-        case 3 :
-            if(flag1==0) {
-                US1_av=US1;
-            }
-            US1=temp.read_us()/58.31;
-            if(((US1-US1_av)>50)||((US1-US1_av)<-50)) {
-                float temp=US1;
-                US1=US1_av;
-                US1_av=temp;
-                flag1=1;
-            } else {
-                flag1=0;
-            }
-            if(US1<0||US1>150)US1=150;
-            break;
-    }
-}
-float vitesse(float vit)
-{
-    if(vit<0) vit=0;
-    if(vit>VMAX) vit=VMAX;
-    return ((vit/100.0)*PERIOD);
-}

diff -r 5f7df5c74a77 -r 24a3fa0f912a fct.h
--- a/fct.h	Wed May 31 08:48:23 2017 +0000
+++ b/fct.h	Wed Jun 07 16:14:27 2017 +0000
@@ -2,53 +2,43 @@
 #define FCT_H
 //GLOBALES
 extern BusOut leds;
-extern DigitalOut trigger1;
-extern DigitalOut trigger2;
-extern DigitalOut trigger3;
-extern InterruptIn echo;
-extern AnalogIn AnaG;
-extern AnalogIn AnaAV;
-extern PwmOut MG; //vitesse moteur gauche
-extern PwmOut MD; //vitesse moteur droit
-extern DigitalOut sensMG;  // sens moteur gauche
-extern DigitalOut sensMD;  // sens moteur droit
-extern Timer temp,t,t2;
-extern Ticker tic1,tic2;
+
+extern Serial CamPixy;
+
+extern DigitalOut trig1;//US1
+extern InterruptIn echo1;
+extern DigitalOut trig2;//US2
+extern InterruptIn echo2;
+extern DigitalOut trig3;//US3
+extern InterruptIn echo3;
+
+extern I2C monI2C;
+extern PwmOut Servo;
 
-extern int drap,flag1,flag2,flag3,flag4,flag5;
-extern float US1,US2,US3,AN1,AN2,US1_av,US2_av,US3_av,AN1_av,AN2_av; 
-extern float E_av,E,iE,E0,E1,E2,E3;
-extern float cmdG,cmdD;
-extern int etat;
-extern float iecart,ecart_av,ecart;
+extern AnalogIn SD_1; // capteur de distance courte droite
+extern AnalogIn SD_2; // capteur de distance courte gauche
+extern AnalogIn LD_1; // capteur de distance longue droite
+extern AnalogIn LD_2; // capteur de distance longue gauche
+
+extern InterruptIn I_D;
+extern InterruptIn I_G;
+
+extern SPI spi;
+
+extern AnalogIn C1;// capteur de ligne blanche 1
+extern AnalogIn C3;// capteur de ligne blanche 3 
+//-- le capteur de ligne 2 est sur un MCP3201(spi) dont le CS est sur p13
+extern DigitalOut CS_C2;
+
+extern PwmOut vitD;
+extern PwmOut vitG;
+
+//CONSTANTES
+#define ADR_PCF 0x70
+#define PERIOD 0.0001
 
 
-//CONSTANTES 
-#define PERIOD 0.0001
-#define VMOY 45
-#define VMAX 70
-#define Kp_dist 0.2
-#define Kp_ecart 0.22
-#define Td_ecart 0.0
-#define Ti_ecart 1000.0
-#define limitmin 3
-#define limitmax 150
-#define Te 0.001
-#define Ti 2.0
-#define a 24.0
-#define b 0.1
 //PROTOTYPES
-void contournement();
-void stopMotor();
-void suivi_mur();
-void rotation_horaire();
-void init();
-void asservissement();
-void fcttrig();
-float vitesse(float);
-void start();
-void stop();
-float vitesse(float);
-void mesAN();
+
 
 #endif

diff -r 5f7df5c74a77 -r 24a3fa0f912a globals.cpp
--- a/globals.cpp	Wed May 31 08:48:23 2017 +0000
+++ b/globals.cpp	Wed Jun 07 16:14:27 2017 +0000
@@ -1,22 +1,32 @@
 #include "mbed.h"
 BusOut leds(LED1,LED2,LED3,LED4);
-DigitalOut trigger1(p14);
-DigitalOut trigger2(p16);
-DigitalOut trigger3(p18);
-InterruptIn echo(p11);
-AnalogIn AnaG(p17);
-AnalogIn AnaAV(p15);
-PwmOut MD(p21); //vitesse moteur gauche
-PwmOut MG(p24); //vitesse moteur droit
-DigitalOut sensMG(p23);  // sens moteur gauche
-DigitalOut sensMD(p26);  // sens moteur droit
-Timer temp,t,t2;
-Ticker tic1,tic2;
+
+Serial CamPixy(p28,p27);
+
+DigitalOut trig1(p11);//US1
+InterruptIn echo1(p12);
+DigitalOut trig2(p8);//US2
+InterruptIn echo2(p24);
+DigitalOut trig3(p26);//US3
+InterruptIn echo3(p23);
+
+I2C monI2C(p9,p10);
+PwmOut Servo(p21);
 
-int drap=1,flag1=0,flag2=0,flag3=0,flag4=0,flag5=0;
-float US1,US2,US3,AN1,AN2,US1_av=50,US2_av=50,US3_av=50,AN1_av=50,AN2_av=50;
-float E_av,E,iE=0,E0=0,E1=0,E2=0,E3=0;
-float cmdG=0,cmdD=0;
-int etat=0;
-float iecart=0,ecart_av,ecart;
+AnalogIn SD_1(p19); // capteur de distance courte droite
+AnalogIn SD_2(p20); // capteur de distance courte gauche
+AnalogIn LD_1(p17); // capteur de distance longue droite
+AnalogIn LD_2(p18); // capteur de distance longue gauche
+
+InterruptIn I_D(p30);
+InterruptIn I_G(p29);
 
+SPI spi(p5,p6,p7);
+
+AnalogIn C1(p15);// capteur de ligne blanche 1
+AnalogIn C3(p16);// capteur de ligne blanche 3 
+//-- le capteur de ligne 2 est sur un MCP3201(spi) dont le CS est sur p13
+DigitalOut CS_C2(p13);
+
+PwmOut vitD(p25);
+PwmOut vitG(p22);

diff -r 5f7df5c74a77 -r 24a3fa0f912a main.cpp
--- a/main.cpp	Wed May 31 08:48:23 2017 +0000
+++ b/main.cpp	Wed Jun 07 16:14:27 2017 +0000
@@ -1,60 +1,29 @@
 #include "mbed.h"
 #include "fct.h"
-BusOut ledsetat(p12,p13);
-
+DigitalOut cs(p13);
+DigitalIn bp(p14);
 int main()
 {
-    int etat=0;
-    init();
+    char A=0x80;
+    char B=0x00;
+    monI2C.write(ADR_PCF,&A,1);
+    vitG.period(PERIOD);
+    vitD.period(PERIOD);
+    vitG.pulsewidth(PERIOD);
+    vitD.pulsewidth(PERIOD);
+    cs = 1;                //initialisation de CS à ‘1’
+    spi.format(16,0);
+    spi.frequency(1000000);
+    bp.mode(PullUp);
     while(1) {
-       // printf("etat=%d US1=%.0f US2=%.0f US3=%.0f erreur=%.0f AN1=%.0f AN2=%.0f cmdD=%.0f cmdG=%.0f\n\r",etat,US1,US2,US3,(US2-US1),AN1,AN2,cmdD,cmdG);
-        //wait(0.05);
-        ledsetat.write(etat);
-        switch(etat) {
-            case 0 :
-                if((AN2>10 && AN2<20) || US3<13) {
-                    etat=1;
-                    stopMotor();
-                }
-                if(US2>100 && US1<40) {
-                    etat=2;
-                    stopMotor();
-                }
-                break;
-            case 1 :
-                if(US2>100 && US1<40) {
-                    etat=2;
-                    stopMotor();
-                }
-                if(AN2>30) {
-                    etat=0;
-                    t2.start();
-                    t2.reset();
-                    stopMotor();
-                }
-
-                break;
-            case 2 :
-                if((AN2>10 && AN2<20) || US3<13) {
-                    etat=1;
-                    stopMotor();
-                }
-                if(t2.read()>2.5) {
-                    etat=0;
-                }
-                break;
-        }
-        switch(etat) {
-            case 0 :
-                suivi_mur();
-                break;
-            case 1 :
-                rotation_horaire();
-                break;
-            case 2 :
-                contournement();
-                break;
-        }
-
+        leds.write(bp.read()+1);
+        //cs=0;
+        monI2C.write(ADR_PCF,&B,1);
+        unsigned int valeur = spi.write(0x00);
+        valeur = (valeur>>1)&0x0FFF;
+        printf("valeur lue = %d\n\r", valeur);
+        //cs=1;
+        monI2C.write(ADR_PCF,&A,1);
+        wait(0.05);
     }
 }