Revision 1:ba1ca7a99573, committed 2020-07-17

Comitter:

evgeniik

Date:

Fri Jul 17 12:45:32 2020 +0000

Parent:

0:dec1366627ac

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 12:45:32 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/Motor.lib	Fri Jul 17 12:45:32 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/AIMergence/code/MotorDC/#f265e441bcd9

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

--- a/main.cpp	Fri Jul 17 12:42:17 2020 +0000
+++ b/main.cpp	Fri Jul 17 12:45:32 2020 +0000
@@ -1,5 +1,224 @@
 #include "mbed.h"
+#include "VL53L0X.h"
+#include "Multi_16.h"
+#include "Motor.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 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);
     
-}
\ No newline at end of file
+    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];
+        float v1 = (c + d + (1000-g))/3000;
+        float v2 = (c + g + (1000-d))/3000;
+        
+        //if (v1 < 0.3) {v1 = 0;}
+        //if (v2 < 0.3) {v2 = 0;}
+        //if (v1 > 0.4) {v1 = 1;} @corentin
+        //if (v2 > 0.4) {v2 = 1;}
+       
+        m1.speed(v1);
+        m2.speed(v2);
+        
+        // End
+        
+        
+        ///////////////0/////////1//////////////
+        // Start pour Sylvain
+        /*bool c = seuil(mt.dist[1], 500);
+        bool g = seuil(mt.dist[3], 500);
+        bool d = seuil(mt.dist[6], 500);
+        
+        bool v1 = !d + g*!c;
+        bool v2 = !g*!c + !g*d + !c*d;
+        
+        printf("\ v1 = %d \t v2 = %d \n", (int)v1, (int)v2);
+        m1.speed((int)v1);
+        m2.speed((int)v2);*/
+        
+        // End
+    } // end while
+    }
+ 
+}
+