Programme de controle du robot joueur

Dependencies:   m3piBluetooth mbed

Fork of Serial_HelloWorld_Mbed by mbed official

Files at this revision

API Documentation at this revision

Comitter:
Maximousse
Date:
Thu May 03 13:00:03 2018 +0000
Parent:
0:879aa9d0247b
Commit message:
BT_Control_commit

Changed in this revision

m3pi.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 879aa9d0247b -r 6d8c50dcae28 m3pi.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/m3pi.lib	Thu May 03 13:00:03 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/Polybot1/code/m3piBluetooth/#b2df3579590b
diff -r 879aa9d0247b -r 6d8c50dcae28 main.cpp
--- a/main.cpp	Tue Feb 12 17:39:05 2013 +0000
+++ b/main.cpp	Thu May 03 13:00:03 2018 +0000
@@ -1,10 +1,153 @@
+
+
+
+//-------------------------BT CONTROL
 #include "mbed.h"
- 
-Serial pc(USBTX, USBRX); // tx, rx
- 
+#include "m3pi.h"
+//#include "MSCFileSystem.h"
+
+Serial pc(USBTX, USBRX); //tx, rx
+Serial bt(p28,p27); //bluetooth
+
+
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+DigitalOut leds [4] = {LED1, LED2, LED3, LED4};
+
+m3pi m3pi;
+
+//MSCFileSystem fs("fs");
+
+BusOut myleds(LED1, LED2, LED3, LED4);
+
+const int THRESH = 300;
+
+unsigned short sensors[5], mems[5], nbCapt, memsNb;
+char tab[4];
+float position, proportional, last_proportional, integral;
+
+unsigned char dir;
+float speed, duration, s1, s2;
+
+
+void getCapt(bool mem = false)
+{
+    int v = 0;
+    nbCapt = 0;
+    
+    m3pi.calibrated_sensors(sensors);
+    
+    for(int i=0 ; i<5 ;i++)
+    {
+        sensors[i] = (sensors[i] > THRESH ? true : false);
+        if(sensors[i])
+        {
+            v += 0x1<<i;
+            nbCapt++;
+        }
+        if(mem)
+        {
+            mems[i] = sensors[i];
+            memsNb = nbCapt;
+        }
+    }
+    m3pi.leds(v);
+}
+
+
+void set_motors(float mr, float ml)
+{
+    m3pi.left_motor(ml);
+    m3pi.right_motor(mr);
+}
+
+void suiviIntersect(float intervalTour = 0.0f)
+{
+    bt.printf("startSuivi_");
+    bool done = false;
+    unsigned int idx=0;
+    float vl, vr, omega, l_k=0, l_k1=0, sum=0, vmax=0.50f;
+    
+#if 1 // Low speed
+    const float Ki=0.005f, Kp=0.5f, Kd=1.5f, v=0.2;
+#else
+    const float Ki=0.0005f, Kp=0.05f, Kd=1.5f, v=0.80f;
+#endif
+    m3pi.sensor_auto_calibrate();
+    wait(1.0);
+    bt.printf("---> While in<--- ");
+    Timer t; t.start();
+    while(!done)
+    {
+        idx++;
+        getCapt();
+        // PID
+        l_k1 = l_k;
+        l_k = m3pi.line_position();
+        //v = sp;
+        omega = l_k*Kp; // Proportionnal
+        sum += l_k;
+        omega += sum*Ki; // Integral
+        omega += (l_k-l_k1)*Kd;
+        //omega *= v;
+        
+        
+        vl = v+omega < vmax ? (v+omega > -vmax ? v+omega : vmax) : vmax;
+        vr = v-omega < vmax ? (v-omega > -vmax ? v-omega : -vmax): vmax;
+        
+        
+        //------------------------------------- Pour les zigzags
+        //getCurrentSerial().printf("? hola que pasa ?");
+        
+        //-------------------------------------
+        
+        //-----------------Intersection
+        if(sensors[0] || sensors[4])
+        {
+            done = true;
+        }
+        set_motors(vr,vl);
+    }
+    bt.printf("-->While out<--");
+    
+    return;
+}
+
+
+
+
+
+
+
+
+
+void assignDirection(char c, float s){
+    if (c == 'a') m3pi.left(s);
+    if (c == 'c') m3pi.right(s);
+    if (c == 'd') {
+        m3pi.right(s);
+        wait(0.355);
+    }
+    wait(0.355);
+}
+
+
+
+
 int main() {
-    pc.printf("Hello World!\n");
+    bt.baud(115200);
+    char c = 'a';
+    bt.printf("Hello World!\n");
     while(1) {
-        pc.putc(pc.getc() + 1);
+        m3pi.stop();
+        c =  bt.getc();
+        bt.printf("Je viens de recevoir : %c \n", c);
+        //move
+        assignDirection(c, 0.2);
+        getCapt();
+        suiviIntersect();
     }
-}
\ No newline at end of file
+}
+