Diff: robot.cpp

Revision:

17:47e107f9587b

Parent:

16:73db7ef2deb6

Child:

18:501f1007a572

--- a/robot.cpp	Mon Mar 28 21:39:06 2016 +0000
+++ b/robot.cpp	Sun Apr 03 19:00:21 2016 +0000
@@ -1,8 +1,11 @@
 #include "robot.h"
 #include "global.h"
 
-// Global Definitions
-// ----------------------------------------------------------------
+enum System {
+    MOTOR = '0', 
+    SPEED = '1', 
+    STEERING = '2'
+};
 
 // Function prototypes
 // ----------------------------------------------------------------
@@ -12,7 +15,6 @@
 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
@@ -29,10 +31,24 @@
 osMutexId motorMutex;
 osMutexDef(motorMutex);
 
+// Variables
+// ----------------------------------------------------------------
+
+// For tunning and response measurement
+bool isTunning;
+int motorSample;
+int navigationSample;
+float leftMotorResponse[MOTOR_SAMPLES];
+float rightMotorResponse[MOTOR_SAMPLES];
+float speedResponse[NAVIGATION_SAMPLES];
+float steeringResponse[NAVIGATION_SAMPLES];
+
+
 // Set points and display variables
-float leftMotor, rightMotor;
 int x, height;
 float speed, steering;
+float leftMotor, rightMotor;
+
 
 // Functions
 // ----------------------------------------------------------------
@@ -90,15 +106,15 @@
     
     while (1) {
         osTimerStart(oneShotId, 2000); // Start or restart the watchdog timer interrupt and set to  2000ms.
-        if (bt.readable()) {
-            bt.scanf("%f :: %f", &speedRate, &steeringRate); 
+        if (pc.readable()) {
+            pc.scanf("%f :: %f", &speedRate, &steeringRate); 
             //flushBuffer()
 
             speed = SPEED_MAX * speedRate;
             steering = (SPEED_MAX * steeringRate)/3; 
             pc.printf("speed: %f, steering: %f \r\n", speedRate, steeringRate);  
             
-            bt.putc('~'); 
+            pc.putc('~'); 
         }
         
         osMutexWait(motorMutex, osWaitForever); 
@@ -109,24 +125,105 @@
 }
 
 void Tunning(void) {
+    System system;
+    int i;
     char key;
     float increment = 1;
-    motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD);
-    sensorPeriodicInt.attach(&NavigationISR, NAVIGATION_PERIOD);
+    
+    isTunning = true;
+    
+    pc.printf("\e[1;1H\e[2J");
+    pc.printf("Motor (0), Speed (1), Steering (2), Print (p) \r\n");
+    pc.printf("=> ");
     
     while (1) {
-        osTimerStart(oneShotId, 2000); // Start or restart the watchdog timer interrupt and set to  2000ms.
+        osTimerStart(oneShotId, 20000); // Start or restart the watchdog timer interrupt and set to  20000ms.
         if (pc.readable()) {
             key = pc.getc();
-            if (key == 'q') {
-                 ShutDown();
-                 return;
+            if ((key=='0')||(key=='1')||(key=='2')) {
+                system = (System)key;
+            } else if (key == 'q') {
+                ShutDown();
+                return;
+            } else if (key == 'p') {
+                
+                pc.printf("\r\n\r\n  Left Motor \t Right Motor \t Speed   \t Steering \r\n");
+                
+                while ((i < MOTOR_SAMPLES)||(i < NAVIGATION_SAMPLES)) {
+                    if (i < MOTOR_SAMPLES) {
+                        pc.printf("  %f, \t %f, \t ", 
+                            rightMotorResponse[i],
+                            leftMotorResponse[i]
+                        );
+                    } else pc.printf("           \t             \t");
+                    
+                    if (i < NAVIGATION_SAMPLES) {
+                        pc.printf("%f, \t %f ", 
+                            speedResponse[i],
+                            steeringResponse[i]
+                        );
+                    } else pc.printf("           \t   ");
+                    pc.printf("\r\n");
+                    i++;
+                }
+                
+            } else {
+                switch (system) {
+                    case MOTOR:
+                        if (key == 'a') leftMotorPI.kP = leftMotorPI.kP + increment;   
+                        else if (key == 'z') leftMotorPI.kP = leftMotorPI.kP - increment; 
+                        else if (key == 's') leftMotorPI.kI = leftMotorPI.kI + increment; 
+                        else if (key == 'x') leftMotorPI.kI = leftMotorPI.kI - increment; 
+                        break;
+                    case SPEED:
+                        if (key == 'a') heightPI.kP = heightPI.kP + increment;   
+                        else if (key == 'z') heightPI.kP = heightPI.kP - increment; 
+                        else if (key == 's') heightPI.kI = heightPI.kI + increment; 
+                        else if (key == 'x') heightPI.kI = heightPI.kI - increment; 
+                        break;
+                   case STEERING:
+                        if (key == 'a') xPI.kP = xPI.kP + increment;   
+                        else if (key == 'z') xPI.kP = xPI.kP - increment; 
+                        else if (key == 's') xPI.kI = xPI.kI + increment; 
+                        else if (key == 'x') xPI.kI = xPI.kI - increment; 
+                        break;
+                }
+                
+                if (key == 'd') increment = increment*10;   
+                else if (key == 'c') increment = increment/10; 
             }
-            UpdateGains(key, &increment);  
+            
+            pc.printf("\e[1;1H\e[2J");
+            pc.printf("Current system: %c \r\n\r\n", system);
+            
+            pc.printf("Motor (0) \t Speed (1) \t Steering (2) \t Print (p) \r\n");
+            pc.printf("Kp (a/z)  \t Ki (s/x)  \t inc (d/c) \r\n\r\n");
+            
             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); 
+            pc.printf("Speed     \t KP: %f, KI: %f \r\n", heightPI.kP, heightPI.kI); 
+            pc.printf("Steering  \t KP: %f, KI: %f \r\n\r\n", xPI.kP, xPI.kI);
+            pc.printf("=> ");
+            
+            if (key != 'p') {
+                motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD);
+                sensorPeriodicInt.attach(&NavigationISR, NAVIGATION_PERIOD);
+                motorSample = 0;
+                navigationSample = 0;
+                i = 0;
+            }
+        }
+        
+        // Stop when all samples are collected
+        if ((navigationSample >= NAVIGATION_SAMPLES)&&(motorSample >= MOTOR_SAMPLES)) {
+            motorPeriodicInt.detach();
+            sensorPeriodicInt.detach();   
+            leftPwm.pulsewidth(0);
+            rightPwm.pulsewidth(0);
+            xPI.Reset();
+            heightPI.Reset();
+            leftMotorPI.Reset();
+            rightMotorPI.Reset();
         }
         Thread::wait(500); // Go to sleep for 500 ms
     }
@@ -135,13 +232,18 @@
 void ShutDown(void) {
     motorPeriodicInt.detach();
     sensorPeriodicInt.detach();
-    osThreadTerminate(navigationId);
-    osThreadTerminate(motorId);
     leftPwm.pulsewidth(0);
     rightPwm.pulsewidth(0);
+    isTunning = false;
+    motorSample = 0;
+    navigationSample = 0;
+    xPI.Reset();
+    heightPI.Reset();
+    leftMotorPI.Reset();
+    rightMotorPI.Reset();
+    pc.printf("Robot Shutdown... \r\n\r\n");
 }
 
-
 // Threads
 // ----------------------------------------------------------------
 void NavigationThread(void const *argument) {
@@ -183,6 +285,12 @@
                 missingCount++;    
             }
         }
+        
+        if (isTunning && (navigationSample < NAVIGATION_SAMPLES)) {
+            speedResponse[navigationSample] = speed; 
+            steeringResponse[navigationSample] = steering;
+            navigationSample++;
+        }
     } 
 }
 
@@ -222,6 +330,12 @@
         
         leftPwm.pulsewidth(fabs(timeOnLeft));
         rightPwm.pulsewidth(fabs(timeOnRight));
+        
+        if (isTunning && (motorSample < MOTOR_SAMPLES)) {
+            leftMotorResponse[motorSample] = timeOnLeft; 
+            rightMotorResponse[motorSample] = timeOnRight;
+            motorSample++;
+        } 
     } 
 }
 
@@ -246,52 +360,4 @@
 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