Revision 12:3058f9fb09eb, committed 2013-03-17

Comitter:

IanTheMBEDMaster

Date:

Sun Mar 17 18:44:47 2013 +0000

Parent:

11:521c3e8e6491

Child:

13:aaac0105a486

Commit message:

Cleaned up some code, the IR input can now be seen in debug mode. An emergency stop condition has now been added as well.

Changed in this revision

--- a/main.cpp	Fri Mar 15 21:33:02 2013 +0000
+++ b/main.cpp	Sun Mar 17 18:44:47 2013 +0000
@@ -73,7 +73,7 @@
 SCK Quad Enc     7|-|
 SPI Start Quad E 8|-|
 SPI Reset Quad E 9|-|
-
+Emergency Stop  10|-|
 Bluetooth tx    13|-|28
 Bluetooth rx    14|-|27
                 15|-|26 Brake, Left Motor, M1
@@ -100,7 +100,7 @@
 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
 DigitalOut SpiStart(p8); // Places SPI interace on the DE0 FPGA into control mode
-InterruptIn Bumper(p10);  // External interrupt pin
+InterruptIn EmerStop(p10);  // External interrupt pin
 AnalogIn IRFront(p19); // Front IR Ranger Input
 AnalogIn IRRear(p20); // Rear IR Ranger Input
 Ticker PeriodicInt;                 
@@ -112,7 +112,6 @@
     char c;
     
     InitializeSystem();
-    BTInit();
     BtS.printf("\r\n --- Robot Initialization Complete --- \r\n");  
     DisplayMenu();
     
@@ -127,7 +126,7 @@
             if (c == 'q')
             {
                 action = 0;
-                Ramp(0, 2000, 0);
+                Ramp(0, 1000, 0);
                 DisplayMenu();
                 continue;
             }
@@ -139,6 +138,7 @@
                 {
                     case 'd':
                         action = 1;
+                        BtS.printf("\n\rTap w-a-s-d keys for differential speed control\r\nPress 'q' to quit \r\n");
                         break;
                     case 'w':
                         action = 2;
@@ -150,14 +150,13 @@
                         action = 0;
                         break;
                     default:
-                        BtS.printf("\n\r CCommand not recognized \n\r");
+                        BtS.printf("\n\r Command not recognized \n\r");
                         action = 0;
                         break;
                 }
                 continue;
             }
             
-            
             if (action == 1)
             {
                 // keyboard input to drive robot using wasd 
@@ -180,6 +179,7 @@
                         userSetR = userSetR - 0.025;
                         break;               
                 }
+                continue;
             }
             
             if (action == 2)
@@ -196,14 +196,10 @@
                 BtS.printf("%f", newSpeed);
                 Ramp(newSpeed, 1000, 0);
                 //userSetR = userSetL;
-            }
-            
-
-            
+            }            
         }// close if(BtS.readable())
         
         
-        
         if (action == 2)
         {
             IRChecker();
@@ -213,14 +209,19 @@
         {
             // display debug messages
             
-            //Var_Lock.lock();
+            float IRF, IRR;
+            IRF = IRFront.read();
+            IRR = IRRear.read();
+                  
+            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();
+            BtS.printf("cSP  L: %f R: %f \n\r", cSetL, cSetR);
+            BtS.printf("IR   F: %f R: %f \n\r\n", IRF, IRR);
+            Var_Lock.unlock();
             
             Thread::wait(2000);
         }
@@ -237,55 +238,10 @@
     {
         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();
-        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 (BtS.readable()){
-            x=BtS.getc();
-            BtS.putc(x); //Echo keyboard entry
-            osTimerStart(OneShot, 2000); // Set the watchdog timer interrupt to 2s.
-               
-            } 
+
         if(BtS.readable()) 
         {
             c = BtS.getc();
@@ -332,8 +288,8 @@
         float prevu1, prevu2;
         //float eL = 0;
         //float eR = 0;
-        const unsigned short maxError = 1000;
-        const unsigned short maxAcc = 10000;
+        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;
@@ -347,11 +303,8 @@
         // calculate error
         eL = userSetL - fbSpeedL;
         eR = userSetR - fbSpeedR;
-        //eL = -eL;
-        //eR = -eR;
         
-        // prevent overflow / bound the error
-        /*
+        // prevent overflow / bound the error    
         if (eL > maxError)
         {
             eL = maxError;
@@ -367,8 +320,7 @@
         if (eR < -maxError);
         {
             eR = -maxError;
-        }
-        */   
+        } 
         
         // accumulated error (integration)
         if (prevu1 < 1 && prevu1 > -1)
@@ -381,7 +333,6 @@
         }
         
         // bound the accumulatd error
-        /*
         if (aeL > maxAcc)
         {
             aeL = maxAcc;
@@ -398,7 +349,6 @@
         {
             aeR = -maxAcc;
         }
-        */
         
         u1 = Kp*eL + Ki*aeL;
         u2 = Kp*eR + Ki*aeR;
@@ -406,17 +356,7 @@
         cSetL = userSetL + u1;
         cSetR = userSetR + u2;
         
-        //u1 = -u1;
-        //u2 = -u2;
-        // Is signaled by a periodic timer interrupt handler
-        /*
-        Read incremental position, dPosition, and time interval from the QEI.
-        e = Setpoint – dPosition // e is the velocity error
-        xState = xState + e;    // x is the Euler approximation to the integral of e.
-        u = Kp*e + Ki*xState;       // u is the control signal
-        Update PWM on-time register with abs(u);
-        Update the DIR pin on the LMD18200 with the sign of u.
-        */
+        // data recording code
         /*if (userSetL == 0.8f)
         {
             if (RecCount < 100)
@@ -433,6 +373,7 @@
             }
           
         }*/
+        
         SetLeftMotorSpeed(cSetL);
         SetRightMotorSpeed(cSetR);
     } 
@@ -467,13 +408,14 @@
     osSignalSet(ExtCollision,0x1);
 }
 
+
 // --- Initialization Functions
 void InitializeSystem()
 {
     led3=0;
     led4=0; 
     
-    Bumper.rise(&ExtCollisionISR); // Atach the address of the interrupt handler to the rising edge of Bumper
+    EmerStop.fall(&ExtCollisionISR); // Atach the address of the interrupt handler to the rising edge of Bumper
     
     // Start execution of the Threads
     PiControl = osThreadCreate(osThread(PiControlThread), NULL);
@@ -483,6 +425,7 @@
     
     InitializePWM();
     InitializeEncoder();
+    BTInit();
 }
 
 void InitializePWM()
@@ -504,6 +447,11 @@
                             // starting at base address 0 within the peripheral.
 }
 
+void BTInit()
+{
+    BtS.printf("AT+BTCANCEL\r\n");
+    BtS.printf("AT+BTSCAN\r\n");
+}
 
 // --- Other Functions
 void SetLeftMotorSpeed(float u)
@@ -584,37 +532,13 @@
     wait_us(5);
     SpiStart = 0;
     DE0.write(0x8004);
+    
     // read in 4 16-bit words
     Var_Lock.lock();
     dPositionLeft = DE0.write(Dummy);   // Read QEI-0 position register 
     dTimeLeft = DE0.write(Dummy);       // Read QE-0 time interval register
     dPositionRight = DE0.write(Dummy);  // Read QEI-1 position register 
     dTimeRight = DE0.write(Dummy);      // Read QEI-1 time interval register
-    
-    // figure out which direction the motor is turning
-    /*
-    if (dPositionLeft > 32767)
-    {
-        // turning backwards
-        *leftSpeed = -(65535 - dPositionLeft)/leftMaxPos;
-    }
-    else
-    {
-        // turning forwards
-        *leftSpeed = dPositionLeft/leftMaxPos;
-    }
-    
-    if (dPositionRight > 32767)
-    {
-        // turning backwards
-        *rightSpeed = -(65535 - dPositionRight)/rightMaxPos;
-    }
-    else
-    {
-        // turning forwards
-        *rightSpeed = dPositionRight/rightMaxPos;
-    }
-    */
     Var_Lock.unlock();
     
     // calcspeed
@@ -698,23 +622,15 @@
   */ 
 }
 
-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("\r\n\nPress the corresponding key to do something:\r\n");
     BtS.printf("| Key | Action\n\r");
-    BtS.printf("|-----|-------\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");
-    
+    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)
@@ -746,9 +662,6 @@
         if (motor == 2)
         {
             userSetR += changeR;
-        }
-        
-        
-    }
-    
+        }  
+    }   
 }
\ No newline at end of file