Diff: main.cpp

Revision:

1:47b9204516a0

Parent:

0:c085b14ea8df

--- a/main.cpp	Tue Jan 05 15:18:53 2016 +0000
+++ b/main.cpp	Wed Jan 06 15:20:09 2016 +0000
@@ -1,57 +1,144 @@
 #include "mbed.h"
 #include "HCSR04.h"
 
-#define ECHO_1  PA_9
-#define TRIG_1  PC_7
-#define MAXSIZE 10
+#define ECHO_FRONT  D8
+#define TRIG_FRONT  D9
 
-DigitalIn bp(PUSH);
-Serial pc(USBTX, USBRX);    //UART
-HCSR04 sensor1(TRIG_1, ECHO_1);
+#define ECHO_BACK  D10
+#define TRIG_BACK  D11
+
 
 
-int i;
-int distance[10]
+Serial pc(USBTX, USBRX);    //UART
+HCSR04 sensorFront(TRIG_FRONT, ECHO_FRONT);
+HCSR04 sensorBack(TRIG_BACK, ECHO_BACK);
+
+PwmOut PWM_R(PB_3);
+PwmOut PWM_L(PB_10);
+
+DigitalOut B_R(PB_5);
+DigitalOut F_R(PA_10);
+
+DigitalOut B_L(PA_8);
+DigitalOut F_L(PB_4);
 
+/*
+* rotation infinitesimale vers la gauche, de puissance 0<P<1
+*/
+void rotationG(float P){
+    PWM_R = P;
+    B_R = 0;
+    F_R = 1;
+    PWM_L = P;
+    B_L = 0;
+    F_L = 0;
+    wait(0.05);
+}
 
 /*
-* fifo de taille size_ contenant des distances de type double, et accessible par les méthodes suivantes :
-*   -push : ajoute une donnée en entrée de la fifo
-*   -getn : retourne la distance de profondeur n.
+* rotation infinitesimale vers la droite, de puissance 0<P<1
 */
-class fifo {
-    public:
-        void fifo(int n){
-            size_=n;
-        }
-        void push(int value){
-            for(i=size_-1;i>0;i--){
-                distance_[i]=distance_[i-1];
-            }
-            distance_[0]=value;
-        }
-        double getn(int n){
-            return distance_[n];
-        }
-    private :
-    double distance_[MAXSIZE];
-    int size_;
+void rotationD(float P){
+    PWM_R = P;
+    B_R = 0;
+    F_R = 0;
+    PWM_L = P;
+    B_L = 0;
+    F_L = 1;
+    wait(0.05);
+}
+
+
+void virageGauche(int angle){}
+
+//analyse des valeurs enregistrées --> angle de mur --> procédure de virage d'un angle donnée+ avancer 2cm --> mesure de distance --> correction angle --> avance
+
+//Avancer tout droit 
+void avancer(float P){
+    PWM_R = P;
+    B_R = 0;
+    F_R = 1;
+    PWM_L = P;
+    B_L = 0;
+    F_L = 1;
+    wait(0.05);
 }
 
-class PIDController{
-    public:
-        PIDController(double Kp, double Ki, double Kd){
-            
-        }
-    private:
-        
-}
 
 int main() {
-    fifo fifoDist;
+    
+    int distFront = 22;
+    int distBack = 22;
+    int difDist;
+    
+    B_R = 0;
+    F_R = 0;
+    B_L = 0;
+    F_L = 0;
+    
+    PWM_R.period_ms(20);
+    PWM_L.period_ms(20);
+
     while(1) {
-        //On mesure la distance et on l'ajoute à la fifo
-        fifoDist.push(sensor1.distance(1));
-        wait(0.2);
+
+        distFront = sensorFront.distance(1);
+        distBack = sensorBack.distance(1);
+        
+        difDist = distBack - distFront;
+        
+        if(difDist<-10){
+            rotationG(0.8); //rotation à gauche
+        }else if(difDist<0){
+            rotationG(0.8); //rotation à gauche
+            avancer(0.7);
+        }else if(difDist>0){
+            rotationD(0.8);    //rotation à droite
+            avancer(0.7);
+        }else if(difDist>10){
+            rotationG(0.8);
+        }
+
     }
 }
+    
+/* CODE POUR 1 CAPTEUR ------- 
+int main() {
+
+    int dist[3] = {0,0,0};
+    
+    int distance = 0;
+    
+    B_R = 0;
+    F_R = 0;
+    B_L = 0;
+    F_L = 0;
+    
+    PWM_R.period_ms(20);
+    PWM_L.period_ms(20);
+
+    while(1) {
+
+        dist[2] = dist[1];
+        dist[1] = dist[0];
+        dist[0] = sensor.distance(1);
+        
+        if(dist[0] > 40) dist[0] = 40;
+        //distance = (dist[0] + dist[1] + dist[2])/3.0;
+        //pc.printf("%d\n",dist);
+        distance = dist[0];
+
+        if( distance < 20 ){
+            rotationD(0.5);    //rotation à droite
+        }else if(distance > 30 ){
+            rotationG(0.7); //rotation à gauche
+            wait(0.05);
+            avancer(0.6);
+        }else if(distance > 25 ){
+            rotationG(0.5); //rotation à gauche
+        }else{
+                avancer(0.7);    //rotation à gauche
+        }
+        wait(0.05); //latence
+    }
+}
+*/
\ No newline at end of file