Diff: main.cpp

Revision:

10:a3ecedc8d9d7

Parent:

9:0eb7b173d6c3

Child:

11:521c3e8e6491

--- a/main.cpp	Mon Mar 11 14:25:40 2013 +0000
+++ b/main.cpp	Tue Mar 12 19:40:04 2013 +0000
@@ -30,7 +30,8 @@
 void GetSpeeds();
 void SetLeftMotorSpeed(float u);
 void SetRightMotorSpeed(float u);
-void IRChecker();
+// void IRChecker();
+void BTInit();
 
 // Global variables
 float u1 = 0; 
@@ -91,7 +92,7 @@
 DigitalOut dirL(p25);
 DigitalOut brakeL(p26);
 PwmOut PwmL(p24);
-Serial BluetoothSerial(p13, p14); // (tx, rx) for PC serial channel
+Serial BtS(p13, p14); // (tx, rx) for PC serial channel
 Serial pc(USBTX, USBRX); // (tx, rx) for Parani/Promi Bluetooth serial channel
 SPI DE0(p5, p6, p7); // (mosi, miso, sclk) DE0 is the SPI channel with the DE0 FPGA 
 DigitalOut SpiReset(p9); // Reset for all devices within the slave SPI peripheral in the DE0 FPGA
@@ -106,13 +107,47 @@
 int main() 
 {    
     InitializeSystem();
-    
-    pc.printf("\r\n --- Robot Initialization Complete --- \r\n");    
+    BTInit();
+    BtS.printf("\r\n --- Robot Initialization Complete --- \r\n");    
     
-    BluetoothSerial.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
-    
+    BtS.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
+    char x;
     while(1)
     {
+        Thread::wait(20);
+    
+        if (BtS.readable())
+        {
+            x = BtS.getc();
+            
+            switch(x)
+            {
+                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;
+                case('b'):
+                    // Eventually ramp down to 0, RampDown();
+                    userSetL = 0;
+                    userSetR = 0;
+                    break;                
+            }
+            
+        }
+        
+        /*
         char c;
     
         //Var_Lock.lock();
@@ -124,12 +159,12 @@
         pc.printf("cSP  L: %f R: %f \r\n\n", cSetL, cSetR);
         //Var_Lock.unlock();
         
-        /*if (pc.readable()){
+        if (pc.readable()){
             x=pc.getc();
             pc.putc(x); //Echo keyboard entry
             osTimerStart(OneShot, 2000); // Set the watchdog timer interrupt to 2s.
                
-            }*/
+            } 
         if(pc.readable()) 
         {
             c = pc.getc();
@@ -161,7 +196,7 @@
             }    
         }
             
-        Thread::wait(2000); // Wait 2 seconds
+        Thread::wait(2000);   Wait 2 seconds */
     }
 }
 
@@ -224,9 +259,6 @@
             aeR += eR;
         }
         
-        //aeL += eL;
-        //aeR += eR;
-        
         // bound the accumulatd error
         /*
         if (aeL > maxAcc)
@@ -486,7 +518,7 @@
         fbSpeedR = -1;
     }
 } 
-
+/*
 void IRChecker();
 {
     float IRF, IRR;
@@ -532,4 +564,10 @@
         SetLeftMotorSpeed(SetL);
     }
    
+} */
+
+void BTInit()
+{
+            BtS.printf("AT+BTCANCEL\r\n");
+            BtS.printf("AT+BTSCAN\r\n");
 }
\ No newline at end of file