Diff: robot.cpp

Revision:

19:05b8123905fb

Parent:

18:501f1007a572

--- a/robot.cpp	Thu Apr 07 13:19:29 2016 +0000
+++ b/robot.cpp	Mon Apr 24 21:37:50 2017 +0000
@@ -59,8 +59,6 @@
     rightPwm.pulsewidth(0);
 
     motorMutex = osMutexCreate(osMutex(motorMutex));
-
-    bumper.rise(&CollisionISR); // Attach interrupt handler to rising edge of Bumper
     
     // Start execution of the threads MotorThread, and NavigationThread:
     navigationId = osThreadCreate(osThread(NavigationThread), NULL);
@@ -73,15 +71,17 @@
     //SPI Initialization
     pc.printf("\n\rStarting SPI...");
     deSpi.format(16,1); // 16 bit mode 1
+    deSpi.frequency(500000);
     ioReset = 0; ioReset = 1; 
     wait_us(10); ioReset = 0; spiReset = 0; spiReset = 1; 
     wait_us(10); spiReset = 0; 
     pc.printf("\n\rDevice Id: %d \r\n", deSpi.write(0x8004)); // Read count & time for both motors
+    FlushBuffer();
 }
 
+// Setup robot for auto tracking and listen for commands
 void AutoTrack(void) {
     char key;
-    bool isReady;
     motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD);
     sensorPeriodicInt.attach(&NavigationISR, NAVIGATION_PERIOD);
     
@@ -92,26 +92,21 @@
             if (key == 'q') {
                 ShutDown();
                 return;
-            } else if ((key == '1')||(key == 'm')) {
+            } else if (key == 'm') {
                 ShutDown();
                 ManualControl();
-            } else if (key == '~') {
-                isReady = true;   
-            }
+            } 
         }
         
-        if (isReady) {
-            pc.printf("X: %d, Height: %d \r\n", x, height);
-            pc.printf("Speed: %f \r\nSteering: %f \r\n", speed, steering);
-            isReady = false;  
-            pc.putc('~');
-        }
+        pc.printf("X: %d, Height: %d \r\n", x, height);
+        pc.printf("Speed: %f \r\nSteering: %f \r\n", speed, steering);
+
         Thread::wait(250);
     }
 }
 
+// Setup robot for manual control and listen for commands
 void ManualControl(void) {
-    bool isReady;
     char key;
     motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD);
     float speedRate, steeringRate;
@@ -123,25 +118,25 @@
             if (key == 'q') {
                 ShutDown();
                 return;
-            } else if ((key == '0')||(key == 'a')) {
+            } else if (key == 'a') {
                 ShutDown();
                 AutoTrack();
-            } else if (key == '~') {
-                isReady = true;   
+            } else if (key == '8') {
+                speedRate = (speedRate < 1)? speedRate + 0.1 : 1;
+            } else if (key == '5') {
+                speedRate = (speedRate > -1)? speedRate - 0.1 : -1;
+            } else if (key == '6') {
+                steeringRate = (steeringRate < 1)? steeringRate + 0.1 : 1;
+            } else if (key == '4') {
+                steeringRate = (steeringRate > -1)? steeringRate - 0.1 : -1;
             } 
+            
+            pc.printf("%.2f,%.2f\r\n", speedRate, steeringRate); 
         }
         
-        if (isReady) {
-            pc.scanf("%f :: %f", &speedRate, &steeringRate); 
-            speed = SPEED_MAX * speedRate;
-            steering = (SPEED_MAX * steeringRate);    
-            isReady = false;
-            pc.putc('~'); 
-        }
-        
-        osMutexWait(motorMutex, osWaitForever); 
-        leftMotor = speed - steering;
-        rightMotor = speed + steering;
+        osMutexWait(motorMutex, osWaitForever);  
+        leftMotor = SPEED_MAX*(speedRate + steeringRate);
+        rightMotor = SPEED_MAX*(speedRate - steeringRate);
         osMutexRelease(motorMutex); 
     }
 }
@@ -264,13 +259,13 @@
     leftMotorPI.Reset();
     rightMotorPI.Reset();
     pc.printf("Robot Shutdown... \r\n\r\n");
+    FlushBuffer();
 }
 
 // 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
@@ -279,8 +274,7 @@
         
         // If target returned
         if (count && (pixy.blocks[0].signature == TARGET_DECIMAL)) {
-            missingCount = 0;
-            height = pixy.blocks[0].height; //use this for now
+            height = pixy.blocks[0].height;
             x = pixy.blocks[0].x;
             
             speed = heightPI.Run(HEIGHT_SETPOINT-height, SPEED_MAX);
@@ -288,25 +282,10 @@
 
             // Give setpoints to MotorThread
             osMutexWait(motorMutex, osWaitForever); 
-            if (speed >= 0) {
-                leftMotor = speed - steering;
-                rightMotor = speed + steering;
-            } else {
-                leftMotor = speed - steering;
-                rightMotor = speed + steering;
-            }
+            leftMotor = speed - steering;
+            rightMotor = speed + steering;
             osMutexRelease(motorMutex); 
-        } else {
-            // When the robot can't see the target spin in last known direction
-            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++;    
-            }
-        }
+        } 
         
         if (isTunning && (navigationSample < NAVIGATION_SAMPLES)) {
             speedResponse[navigationSample] = speed; 
@@ -319,11 +298,11 @@
 void MotorThread(void const *argument) {
     float leftSet, rightSet, angVel;
     float timeOnLeft, timeOnRight;
+    short loop;
     short dP, dt;
 
     while (1) {
         osSignalWait(motorSignal, osWaitForever); // Go to sleep until motor signal is set high by ISR
-        led2= !led2; // Alive status - led2 toggles each time MotorControlThread is signaled.
         
         // Get setpoints from navigation
         osMutexWait(motorMutex, osWaitForever); 
@@ -331,21 +310,22 @@
         rightSet = rightMotor;
         osMutexRelease(motorMutex); 
         
+        // Run PI control on right motor
+        dP = deSpi.write(0);
+        dt = deSpi.write(0); 
+        angVel = RadsPerSec(dP, dt); // motors have opposite orientations
+        timeOnRight = rightMotorPI.Run(rightSet-angVel, PWM_PERIOD/2);
+        
         // Run PI control on left motor
         dP = deSpi.write(0);
         dt = deSpi.write(0); 
-        angVel = QE2RadsPerSec(dP, dt);
-        timeOnLeft = leftMotorPI.Run(leftSet-angVel, PWM_PERIOD);
+        angVel = -RadsPerSec(dP, dt);
+        timeOnLeft = leftMotorPI.Run(leftSet-angVel, PWM_PERIOD/2);
         
-        // Run PI control on right motor
-        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 
-        if (timeOnLeft >= 0) leftDir = 1;   
-        else leftDir = 0;
+        if (timeOnLeft >= 0) leftDir = 0;   
+        else leftDir = 1;
         
         if (timeOnRight >= 0) rightDir = 0;   
         else rightDir = 1;
@@ -358,6 +338,10 @@
             rightMotorResponse[motorSample] = timeOnRight;
             motorSample++;
         } 
+        
+        #ifdef DEBUG
+        loop = (loop < 1/MOTOR_PERIOD)? loop + 1 : 0;       
+        #endif
     } 
 }
 
@@ -365,9 +349,9 @@
 // Interrupt Service Routines
 // ----------------------------------------------------------------
 
-// TODO: Shutdown system
 void WatchdogISR(void const *n) {
     led3=1; // Activated when the watchdog timer times out
+    ShutDown();
 }
 
 void MotorISR(void) {
@@ -376,10 +360,4 @@
 
 void NavigationISR(void) {
     osSignalSet(navigationId,0x1); // Activate the signal, SensorControl, with each periodic timer interrupt
-}
-    
-// TODO: Shutdown system
-void CollisionISR(void) {
-    led4 = 1; // Activated on collision
-    
 }
\ No newline at end of file