Diff: main.cpp

Revision:

2:d5b8f8e62923

Parent:

1:2d878962e6ea

Child:

3:9ef929639c4a

diff -r 2d878962e6ea -r d5b8f8e62923 main.cpp
--- a/main.cpp	Thu Jun 29 08:52:40 2017 +0000
+++ b/main.cpp	Thu Jun 29 09:47:49 2017 +0000
@@ -21,9 +21,9 @@
 #define n1_id       3
 #define n2_id       4
 #define n3_id       0
-#define Kp          0.0
-#define Ki          0.0
-#define Kd          0.0
+#define yaw_Kp      0.07
+#define yaw_Ki      0
+#define yaw_Kd      0
 
 DigitalOut led(LED1);
 Serial pc(USBTX,USBRX);
@@ -32,18 +32,24 @@
 Ticker output_timer;
 Mecanum mecanum;
 Bno055 bno055;
+Position_pid yaw_pid;
+
 
 void setup();
 void output();
 void put_output();
 float m1, m2, m3, m4;
 float a;
+float yaw, target_yaw;
 
 int main()
 {
     setup();
-    while(1) {        
-        mecanum.sbdbt_cal(sbdbt.left_x, sbdbt.left_y, sbdbt.l1, sbdbt.r1, 0, bno055.getYawRad());
+    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();
     }
@@ -59,6 +65,7 @@
     sbdbt.begin(sbdbt_baud);
     rs422.begin(rs422_baud);
     output_timer.attach(&output, output_period);
+    yaw_pid.setup(yaw_Kp, yaw_Ki, yaw_Kd);
     mecanum.setupdeg(bno055.getYawRad());
 }