Diff: main.cpp

Revision:

4:a6cc2f03e69b

Parent:

3:9ef929639c4a

Child:

5:6efda58ff71b

--- a/main.cpp	Fri Jun 30 00:55:12 2017 +0000
+++ b/main.cpp	Fri Jun 30 01:18:54 2017 +0000
@@ -1,4 +1,5 @@
 #include "mbed.h"
+#include "math.h"
 #include "bit_test.h"
 #include "RS422_put.h"
 #include "sbdbt.h"
@@ -7,6 +8,7 @@
 #include "bno055_use.h"
 #include "pid.h"
 #include "limit.h"
+#include "accelerator.h"
 
 #define pc_baud     460800
 #define sbdbt_tx    p13
@@ -24,6 +26,7 @@
 #define yaw_Kp      0.07
 #define yaw_Ki      0
 #define yaw_Kd      0
+#define acceleration   5
 
 DigitalOut led(LED1);
 Serial pc(USBTX,USBRX);
@@ -33,25 +36,20 @@
 Mecanum mecanum;
 Bno055 bno055;
 Position_pid yaw_pid;
-
+Accel v1;
+Accel v2;
+Accel v3;
+Accel v4;
 
 void setup();
 void output();
 void put_output();
-float m1, m2, m3, m4;
-float a;
 float yaw, target_yaw;
 
 int main()
 {
     setup();
     while(1) {
-        yaw = bno055.getYawRad();
-        target_yaw = yaw;
-        yaw_pid.cal(target_yaw, yaw, output_period);
-        mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, yaw_pid.duty(), 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();
     }
 }
 
@@ -67,27 +65,34 @@
     output_timer.attach(&output, output_period);
     yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd);
     mecanum.setupdeg(bno055.getYawRad());
+    v1.setup(acceleration,output_period);
+    v2.setup(acceleration,output_period);
+    v3.setup(acceleration,output_period);
+    v4.setup(acceleration,output_period);
 }
 
 void put_output(){
-    m1 = mecanum.v1();
-    m2 = mecanum.v2();
-    m3 = mecanum.v3();
-    m4 = mecanum.v4();
+    yaw = bno055.getYawRad();
+    target_yaw = yaw;
+    yaw_pid.cal(target_yaw, yaw, output_period);
+    mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, yaw_pid.duty(), 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()); 
 }
 
 void output()
 {
+    put_output();
+
     static int counter;
     int id[nucleo_num] = {n1_id, n2_id, n3_id};
 
     switch (counter) {
         case 0:
-            rs422.put(id[counter],m1 ,m3);
+            rs422.put(id[counter], v1.duty(mecanum.v1()), v3.duty(mecanum.v3()));
             counter++;
             break;
         case 1:
-            rs422.put(id[counter],m2 ,m4);
+            rs422.put(id[counter], v2.duty(mecanum.v2()), v4.duty(mecanum.v4()));
             counter ++;
             break;
         case 2: