Revision 2:f3ca81bbfabf, committed 2020-07-17

Comitter:

sylvaingauthier

Date:

Fri Jul 17 14:04:04 2020 +0000

Parent:

1:53b992a47b28

Commit message:

test

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/16_Channel_Analog.lib	Fri Jul 17 14:04:04 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/AIMergence/code/16_Channel_Analog/#72ebae234e48

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VL53L0X.lib	Fri Jul 17 14:04:04 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/AIMergence/code/VL53L0X/#ab04ed7aa3ec

--- a/main.cpp	Fri Jul 17 10:08:05 2020 +0000
+++ b/main.cpp	Fri Jul 17 14:04:04 2020 +0000
@@ -1,38 +1,223 @@
-/* mbed Microcontroller Library
- * Copyright (c) 2019 ARM Limited
- * SPDX-License-Identifier: Apache-2.0
- */
-
-#include "mbed.h"
-#include "perceptron.h"
-#include "Motor.h"
-#include <vector>
-#include <iostream>
-#include <stdio.h>
-
-int main()
-{
-    Motor m1(D8, D9, D10); // pwm, fwd, rev
-    Motor m2(D3, D4, D5); // pwm, fwd, rev
-    
-    printf("debut\r\n");
-    vector<float> Wg = {1,1,-1,-1};
-    perceptron perceptronG(Wg);
-    
-    vector<float> Wd = {1,-1,-1,1};
-    perceptron perceptronD(Wd);
-    while(true){
-        
-        vector<int> X = {1,1,1};
-        
-        printf("X=%d,%d,%d\r\n",X[0],X[1],X[2]);
-        
-        int Vg=perceptronG.predict(X);
-        int Vd=perceptronD.predict(X);
-        
-        printf("Vg=%d\r\n",Vg);
-        printf("Vd=%d\r\n",Vd);
-        m1.speed(Vg);
-        m2.speed(Vd);
-    }
-}
+#include "mbed.h"
+#include "VL53L0X.h"
+#include "Multi_16.h"
+#include "Motor.h"
+#include "perceptron.h"
+
+#include <vector>
+#include <iostream>
+#include <stdio.h>
+
+Motor m1(A2, A3, A5); // pwm, fwd, rev
+Motor m2(A1, D13, A0); // pwm, fwd, rev
+
+#define range_addr  (0x56)
+#define mux_range 7
+
+#define range1_XSHUT   D2
+#define VL53L0_I2C_SDA   PA_10
+#define VL53L0_I2C_SCL   PA_9
+Serial pc(SERIAL_TX, SERIAL_RX);
+
+Serial blut(PB_6, PB_7);//Tx, Rx
+
+static DevI2C devI2c(VL53L0_I2C_SDA, VL53L0_I2C_SCL); 
+static DigitalOut shutdown_pin(range1_XSHUT);
+static VL53L0X tof(&devI2c, &shutdown_pin);
+
+typedef struct Tofs {
+   int      nmb;
+   int      n_tof_found;
+   bool     one;
+   bool     presence[7];
+   uint32_t address[7];
+   uint32_t dist[7];
+   uint32_t correction[7];
+} MultiTof;
+
+MultiTof mt;
+
+void verif_tof(void);
+Ticker time_up;
+ 
+
+
+// Capteurs TOF /*Contruct the sensors*/ 
+//static DigitalOut shutdown_pin(range1_XSHUT);
+//static VL53L0X tof(&devI2c, &shutdown_pin);
+
+
+
+// Multiplexeur
+static Mux mux(D9,D10,D11,D12,D3); // pour L432KC
+//static Mux mux(D2,D3,D4,D5,D6);
+
+int init_tof(int i){
+    int status = 0;
+    mux.set_channel(i);
+    //tof.VL53L0X_off();
+
+    tof.default_addr();
+    status = tof.init_sensor(range_addr);
+    if (status == VL53L0X_ERROR_NONE){
+        mt.one = true;
+        mt.presence[i] = true;
+        //tof.VL53L0X_off();
+        //tof.VL53L0X_on();
+        //mt.address[i]  = 0x52;
+        mt.address[i]  = range_addr;
+        return 0;                       // init ok (address was default = 0x52 and now address = 0x56)
+    }
+    else{
+        tof.set_addr_bib(range_addr);
+        if (status == VL53L0X_ERROR_NONE){
+            mt.one = true;
+            mt.presence[i] = true;
+            //tof.VL53L0X_off();
+            //tof.VL53L0X_on();
+            //mt.address[i]  = 0x52;
+            mt.address[i]  = range_addr;
+            return 0;                  // init ok (address was 0x56)
+        }
+        else return -1;                 // not detected
+    }
+}
+
+int init_tofs() {    
+    int etat = 0;
+    tof.VL53L0X_off();
+    
+    
+    for (int i = 0; i<mt.nmb; i++){
+        etat = init_tof(i);
+        if (etat == 0){
+            mt.n_tof_found++;
+            if (i<9) printf("+\t");
+            else printf("+\t");
+        }
+        else{
+            if (i<9) printf("-\t");
+            else printf("-\t");
+        }
+    }
+    
+    printf("\n");
+    if (mt.one) { printf("At least 1 is found. Nmb : %d\n", mt.n_tof_found);tof.VL53L0X_off();tof.VL53L0X_on(); return 0; }
+    else return -1;
+}
+
+
+bool seuil(float val, float s){
+    return val < s;
+}
+
+ 
+int main()
+{
+    int status;
+    uint32_t mesure;
+    // Initialisation de MultiTof
+    mt.correction[0] = 1.11;
+    mt.correction[1] = 1.08;
+    mt.correction[2] = 1;
+    mt.correction[3] = 1.05;
+    mt.correction[4] = 1;
+    mt.correction[5] = 1.18;
+    mt.correction[6] = 1;
+    
+    mt.nmb = mux_range;
+    mt.n_tof_found  = 0;
+    mt.one = false;
+    for (int i=0; i<mt.nmb; i++){
+        mt.presence[i] = false;      // presence du capteur
+        mt.address[i]  = 0x52;       // address (default = 0x52)
+        mt.dist[i]     = 9999;       // distance
+    }
+    
+    status = init_tofs();
+    
+    for(int i=0; i<mt.nmb; i++) { printf("%x\t", mt.address[i]); }
+    printf("\n");
+    
+    printf("status = %d \n", status);
+    
+    //time_up.attach(&verif_tof, 20);
+    
+    vector<float> Wg = {1,1,-1,-1};
+    perceptron perceptronG(Wg);
+    
+    vector<float> Wd = {1,-1,-1,1};
+    perceptron perceptronD(Wd);
+    
+    float seuil = 500;
+    
+    if (!status){
+    while(1){
+        Timer t;
+        t.start();
+        for(int i=0; i<mux_range; i++){
+            if (i == 1 || i == 3 || i == 6){
+            mux.set_channel(i);
+            if (mt.presence[i]){
+                if (mt.address[i] == range_addr){
+                    tof.set_addr_bib(mt.address[i]);
+                    status = tof.get_distance(&mesure);
+                    if (status == VL53L0X_ERROR_NONE) { mt.dist[i] = mesure*mt.correction[i]; printf("%d\t", mt.dist[i]); }
+                    else {
+                        tof.default_addr();
+                        status = tof.get_distance(&mesure);
+                        if (status == VL53L0X_ERROR_NONE){ mt.dist[i] = mesure*mt.correction[i]; mt.presence[i] = true; mt.address[i] = 0x52; printf("%d\t", mt.dist[i]); }
+                        else if (status == -6) { mt.dist[i]+=200; printf("%d*\t", mt.dist[i]); } //printf("OoR\t");
+                        else { printf("--\t"); mt.presence[i] = false; }
+                    }
+                }
+                else{
+                    tof.default_addr();
+                    status = tof.get_distance(&mesure);
+                    if (status == VL53L0X_ERROR_NONE){ mt.dist[i] = mesure*mt.correction[i]; mt.presence[i] = true; mt.address[i] = 0x52; printf("%d\t", mt.dist[i]); }
+                    else if (status == -6) { mt.dist[i]+=200; printf("%d*\t", mt.dist[i]); }//printf("OoR\t");
+                    else { printf("--\t"); mt.presence[i] = false; }
+                }
+            }
+            else{
+                printf("--\t");
+                tof.default_addr();
+                status = init_tof(i);//tof.init_sensor(range_addr);
+            }
+            }
+        }
+        mt.one = false;
+        for(int i=0; i<mt.nmb; i++){ if (mt.presence[i] == true) {mt.one = true; break;} }
+        t.stop(); printf("%f \t one = %d \n", t.read(), mt.one);
+        for(int i=0; i<mux_range; i++){
+            if (mt.dist[i] > 1000) mt.dist[i] = 1000;
+        }
+        //wait(0.1);
+        //blut.printf("TOF/|%d|%d|%d|%d|%d|%d|%d/TOF", 200, mt.dist[1], mt.dist[2], mt.dist[3], mt.dist[4], mt.dist[5], mt.dist[6]);
+        blut.printf("t/|%d||%d||%d||%d||%d||%d||%d|*/t", mt.dist[0], mt.dist[1], mt.dist[2], mt.dist[3], mt.dist[4], mt.dist[5], mt.dist[6]);
+        
+        
+        /////////////lineaire//////////////////////
+        // Start pour Sylvain
+        float c = mt.dist[1];
+        float g = mt.dist[3];
+        float d = mt.dist[6];
+        
+        vector<int> X = {g<seuil,c<seuil,d<seuil};
+        
+        printf("X=%d,%d,%d\r\n",X[0],X[1],X[2]);
+        
+        int v1=perceptronG.predict(X);
+        int v2=perceptronD.predict(X);
+        
+        if (v1 ==0){v1=-1;}
+        if (v2 ==0){v2=-1;}
+        
+        m1.speed(v1);
+        m2.speed(v2);
+        
+    } // end while
+    }
+ 
+}
+