Diff: main.cpp

Revision:

5:6efda58ff71b

Parent:

4:a6cc2f03e69b

Child:

7:c4ae1d001d09

Child:

8:3b7530174f8a

--- a/main.cpp	Fri Jun 30 01:18:54 2017 +0000
+++ b/main.cpp	Fri Jun 30 05:40:02 2017 +0000
@@ -23,9 +23,9 @@
 #define n1_id       3
 #define n2_id       4
 #define n3_id       0
-#define yaw_Kp      0.07
-#define yaw_Ki      0
-#define yaw_Kd      0
+#define yaw_Kp      0.01
+#define yaw_Ki      0.01
+#define yaw_Kd      0.01
 #define acceleration   5
 
 DigitalOut led(LED1);
@@ -71,12 +71,13 @@
     v4.setup(acceleration,output_period);
 }
 
-void put_output(){
+void put_output()
+{
     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()); 
+//    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()
@@ -86,6 +87,10 @@
     static int counter;
     int id[nucleo_num] = {n1_id, n2_id, n3_id};
 
+    if(sbdbt.batu) {
+        mecanum.boost();
+    }
+
     switch (counter) {
         case 0:
             rs422.put(id[counter], v1.duty(mecanum.v1()), v3.duty(mecanum.v3()));
@@ -102,4 +107,6 @@
         default:
             break;
     };
-}
\ No newline at end of file
+
+}
+