Diff: robot.cpp

Revision:

14:2d609d465f00

Parent:

7:5ef312aa2678

Child:

15:caa5a93a31d7

--- a/robot.cpp	Fri Mar 25 19:34:16 2016 +0000
+++ b/robot.cpp	Sat Mar 26 17:52:34 2016 +0000
@@ -12,6 +12,8 @@
 void WatchdogISR(void const *n);
 void MotorThread(void const *argument);
 void NavigationThread(void const *argument);
+void UpdateGains(char key, float *increment);
+void ShutDown(void);
 
 // Interrupt and thread control
 osThreadId motorId, navigationId, wdtId;
@@ -28,15 +30,14 @@
 
 // Set points and display variables
 float leftMotor, rightMotor;
-float timeOnLeft, timeOnRight; // REMOVELATER
-short dtR, dPR; // REMOVELATER
-short dt, dP; // REMOVELATER
 int x, height;
 float speed, steering;
 
 // Functions
 // ----------------------------------------------------------------
-void AutoTrack() { 
+void AutoTrack(bool isTunning) { 
+    float increment = 1;
+    char key;
     
     leftPwm.period(PWM_PERIOD);
     leftPwm.pulsewidth(0);
@@ -61,7 +62,7 @@
     ioReset = 0; ioReset = 1; 
     wait_us(10); ioReset = 0; spiReset = 0; spiReset = 1; 
     wait_us(10); spiReset = 0; 
-    pc.printf("\n\rDevice Id: %d", deSpi.write(0x8004)); // Read count & time for both motors
+    pc.printf("\n\rDevice Id: %d \r\n", deSpi.write(0x8004)); // Read count & time for both motors
     
     // Specify periodic ISRs and their intervals in seconds
     // TODO: Optimize interrupt time for motor... Currently too fast
@@ -71,13 +72,32 @@
     while (1) {
         
         osTimerStart(oneShotId, 2000); // Start or restart the watchdog timer interrupt and set to  2000ms.
-
-        //pc.printf("X Coordinate: %d, Height: %d \r\n", x, height);
-        pc.printf("Speed Set: %f, Steering Set: %f \r\n", speed, steering);
-        pc.printf("Left Motor Set: %f, Right Motor Set: %f \n\r", leftMotor, rightMotor);
-        //pc.printf("Left time %d, Left pos: %d \n\r", dt, dP);
-        //pc.printf("Right time %d, Right pos: %d \n\r", dtR, dPR);
-        pc.printf("Left Motor: %f, Right Motor: %f \n\r\r\n", timeOnLeft, timeOnRight);
+        
+        if (isTunning) {
+            if (pc.readable()) {
+                key = pc.getc();
+                if (key == 'q') {
+                     ShutDown();
+                     return;
+                }
+                UpdateGains(key, &increment);  
+                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); 
+            }
+        } else {
+            if (pc.readable()) {
+                key = pc.getc();
+                if (key == 'q') {
+                     ShutDown();
+                     return;
+                }
+            }
+            pc.printf("X Coordinate: %d, Height: %d \r\n", x, height);
+            pc.printf("Speed Set: %f, Steering Set: %f \r\n", speed, steering);
+            pc.printf("Left Motor Set: %f, Right Motor Set: %f \n\r\r\n\r\n", leftMotor, rightMotor);
+        }
         
         Thread::wait(500); // Go to sleep for 500 ms
     }
@@ -87,11 +107,19 @@
 
 }
 
+void ShutDown(void) {
+    osThreadTerminate(navigationId);
+    osThreadTerminate(motorId);
+    leftPwm.pulsewidth(0);
+    rightPwm.pulsewidth(0);
+}
+
 
 // Threads
 // ----------------------------------------------------------------
 void NavigationThread(void const *argument) {
     int count;
+    int missingCount;
     
     while (1) {
         osSignalWait(navigationSignal, osWaitForever); // Go to sleep until navigation signal is set high by ISR
@@ -100,6 +128,7 @@
         
         // If target returned
         if (count && (pixy.blocks[0].signature == TARGET_DECIMAL)) {
+            missingCount = 0;
             height = pixy.blocks[0].height; //use this for now
             x = pixy.blocks[0].x;
             
@@ -116,13 +145,23 @@
                 rightMotor = speed + steering;
             }
             osMutexRelease(motorMutex); 
+        } else {
+            if (missingCount >= 30) {
+                osMutexWait(motorMutex, osWaitForever);
+                leftMotor = (leftMotor > rightMotor)? SPEED_MAX/3 : -SPEED_MAX/3;
+                rightMotor = (rightMotor > leftMotor)? SPEED_MAX/3 : -SPEED_MAX/3;
+                osMutexRelease(motorMutex); 
+            } else {
+                missingCount++;    
+            }
         }
     } 
 }
 
 void MotorThread(void const *argument) {
     float leftSet, rightSet, angVel;
-    //short dP, dt;
+    float timeOnLeft, timeOnRight;
+    short dP, dt;
 
     while (1) {
         osSignalWait(motorSignal, osWaitForever); // Go to sleep until motor signal is set high by ISR
@@ -141,9 +180,9 @@
         timeOnLeft = leftMotorPI.Run(leftSet-angVel, PWM_PERIOD);
         
         // Run PI control on right motor
-        dPR = deSpi.write(0);
-        dtR = deSpi.write(0); 
-        angVel = -QE2RadsPerSec(dPR, dtR); // motors have opposite orientations
+        dP = deSpi.write(0);
+        dt = deSpi.write(0); 
+        angVel = -QE2RadsPerSec(dP, dt); // motors have opposite orientations
         timeOnRight = rightMotorPI.Run(rightSet-angVel, PWM_PERIOD); //
         
         // Output new PWM and direction 
@@ -178,4 +217,53 @@
 // TODO: Shutdown system
 void CollisionISR(void) {
     led4 = 1; // Activated on collision
+    
+}
+
+// Nasty case function to update gain values
+void UpdateGains(char key, float *increment) {
+    switch(key) {
+        case 'e':
+            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 '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 '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; 
+    }
 }
\ No newline at end of file