Diff: main.cpp

Revision:

0:bf96e953cdb8

Child:

1:2d878962e6ea

diff -r 000000000000 -r bf96e953cdb8 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jun 26 09:59:14 2017 +0000
@@ -0,0 +1,92 @@
+#include "mbed.h"
+#include "bit_test.h"
+#include "RS422_put.h"
+#include "sbdbt.h"
+#include "mecanum.h"
+#include "bno055_lib.h"
+#include "bno055_use.h"
+#include "pid.h"
+#include "limit.h"
+
+#define pc_baud     460800
+#define sbdbt_tx    p13
+#define sbdbt_rx    p14
+#define sbdbt_baud  115200
+#define rs422_tx    p28
+#define rs422_rx    p27
+#define rs422_baud  115200
+#define output_period   0.005
+#define nucleo_num  3
+#define pi          3.141592
+#define n1_id       3
+#define n2_id       4
+#define n3_id       0
+#define Kp          0.0
+#define Ki          0.0
+#define Kd          0.0
+
+DigitalOut led(LED1);
+Serial pc(USBTX,USBRX);
+RS422 rs422(rs422_tx, rs422_rx);
+Sbdbt sbdbt(sbdbt_tx, sbdbt_rx);
+Ticker output_timer;
+Mecanum mecanum;
+Bno055 bno055;
+
+void setup();
+void output();
+void put_output();
+float m1, m2, m3, m4;
+float a;
+
+int main()
+{
+    setup();
+    while(1) {        
+        mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, 0, bno055.getYawRad());
+        pc.printf("%f\t data %f\t %f\t %f\t %f\t\r\n", bno055.getYawRad(), sbdbt.left_x, sbdbt.left_y, mecanum.VX(), mecanum.VY());
+        put_output();
+    }
+}
+
+void setup()
+{
+    bno055.begin();
+    wait(1);
+    bno055.firstRead();
+    pc.baud(pc_baud);
+    sbdbt.begin(sbdbt_baud);
+    rs422.begin(rs422_baud);
+    output_timer.attach(&output, output_period);
+    mecanum.setupdeg(bno055.getYawRad());
+}
+
+void put_output(){
+    m1 = mecanum.v1();
+    m2 = mecanum.v2();
+    m3 = mecanum.v3();
+    m4 = mecanum.v4();
+}
+
+void output()
+{
+    static int counter;
+    int id[nucleo_num] = {n1_id, n2_id, n3_id};
+
+    switch (counter) {
+        case 0:
+            rs422.put(id[counter],m1 ,m3);
+            counter++;
+            break;
+        case 1:
+            rs422.put(id[counter],m2 ,m4);
+            counter ++;
+            break;
+        case 2:
+            rs422.put(id[counter], 0.0, 0.0);
+            counter = 0;
+            break;
+        default:
+            break;
+    };
+}
\ No newline at end of file