Diff: main.cpp

Revision:

13:aaac0105a486

Parent:

12:3058f9fb09eb

Child:

14:669f2f1566b0

--- a/main.cpp	Sun Mar 17 18:44:47 2013 +0000
+++ b/main.cpp	Mon Mar 18 02:28:12 2013 +0000
@@ -5,6 +5,8 @@
 #include "mbed.h"
 #include "rtos.h"
 
+extern "C" void mbed_reset();
+
 // --- Constants
 #define Dummy 0
 
@@ -51,6 +53,11 @@
 float RecE[100]; // Record Error
 int RecCount = 0; // Record Counter
 unsigned short action;
+float DF = 0;
+float DR = 0;
+float TSpeedL = 0;
+float TSpeedR = 0;
+float Turn = 0.01;
 Mutex Var_Lock;
 
 // global variables to eventually be removed
@@ -141,6 +148,12 @@
                         BtS.printf("\n\rTap w-a-s-d keys for differential speed control\r\nPress 'q' to quit \r\n");
                         break;
                     case 'w':
+                        BtS.printf("\n\r Welcome to Wall Following, use w-s to control robot speed");
+                        BtS.printf("\n\r Use a-d to increase/decrease turning radius");
+                        userSetL = 0.1;
+                        userSetR = 0.1;
+                        TSpeedL = 0.1;
+                        TSpeedR = 0.1;
                         action = 2;
                         break;
                     case '0':
@@ -185,6 +198,23 @@
             if (action == 2)
             {
                 // keyboard input to wall following
+                switch(c)
+                {
+                    case('w'):
+                        TSpeedL = TSpeedL + 0.05;
+                        TSpeedR = TSpeedR + 0.05;
+                        break;
+                    case('s'):
+                        TSpeedL = TSpeedL - 0.05;
+                        TSpeedR = TSpeedR - 0.05;
+                        break;
+                    case('a'):
+                        Turn = Turn + 0.005;
+                        break;
+                    case('d'):
+                        Turn = Turn - 0.005;
+                        break;               
+                }
             }
             if (action == 3)
             {
@@ -198,13 +228,14 @@
                 //userSetR = userSetL;
             }            
         }// close if(BtS.readable())
-        
-        
         if (action == 2)
         {
-            IRChecker();
-            action = 0;
+            Var_Lock.lock();
+            BtS.printf("IR   F: %f R: %f \n\r", DF, DR);
+            Var_Lock.unlock();
+            Thread::wait(1500);
         }
+        
         if (action == 3)
         {
             // display debug messages
@@ -220,9 +251,8 @@
             BtS.printf("e    L: %f R: %f \r\n", eL, eR);
             BtS.printf("Ae   L: %f R: %f \n\r", aeL, aeR);
             BtS.printf("cSP  L: %f R: %f \n\r", cSetL, cSetR);
-            BtS.printf("IR   F: %f R: %f \n\r\n", IRF, IRR);
+            BtS.printf("IR   F: %f R: %f \n\r", IRF, IRR);
             Var_Lock.unlock();
-            
             Thread::wait(2000);
         }
           
@@ -280,6 +310,9 @@
 // ******** Control Thread ********
 void PiControlThread(void const *argument) 
 {
+    float maxError = 1.0f;
+    float maxAcc = 10.0f;
+    
     while (1) 
     {
         osSignalWait(SignalPi, osWaitForever); 
@@ -288,12 +321,15 @@
         float prevu1, prevu2;
         //float eL = 0;
         //float eR = 0;
-        const unsigned short maxError = 1;
-        const unsigned short maxAcc = 10;
+
         // Kp = 0.1, Ki = 0.5
         const float Kp = 0.1f;
         const float Ki = 0.5f;
         
+        if (action == 2)
+        {
+            IRChecker();
+        }
         prevu1 = u1;
         prevu2 = u2;
         
@@ -304,14 +340,15 @@
         eL = userSetL - fbSpeedL;
         eR = userSetR - fbSpeedR;
         
+        /*
         // prevent overflow / bound the error    
-        if (eL > maxError)
+        if (eL > 1)
         {
-            eL = maxError;
+            eL = 1;
         }
-        if (eL < -maxError);
+        if (eL < -1);
         {
-            eL = -maxError;
+            eL = -1;
         }
         if (eR > maxError)
         {
@@ -321,7 +358,7 @@
         {
             eR = -maxError;
         } 
-        
+        */
         // accumulated error (integration)
         if (prevu1 < 1 && prevu1 > -1)
         {    
@@ -332,6 +369,7 @@
             aeR += eR;
         }
         
+        /*
         // bound the accumulatd error
         if (aeL > maxAcc)
         {
@@ -349,6 +387,7 @@
         {
             aeR = -maxAcc;
         }
+        */
         
         u1 = Kp*eL + Ki*aeL;
         u2 = Kp*eR + Ki*aeR;
@@ -385,7 +424,11 @@
     while (1) 
     {
         osSignalWait(SignalExtCollision, osWaitForever);
-        led4 = !led4;
+        userSetL = 0;
+        userSetR = 0;
+        SetLeftMotorSpeed(userSetL);
+        SetRightMotorSpeed(userSetR);
+        mbed_reset();
     }
 }
 
@@ -415,7 +458,7 @@
     led3=0;
     led4=0; 
     
-    EmerStop.fall(&ExtCollisionISR); // Atach the address of the interrupt handler to the rising edge of Bumper
+    EmerStop.rise(&ExtCollisionISR); // Atach the address of the interrupt handler to the rising edge of Bumper
     
     // Start execution of the Threads
     PiControl = osThreadCreate(osThread(PiControlThread), NULL);
@@ -567,59 +610,54 @@
 void IRChecker()
 {
     float IRF, IRR;
-    char z;
+    float aF = 22.6321; // 34.2911
+    float bF = -0.1721; // -0.2608
+    float aR = 22.6021; // 34.2456
+    float bR = -0.0376; // -0.0569
+    float Nominal = 25;
+    
     // Read Sensors
-    while (z !=  'q')
-    {
-        IRF = IRFront.read();
-        IRR = IRRear.read();
-        // Voltage Corresponding to nominal distance
-        BtS.printf("\n\rFront Sensor: %f", IRF);
-        BtS.printf("\n\rRear Sensor: %f\n\r", IRR);
-        if (BtS.readable())
-        {
-            z = BtS.getc();
-        }
-        Thread::wait(1000);
-    }
-    float Nominal = 3.0;
-    // Variable for turning robots
-    float Turn = 0.1;
+    IRF = 3.3*IRFront.read();
+    IRR = 3.3*IRRear.read();
+
     
-    // Ensure Robot isn't closer than 10cm from wall (reads no voltage)
+    // Calculate distance based on voltage
+    DF = aF/(IRF+bF);
+    DR = aR/(IRR+bR);
     
-    /* Compare to nominal voltage and adjust
-    if (IRF > Nominal && IRR > Nominal)
+    // Display Distances
+    // BtS.printf("IRD  F: %f R: %f\n\r\n", DF, DR);
+    
+    // Compare to nominal voltage and adjust
+    while(1)
     {
-        SetR = SetR - Turn;
-        SetL = SetL + Turn;
-        SetRightMotorSpeed(SetR);
-        SetLeftMotorSpeed(SetL);
-    }
-    if (IRF < Nominal && IRR < Nominal
-    {
-        SetR = SetR - Turn;
-        SetL = SetL + Turn;
-        SetRightMotorSpeed(SetR);
-        SetLeftMotorSpeed(SetL);
-    }
-    
-    // Compare rangers to each other
-    if (IRF > IRR) // IRF closer than IRR
-    {
-        SetR = SetR + Turn;
-        SetL = SetL - Turn;
-        SetRightMotorSpeed(SetR);
-        SetLeftMotorSpeed(SetL);
-    }
-    if (IRF < IRR) // IRF further than IRR
-    {
-        SetR = SetR - Turn;
-        SetL = SetL + Turn;
-        SetRightMotorSpeed(SetR);
-        SetLeftMotorSpeed(SetL);
-    }
-  */ 
+        if (DF > Nominal && DR > Nominal)
+        {
+            userSetR = TSpeedR + Turn;
+            userSetL = TSpeedL - Turn;
+            break;
+        }
+        if (DF < Nominal && DR < Nominal)
+        {
+            userSetR = TSpeedR - Turn;
+            userSetL = TSpeedL + Turn;
+            break;
+        }
+        
+        // Compare rangers to each other
+        if (DF > DR) // IRF further than IRR
+        {
+            userSetR = TSpeedR + Turn;
+            userSetL = TSpeedL - Turn;
+            break;
+        }
+        if (DF < DR) // IRF closer than IRR
+        {
+            userSetR = TSpeedR - Turn;
+            userSetL = TSpeedL + Turn;
+            break;
+        }
+    } 
 }
 
 void DisplayMenu()