Revision 11:521c3e8e6491, committed 2013-03-15

Comitter:

LtBarbershop

Date:

Fri Mar 15 21:33:02 2013 +0000

Parent:

10:a3ecedc8d9d7

Child:

12:3058f9fb09eb

Commit message:

March 15, Completely bluetooth controlled, menu (debug, wasd, IR value), ramp function.

Changed in this revision

--- a/main.cpp	Tue Mar 12 19:40:04 2013 +0000
+++ b/main.cpp	Fri Mar 15 21:33:02 2013 +0000
@@ -30,7 +30,9 @@
 void GetSpeeds();
 void SetLeftMotorSpeed(float u);
 void SetRightMotorSpeed(float u);
-// void IRChecker();
+void DisplayMenu();
+void Ramp(float speed, unsigned int time, unsigned short motor);
+void IRChecker();
 void BTInit();
 
 // Global variables
@@ -48,6 +50,7 @@
 float RecX[100]; // Record Integrator Output
 float RecE[100]; // Record Error
 int RecCount = 0; // Record Counter
+unsigned short action;
 Mutex Var_Lock;
 
 // global variables to eventually be removed
@@ -106,6 +109,124 @@
 // ******** Main Thread ********
 int main() 
 {    
+    char c;
+    
+    InitializeSystem();
+    BTInit();
+    BtS.printf("\r\n --- Robot Initialization Complete --- \r\n");  
+    DisplayMenu();
+    
+    //BtS.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
+    while(1)
+    {        
+        if(BtS.readable()) 
+        {
+            c = BtS.getc();
+            
+            // quit
+            if (c == 'q')
+            {
+                action = 0;
+                Ramp(0, 2000, 0);
+                DisplayMenu();
+                continue;
+            }
+            
+            if (action == 0)
+            {
+                // choose a menu selection
+                switch(c)
+                {
+                    case 'd':
+                        action = 1;
+                        break;
+                    case 'w':
+                        action = 2;
+                        break;
+                    case '0':
+                        action = 3;
+                        break;
+                    case 'q':
+                        action = 0;
+                        break;
+                    default:
+                        BtS.printf("\n\r CCommand not recognized \n\r");
+                        action = 0;
+                        break;
+                }
+                continue;
+            }
+            
+            
+            if (action == 1)
+            {
+                // keyboard input to drive robot using wasd 
+                switch(c)
+                {
+                    case('w'):
+                        userSetL = userSetL + 0.05;
+                        userSetR = userSetR + 0.05;
+                        break;
+                    case('s'):
+                        userSetL = userSetL - 0.05;
+                        userSetR = userSetR - 0.05;
+                        break;
+                    case('a'):
+                        userSetL = userSetL - 0.025;
+                        userSetR = userSetR + 0.025;
+                        break;
+                    case('d'):
+                        userSetL = userSetL + 0.025;
+                        userSetR = userSetR - 0.025;
+                        break;               
+                }
+            }
+            
+            if (action == 2)
+            {
+                // keyboard input to wall following
+            }
+            if (action == 3)
+            {
+                // keyboard input to debug mode
+                float newSpeed;
+                BtS.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
+                BtS.scanf("%f", &newSpeed);
+                
+                BtS.printf("%f", newSpeed);
+                Ramp(newSpeed, 1000, 0);
+                //userSetR = userSetL;
+            }
+            
+
+            
+        }// close if(BtS.readable())
+        
+        
+        
+        if (action == 2)
+        {
+            IRChecker();
+            action = 0;
+        }
+        if (action == 3)
+        {
+            // display debug messages
+            
+            //Var_Lock.lock();
+            BtS.printf("Pos. L: %d R: %d \n\r", dPositionLeft, dPositionRight);
+            BtS.printf("Time L: %d R: %d \n\r", dTimeLeft, dTimeRight);
+            BtS.printf("fbs  L: %f R: %f \n\r", fbSpeedL, fbSpeedR);
+            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 \r\n\n", cSetL, cSetR);
+            //Var_Lock.unlock();
+            
+            Thread::wait(2000);
+        }
+          
+
+/*
     InitializeSystem();
     BTInit();
     BtS.printf("\r\n --- Robot Initialization Complete --- \r\n");    
@@ -151,23 +272,23 @@
         char c;
     
         //Var_Lock.lock();
-        pc.printf("Pos. L: %d R: %d \n\r", dPositionLeft, dPositionRight);
-        pc.printf("Time L: %d R: %d \n\r", dTimeLeft, dTimeRight);
-        pc.printf("fbs  L: %f R: %f \n\r", fbSpeedL, fbSpeedR);
-        pc.printf("e    L: %f R: %f \r\n", eL, eR);
-        pc.printf("Ae   L: %f R: %f \n\r", aeL, aeR);
-        pc.printf("cSP  L: %f R: %f \r\n\n", cSetL, cSetR);
+        BtS.printf("Pos. L: %d R: %d \n\r", dPositionLeft, dPositionRight);
+        BtS.printf("Time L: %d R: %d \n\r", dTimeLeft, dTimeRight);
+        BtS.printf("fbs  L: %f R: %f \n\r", fbSpeedL, fbSpeedR);
+        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 \r\n\n", cSetL, cSetR);
         //Var_Lock.unlock();
         
-        if (pc.readable()){
-            x=pc.getc();
-            pc.putc(x); //Echo keyboard entry
+        if (BtS.readable()){
+            x=BtS.getc();
+            BtS.putc(x); //Echo keyboard entry
             osTimerStart(OneShot, 2000); // Set the watchdog timer interrupt to 2s.
                
             } 
-        if(pc.readable()) 
+        if(BtS.readable()) 
         {
-            c = pc.getc();
+            c = BtS.getc();
             if (c == 'r')
             {
                 userSetL = 0.2;
@@ -176,11 +297,11 @@
             {
                 if (RecCount == 100)
                 {
-                    pc.printf("\n\n\rRecV    RecU    RecX    RecE \n\r");
+                    BtS.printf("\n\n\rRecV    RecU    RecX    RecE \n\r");
                     int i = 0;
                     for (i = 0; i < 100; i++)
                     {
-                        pc.printf("%f, %f, %f, %f \n\r", RecV[i], RecU[i], RecX[i], RecE[i]);
+                        BtS.printf("%f, %f, %f, %f \n\r", RecV[i], RecU[i], RecX[i], RecE[i]);
                     }
                 }    
             
@@ -188,10 +309,10 @@
             
             if (c == 'n')
             {
-                pc.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
-                pc.scanf("%f", &userSetL);
+                BtS.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
+                BtS.scanf("%f", &userSetL);
                 
-                pc.printf("%f", userSetL);
+                BtS.printf("%f", userSetL);
                 userSetR = userSetL;
             }    
         }
@@ -296,7 +417,7 @@
         Update PWM on-time register with abs(u);
         Update the DIR pin on the LMD18200 with the sign of u.
         */
-        if (userSetL == 0.2f)
+        /*if (userSetL == 0.8f)
         {
             if (RecCount < 100)
             {
@@ -310,8 +431,8 @@
             {
                 userSetL = 0;
             }
-           
-        }
+          
+        }*/
         SetLeftMotorSpeed(cSetL);
         SetRightMotorSpeed(cSetR);
     } 
@@ -331,7 +452,7 @@
 void Watchdog(void const *n) 
 {
     led3=1;
-    pc.printf("\n\r Watchdog Timeout! Oh Shit!\n\r");
+    BtS.printf("\n\r Watchdog Timeout! Oh Shit!\n\r");
 }
 
 // ******** Period Timer Interrupt Handler ********
@@ -518,21 +639,32 @@
         fbSpeedR = -1;
     }
 } 
-/*
-void IRChecker();
+
+void IRChecker()
 {
     float IRF, IRR;
+    char z;
     // Read Sensors
-    IRF = IRFront.read();
-    IRR = IRRear.read();
-    // Voltage Corresponding to nominal distance
+    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;
     
     // Ensure Robot isn't closer than 10cm from wall (reads no voltage)
     
-    // Compare to nominal voltage and adjust
+    /* Compare to nominal voltage and adjust
     if (IRF > Nominal && IRR > Nominal)
     {
         SetR = SetR - Turn;
@@ -563,11 +695,60 @@
         SetRightMotorSpeed(SetR);
         SetLeftMotorSpeed(SetL);
     }
-   
-} */
+  */ 
+}
 
 void BTInit()
 {
             BtS.printf("AT+BTCANCEL\r\n");
             BtS.printf("AT+BTSCAN\r\n");
+}
+
+void DisplayMenu()
+{
+    BtS.printf("\r\n --- Robot Initialization Complete --- \r\n\n");
+    BtS.printf(" Press the corresponding key to do something:\r\n");
+    BtS.printf("| Key | Action\n\r");
+    BtS.printf("|-----|-------\n\r");
+    BtS.printf("|  d  | Drive the robot using wasd keys\n\r");
+    BtS.printf("|  w  | Robot performs wall following\n\r");
+    BtS.printf("|  0  | Debug mode\n\r");
+    BtS.printf("|  q  | Quit current action, stop the robot, and return to this menu\n\r\n");
+    
+}
+
+void Ramp(float speed, unsigned int time, unsigned short motor)
+{
+    const unsigned short steps = 20;
+    float changeL = (speed - userSetL)/steps;
+    float changeR = (speed - userSetR)/steps;
+    unsigned short i;
+    // calculate wait time (we wont worry too much about rounding errors)
+    unsigned int waitTime = time/steps;
+    
+    for (i = 0; i < steps; i++)
+    {
+        //Thread::wait(200);
+        Thread::wait(waitTime);
+        
+        if (motor == 0)
+        {
+            // change both speeds
+            userSetL += changeL;
+            userSetR += changeR;
+            continue;
+        }
+        if (motor == 1)
+        {
+            userSetL += changeL;
+            continue;
+        }
+        if (motor == 2)
+        {
+            userSetR += changeR;
+        }
+        
+        
+    }
+    
 }
\ No newline at end of file