Diff: main.cpp

Revision:

14:1d9ae3128002

Parent:

12:c7e91c4c2ffa

Child:

15:d4ff132a616d

--- a/main.cpp	Mon Sep 04 05:35:59 2017 +0000
+++ b/main.cpp	Tue Sep 05 00:41:30 2017 +0000
@@ -9,9 +9,9 @@
 
 #define DEBUG 
 
-#define KP 5
+#define KP 2.5
 #define KI 0 
-#define KD 0
+#define KD 0.05
 #define RATE 0.01
 Omni omni(4);
 Serial RS485(RS485_TX,RS485_RX,38400);
@@ -68,7 +68,7 @@
 }
 int main()
 {   
-    bool airFlag1=0,airFlag2=0,airStatus1=0,airStatus2=0;
+    bool airFlag1 = 0,airFlag2 = 0,airStatus1 = 0,airStatus2 = 0,pidflag = 0;
     int error_val = 0,i = 0;
     double pwm = 0.0;
     init();
@@ -83,10 +83,15 @@
                 powerSw = 1;
             }
             error_val = 0;
-            pid.setProcessValue(R1370.getAngle());
             pc.printf("%lf    %lf     ",R1370.getAngle(),-1*pid.compute());
-            omni.computeXY(0,0,-1*pid.compute());
-            omni.computeXY(-1*con.getStick(2),con.getStick(3),-1*pid.compute());           
+            if((con.getStick(0)==0)&&(pidflag==1)) {
+                pidflag = 0;
+                pid.setSetPoint(R1370.getAngle());;           
+            }else {
+                pidflag = 1;
+                omni.computeXY(-1*con.getStick(2),con.getStick(3),con.getStick(0));
+            }
+            pid.setProcessValue(R1370.getAngle()); 
             for(int i = 0; i < 4; i++) {
                 pc.printf("%lf,",omni.getOutput(i));
                 wheels[i].setSpeed(omni.getOutput(i));
@@ -143,5 +148,6 @@
             error_val++;
             if(error_val > 10) powerSw = 0;
         }
+    wait(RATE);
     }
 }