Diff: robot.cpp

Revision:

8:b0478286ad21

Parent:

7:5ef312aa2678

Child:

9:62fbb69b612c

--- a/robot.cpp	Fri Mar 25 19:34:16 2016 +0000
+++ b/robot.cpp	Fri Mar 25 20:05:04 2016 +0000
@@ -20,7 +20,7 @@
 osTimerDef(Wdtimer, WatchdogISR);
 int32_t motorSignal, navigationSignal;
 Ticker motorPeriodicInt;  
-Ticker sensorPeriodicInt;   
+Ticker navigationPeriodicInt;   
 
 // Mutex to protect left and right motor setpoints
 osMutexId motorMutex;
@@ -28,26 +28,52 @@
 
 // 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;
+bool isInitialized;
 
 // Functions
 // ----------------------------------------------------------------
 void AutoTrack() { 
+    RunMotor();
+    RunNavigation();
     
-    leftPwm.period(PWM_PERIOD);
-    leftPwm.pulsewidth(0);
-    rightPwm.period(PWM_PERIOD);
-    rightPwm.pulsewidth(0);
+    while (1) {
+        
+        ResetWatchDog();
+
+        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);
+        
+        Thread::wait(500); // Go to sleep for 500 ms
+    }
+}
 
+void RunMotor(void) {
+    // TODO: Optimize interrupt time for motor... Currently too fast
+    InitializeRobot();
+    motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD);
+}
+
+void RunNavigation(void) {
+    InitializeRobot();
+    navigationPeriodicInt.attach(&NavigationISR, NAVIGATION_PERIOD);
+}
+
+void ManualControl(void) {
+
+}
+
+// Setup the robot threads and data if needed
+void InitializeRobot(void) {
+    if (isInitialized) return;
+    
     motorMutex = osMutexCreate(osMutex(motorMutex));
-
+    
     bumper.rise(&CollisionISR); // Attach interrupt handler to rising edge of Bumper
     
-    // Start execution of the threads MotorThread, and NavigationThread:
+    // Create MotorThread and NavigationThread:
     navigationId = osThreadCreate(osThread(NavigationThread), NULL);
     motorId = osThreadCreate(osThread(MotorThread), NULL);
  
@@ -61,30 +87,14 @@
     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
-    
-    // Specify periodic ISRs and their intervals in seconds
-    // TODO: Optimize interrupt time for motor... Currently too fast
-    motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD);
-    sensorPeriodicInt.attach(&NavigationISR, NAVIGATION_PERIOD);
+    pc.printf("\n\rDevice Id: %d \r\n\r\n", deSpi.write(0x8004)); // Read count & time for both motors
     
-    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);
-        
-        Thread::wait(500); // Go to sleep for 500 ms
-    }
+    isInitialized = true;
 }
 
-void ManualControl(void) {
-
+// Timesout in 2 seconds
+void ResetWatchDog(void) {
+    osTimerStart(oneShotId, 2000);
 }
 
 
@@ -108,13 +118,8 @@
 
             // 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); 
         }
     } 
@@ -122,7 +127,8 @@
 
 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 +147,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