Diff: tuning.cpp

Revision:

9:62fbb69b612c

Parent:

8:b0478286ad21

Child:

11:a3accd7bbf16

--- a/tuning.cpp	Fri Mar 25 20:05:04 2016 +0000
+++ b/tuning.cpp	Fri Mar 25 21:20:10 2016 +0000
@@ -1,44 +1,72 @@
 #include "tuning.h"
 #include "robot.h"
+#include "global.h"
 
 void SystemTuning(void) {
+    InitializeRobot();
     RunMotor();
     RunNavigation();
     char key;
-    float increment = 0.0000001;
+    float increment = 1;
     
     while (1) {
         
         ResetWatchDog();
         
-        if (bt.readable()) {
-            char key = bt.getc();
+        if (pc.readable()) {
+            key = pc.getc();
             switch(key) {
                 case 'e':
-                    motorKP = motorKP + increment;
+                    leftMotorPI.kP = leftMotorPI.kP + increment;
+                    break;
+                case 'd':
+                    leftMotorPI.kP = leftMotorPI.kP - increment;
+                    break;
+                case 'r':
+                    leftMotorPI.kI = leftMotorPI.kI + increment;
+                    break;
+                case 'f':
+                    leftMotorPI.kI = leftMotorPI.kI - increment;
                     break;
-                case 'e':
-                    motorKP = motorKP + increment;
+                case 't':
+                    xPI.kP = xPI.kP + increment;
+                    break;
+                case 'g':
+                    xPI.kP = xPI.kP - increment;
+                    break;
+                case 'y':
+                    xPI.kI = xPI.kI + increment;
+                    break;
+                case 'h':
+                    xPI.kI = xPI.kI - increment;
                     break;
-                case 'e':
-                    motorKP = motorKP + increment;
-                    break; 
-            }
-            if (inKey == 'i') Kp = Kp + 0.000001;
-            else if (inKey == 'k') Kp = Kp - 0.000001;
-            else if (inKey == 'w') Ki = Ki + 0.000001;
-            else if (inKey == 's') Ki = Ki - 0.000001;
-            
-            if (Setpoint == 6) Setpoint = 3;
-            else Setpoint = 6;
-            
-            led4=0;     
+                case 'u':
+                    heightPI.kP = heightPI.kP + increment;
+                    break;
+                case 'j':
+                    heightPI.kP = heightPI.kP - increment;
+                    break;
+                case 'i':
+                    heightPI.kI = heightPI.kI + increment;
+                    break;
+                case 'k':
+                    heightPI.kI = heightPI.kI - increment;
+                    break;
+                case 'o':
+                    increment = increment * 10;
+                    break;
+                case 'l':
+                    increment = increment / 10;
+                    break;                    
+            }  
+            leftMotorPI.kP = rightMotorPI.kP;
+            leftMotorPI.kI = rightMotorPI.kI;
+            pc.printf("Increment \t %f \r\n", increment);
+            pc.printf("Motor \t KP: %f, KI: %f \r\n", leftMotorPI.kP, leftMotorPI.kI);
+            pc.printf("Steering \t KP: %f, KI: %f \r\n", xPI.kP, xPI.kI);
+            pc.printf("Speed  \t KP: %f, KI: %f \r\n\r\n\r\n", heightPI.kP, heightPI.kI);
         }
         
-        bt.printf("X Coordinate: %d, Height: %d \r\n", x, height);
-        bt.printf("Speed Set: %f, Steering Set: %f \r\n", speed, steering);
-        bt.printf("Left Motor Set: %f, Right Motor Set: %f \n\r", leftMotor, rightMotor);
-        
         Thread::wait(500); // Go to sleep for 500 ms
     }
 }