Diff: main.cpp

Revision:

0:727643d2c6ef

Child:

1:236ac79b07d7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 09 10:25:39 2020 +0000
@@ -0,0 +1,152 @@
+#include "mbed.h"
+#include "m3pi.h"
+
+#define BUFF_SIZE 200
+
+
+Serial pc(USBTX, USBRX);
+DigitalOut myled(LED1);
+DigitalOut m (LED4);
+InterruptIn Msg(USBRX,PullUp);
+m3pi pi;
+int flag;
+char clavier;
+char buffer[BUFF_SIZE];
+
+
+
+void readBattery()
+{
+    char s[500];
+    char* s2 = "Battery:%f V\n";
+    sprintf(s, s2, pi.battery());
+    for (int i=0; i<strlen(s); i++) {
+        pc.putc(s[i]);
+    }
+    pc.printf("\r");
+}
+
+void readSensors()
+{
+    char s[500];
+    char* s2 = "Sensors:(%u,%u,%u,%u,%u) \n";
+    unsigned short val[5];
+    pi.calibrated_sensors(val);
+    sprintf(s, s2, val[0],val[1],val[2],val[3],val[4]);
+    for (int i=0; i<strlen(s); i++) {
+        pc.putc(s[i]);
+    }
+    pc.printf("\r");
+}
+
+
+void Interrupt()
+{
+    //printf("%c",pc.getc());
+    //Command(pc.getc());
+    flag=1;
+    m=1;
+}
+
+
+void Command(char c){
+    switch (c) {
+        case 'z':
+            pi.forward(0.5);
+            wait(0.5);
+            pi.stop();
+            break;
+        case 'q':
+            pi.left(0.5);
+            wait(0.5);
+            pi.stop();
+            break;
+        case 's':
+            pi.backward(0.5);
+            wait(0.5);
+            pi.stop();
+            break;
+        case 'd':
+            pi.right(0.5);
+            wait(0.5);
+            pi.stop();
+            break;
+        case 'b':
+            readBattery();
+            break;
+        case 'c':
+            readSensors();
+            break;
+    }
+}
+
+int main()
+{
+    //pc.attach(&Interrupt);
+    //Msg.rise(&Interrupt);
+    pc.baud(9600);
+    pi.sensor_auto_calibrate();
+    //clavier='a';
+
+    //flag = 0;
+    while(1) {
+        ///BLINK LEDS
+        myled = 1;
+        wait(0.1);
+        myled = 0;
+        wait(0.1);
+        
+        ///GET INPUT
+        int i = 0;
+        while (pc.readable()) {
+            if (i < BUFF_SIZE)
+                buffer[i++] = pc.getc();
+        }
+        while (i > 0) {
+            Command(buffer[i]);
+            pi.cls();
+            pi.locate(0,0);
+            pi.print(&buffer[i], 1);
+            i--;
+        }
+
+/*
+        if(flag) {
+            clavier+=1;
+            pi.print(&clavier,1);
+            //clavier = pc.getc();
+            
+            switch (clavier) {
+                case 'z':
+                    pi.forward(0.5);
+                    wait(0.5);
+                    pi.stop();
+                    break;
+                case 'q':
+                    pi.left(0.5);
+                    wait(0.5);
+                    pi.stop();
+                    break;
+                case 's':
+                    pi.backward(0.5);
+                    wait(0.5);
+                    pi.stop();
+                    break;
+                case 'd':
+                    pi.right(0.5);
+                    wait(0.5);
+                    pi.stop();
+                    break;
+                case 'b':
+                    readBattery();
+                    break;
+                case 'c':
+                    readSensors();
+                    break;
+            }
+            flag=0;
+        }
+        */
+    }
+}
+