Diff: tuning.cpp

Revision:

8:b0478286ad21

Parent:

7:5ef312aa2678

Child:

9:62fbb69b612c

Child:

10:1acbcd54359b

--- a/tuning.cpp	Fri Mar 25 19:34:16 2016 +0000
+++ b/tuning.cpp	Fri Mar 25 20:05:04 2016 +0000
@@ -1,7 +1,46 @@
 #include "tuning.h"
+#include "robot.h"
 
 void SystemTuning(void) {
+    RunMotor();
+    RunNavigation();
+    char key;
+    float increment = 0.0000001;
     
+    while (1) {
+        
+        ResetWatchDog();
+        
+        if (bt.readable()) {
+            char key = bt.getc();
+            switch(key) {
+                case 'e':
+                    motorKP = motorKP + increment;
+                    break;
+                case 'e':
+                    motorKP = motorKP + 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;     
+        }
+        
+        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
+    }
 }
 
 void MotorTuning(void) {