Diff: main.cpp

Revision:

0:6da7d0e457a2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Aug 17 03:49:34 2017 +0000
@@ -0,0 +1,96 @@
+#include "mbed.h"
+#include "omuni.hpp"
+
+int addr[] = {0x10, 0x12, 0x14};
+int armAddr[] = {0x16, 0x18};
+bool arm = false;
+
+Serial p(USBTX, USBRX);
+Serial pc(PA_11, PA_12);
+I2C i2cMaster(D14, D15);
+// archan
+omuni omuni(&i2cMaster, TIM1, TIM2, TIM3, 800, 2.0f, addr, 0.25f, 0.1f);
+
+float vx, vy, ome;
+bool f, pre_f;
+char recv[3] = {0};
+
+void pc_rx()
+{
+    char rtemp = pc.getc();
+    
+    if((rtemp & 0b11000000) == 0b00000000)         recv[0] = rtemp;
+    else if((rtemp & 0b11000000) == 0b01000000)    recv[1] = rtemp;
+    else if((rtemp & 0b11000000) == 0b10000000)    recv[2] = rtemp;
+    
+    float direc = (recv[0] & 0x0f) * 3.141592 / 8.0;
+    float speed = 0.8 * ((recv[0] & 0b00110000) >> 4);
+    vx = speed * cos(direc) * -1;
+    vy = speed * sin(direc) * -1;
+    
+    if(recv[2] & 0b1)
+    {
+        arm = true;
+    }
+    else
+    {
+        arm = false;
+    }
+    
+    if(recv[1] & 0b11000)
+    {
+        f = false;
+        ome = ((recv[1] & 0b11000) >> 3) * 3.141592 / 2.0;
+        if(recv[1] & 0b100000)  ome *= -1;
+    }
+    else
+    {
+        if(recv[1] & 0b11)
+        {
+            f = true;
+//            if(pre_f == false)  omuni.reset_theta();
+        }
+        else
+        {
+            f = false;
+        }
+        ome = (recv[1] & 0b11) * 3.141592 / 2.0;
+        if((recv[1] & 0b100))   ome *= -1;
+    }
+    pre_f = f;
+}
+
+int main()
+{
+    p.baud(115200);
+    pc.baud(9600);
+    pc.printf("Hello!\n");
+    pc.attach(pc_rx, Serial::RxIrq);
+    omuni.set_speed(0.0f, 0.0f);
+    // archan
+    omuni.set_pid(0, 3.0f, 0.07f, 0.05f);
+    omuni.set_pid(1, 3.0f, 0.07f, 0.05f);
+    omuni.set_pid(2, 3.0f, 0.07f, 0.05f);
+    
+    while(1)
+    {
+        wait(0.001);
+        omuni.set_speed(vx, vy, ome, f);
+        omuni.drive();
+        
+        if(arm)
+        {
+            char send = -127;
+            i2cMaster.write(armAddr[0], &send, 1);
+            i2cMaster.write(armAddr[1], &send, 1);
+        }
+        else
+        {
+            char send = 0;
+            i2cMaster.write(armAddr[0], &send, 1);
+            i2cMaster.write(armAddr[1], &send, 1);
+        }
+        
+        p.printf("%f\n", omuni.get_theta());
+    }
+}
\ No newline at end of file