Diff: tuning.cpp

Revision:

13:d46282d84e47

Parent:

12:9d01aa43cd1f

Parent:

10:1acbcd54359b

--- a/tuning.cpp	Sat Mar 26 14:08:16 2016 +0000
+++ b/tuning.cpp	Sat Mar 26 14:09:40 2016 +0000
@@ -71,8 +71,70 @@
     }
 }
 
-void MotorTuning(void) {
-    
+//MotorTuning()
+//Tune the proportional gains until the robot goes straight
+// q  = +speed
+// a = -speed
+// w = +kp left
+// S = -kp left
+// e = +kp right
+// d = -kp right
+// z = stop
+// x = start
+// p = print gain
+void MotorTuning(void) { 
+        float leftset=0,rightset=0;
+        RunMotor();
+        
+        pc.printf("q = +speed\r\n a = -speed\r\n w = +kp left\r\n s = -kp left\r\n e = +kp right\r\n d = -kp right\r\n z = stop\r\n x = start\r\n p = print gains\r\n");
+        
+        while(1){
+        ResetWatchDog();    
+            
+        if (bt.readable()) {
+        char key = bt.getc();
+            switch(key) {
+                case 'x':
+                    rightset=5;
+                    leftset=5;
+                    break;
+                case 'z':
+                    rightset=0;
+                    leftset=0;
+                    break;
+                case 'q':
+                    rightset=rightset+1;
+                    leftset=leftset+1;
+                    break;
+                case 'a':
+                    rightset=rightset+1;
+                    leftset=leftset+1;
+                    break;
+                case 'w':
+                    leftMotorPI.kP = leftMotorPI.kP + 0.000001;
+                    break;
+                case 's':
+                    leftMotorPI.kP = leftMotorPI.kP - 0.000001;
+                    break;
+                case 'e':
+                    rightMotorPI.kP = rightMotorPI.kP + 0.000001;
+                    break;
+                case 'd':
+                    rightMotorPI.kP = rightMotorPI.kP - 0.000001;
+                    break;
+                case 'p':
+                    pc.printf("right gain: %f, left gain: %f\r\n",leftMotorPI.kP,rightMotorPI.kP);
+                    break;
+                }
+        }
+        
+        osMutexWait(motorMutex, osWaitForever); 
+        leftMotor = leftSet;
+        rightMotor = RightSet;
+        osMutexRelease(motorMutex); 
+        
+        Thread::wait(500);
+        }
 }
 
 void SpeedTuning(void) {