Diff: main.cpp

Revision:

0:fd933ea5f19d

Child:

2:82c337a18500

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Aug 05 02:29:14 2017 +0000
@@ -0,0 +1,52 @@
+#include "2017_3_z.h"
+#include "mbed.h"
+#include <math.h>
+#include "MD_PID.h"
+#include "QEI.h"
+
+Omni_3 Omni(p21, p22, p18, p17, p11, p12, p23, p24, p16, p15, p27, p28, p25, p26, p14, p13, p29, p30, 1);
+
+Serial pc (USBTX, USBRX);
+I2CSlave slave(p9, p10);
+
+DigitalOut check1(LED1);
+
+union Get_data{
+    char alldata[3];
+    struct{
+        signed int x :8;
+        signed int y :8;
+        signed int t :8;
+    }data;
+}act;
+
+int main() {
+    Omni.Drive(0, 0, 0);
+    float x, y, st;
+    
+    slave.address(0x08);
+    while(1) {
+        int i = slave.receive();
+        check1 = 0.0;
+        switch (i) {
+            case I2CSlave::ReadAddressed:    
+                check1 = check1 ^ 1;
+                break;
+            case I2CSlave::WriteGeneral:
+                slave.read(act.alldata, 3);
+                check1 = check1 ^ 1;
+                break;
+            case I2CSlave::WriteAddressed:
+                slave.read(act.alldata, 3);
+                check1 = check1 ^ 1;
+                x  = (float)act.data.x / 127.0;
+                y  = (float)act.data.y / 127.0;
+                st = (float)act.data.t / 127.0 * 0.1;
+                break;
+        }
+        Omni.Drive(x, y, st);
+        
+        for(int i = 0; i < 3; i++) act.alldata[i] = 0;
+        
+    }
+}