Diff: main.cpp

Revision:

3:9a39e487b724

Parent:

2:1c5cc180812d

Child:

4:03bf5bdca9fb

--- a/main.cpp	Fri Feb 15 18:46:11 2013 +0000
+++ b/main.cpp	Fri Feb 15 21:11:34 2013 +0000
@@ -27,6 +27,7 @@
 // Global variables for interrupt handler
 float u1;
 float u2;
+int dPositionRight, dTimeRight, dPositionLeft, dTimeLeft;
 
 // --- Processes and threads
 int32_t SignalPi, SignalWdt, SignalExtCollision;
@@ -45,12 +46,12 @@
 
 Bluetooth tx    13|-|28
 Bluetooth rx    14|-|27
-                15|-|26 Brake, Right Motor, M2
-                16|-|25 Dir, Right Motor, M2
-                17|-|24 PWM, Right Motor, M2
-                18|-|23 Brake, Left Motor, M1
-                19|-|22 Dir, Left Motor, M1
-                20|-|21 PWM, Left Motor, M1
+                15|-|26 Brake, Right Motor, M1
+                16|-|25 Dir, Right Motor, M1
+                17|-|24 PWM, Right Motor, M1
+                18|-|23 Brake, Left Motor, M2
+                19|-|22 Dir, Left Motor, M2
+                20|-|21 PWM, Left Motor, M2
 */
 
 // --- IO Port Configuration
@@ -119,18 +120,11 @@
             //else if(x=='a') ...
             //else if(x=='d') ... 
             */
-                if (u1 > 1)
-                {
-                    u1 = 1;
-                }
-                
-                if (u1 < -1)
-                {
-                    u1 = -1;
-                }
         }
             
         Thread::wait(500); // Wait 500 ms
+        
+
     }
 }
 
@@ -146,8 +140,7 @@
         ReadEncoder();
         
         SetLeftMotorSpeed(u1);
-        SetRightMotorSpeed(u2);
-
+        SetRightMotorSpeed(u1);
     } 
 }
 
@@ -165,6 +158,7 @@
 void Watchdog(void const *n) 
 {
     led3=1;
+    pc.printf("\n\r Watchdog Timeout! Oh Shit!\n\r");
 }
 
 // ******** Period Timer Interrupt Handler ********
@@ -191,7 +185,7 @@
     PiControl = osThreadCreate(osThread(PiControlThread), NULL);
     ExtCollision = osThreadCreate(osThread(ExtCollisionThread), NULL);
     osTimerId OneShot = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0);
-    PeriodicInt.attach(&PiControllerISR, .02); // Specify address of the TimerISR (Ticker) function and the interval between interrupts
+    PeriodicInt.attach(&PiControllerISR, 1); // Specify address of the TimerISR (Ticker) function and the interval between interrupts
     
     InitializeEncoder();
 }
@@ -221,6 +215,18 @@
     float d;
     float onTime;
     
+    // bound the input
+    if (u > 1)
+    {
+        u = 1;
+    }
+                
+    if (u < -1)
+    {
+        u = -1;
+    }
+    
+    // calculate duty cycle timing
     T = PWMPeriod;
     d = abs(u);
     onTime = d * T; 
@@ -244,6 +250,18 @@
     float d;
     float onTime;
     
+    // bound the input
+    if (u > 1)
+    {
+        u = 1;
+    }
+                
+    if (u < -1)
+    {
+        u = -1;
+    }
+    
+    // calculate duty cycle timing
     T = PWMPeriod;
     d = abs(u);
     onTime = d * T; 
@@ -263,7 +281,7 @@
 
 void ReadEncoder()
 {
-    int dPositionRight, dTimeRight, dPositionLeft, dTimeLeft;
+    //int dPositionRight, dTimeRight, dPositionLeft, dTimeLeft;
     
     // May be executed in a loop
     dPositionRight = DE0.write(Dummy); // Read QEI-0 position register 
@@ -272,8 +290,10 @@
     dTimeLeft = DE0.write(Dummy);      // Read QEI-1 time interval register
     
     // simply write out the results for now
+    
     pc.printf("Left Position: %d \n\r", dPositionLeft);
     pc.printf("Left Time: %d \n\r", dTimeLeft);
     pc.printf("Right Position: %d \n\r", dPositionRight);
     pc.printf("Right Time: %d \n\n\r", dTimeRight);
+    
 }  
\ No newline at end of file