main.cpp

00001 // Robot Control Code
00002 // Tom Elliott and Ian Colwell
00003 
00004 /* TODO on a sunny day
00005 - reduce the amount of global variables
00006 - try increasing bluetooth buad rate
00007 */
00008 
00009 #include "mbed.h"
00010 #include "rtos.h"
00011 
00012 extern "C" void mbed_reset();
00013 
00014 // --- Constants
00015 #define Dummy 0
00016 
00017 //#define PWMPeriod 0.0005 // orignally 0.001 (gave a high pitched whine)
00018 const float PWMPeriod = 0.00005;
00019 // Control Update = 50ms (time should be 609/610) (if we want to change this we also have to change the way feedback speed is calculated)
00020 //#define ControlUpdate 0.05
00021 const float ControlUpdate = 0.04;
00022 #define EncoderTime 610
00023 
00024 
00025 // --- Function prototypes
00026 void PiControllerISR(void);
00027 void WdtFaultISR(void);
00028 void ExtCollisionISR(void);
00029 void PiControlThread(void const *argument);
00030 void ExtCollisionThread(void const *argument);
00031 void Watchdog(void const *n);
00032 void InitializeSystem();
00033 void InitializeEncoder();
00034 void InitializePWM();
00035 void PwmSetOut(float d, float T);
00036 void GetSpeeds();
00037 void SetLeftMotorSpeed(float u);
00038 void SetRightMotorSpeed(float u);
00039 void DisplayMenu();
00040 void Ramp(float speed, unsigned int time, unsigned short motor);
00041 void IRChecker();
00042 void BTInit();
00043 
00044 // Global variables
00045 float u1 = 0; 
00046 float u2 = 0;
00047 float cSetL = 0;
00048 float cSetR = 0;
00049 float userSetL = 0;
00050 float userSetR = 0;
00051 int startup = 0;
00052 float aeL = 0; 
00053 float aeR = 0;
00054 float RecV[200]; // Record Feedback Speed
00055 float RecU[200]; // Record Motor Input
00056 float RecX[200]; // Record Integrator Output
00057 float RecE[200]; // Record Error
00058 int RecCount = 0; // Record Counter
00059 unsigned short action;
00060 float DF = 0;
00061 float DR = 0;
00062 float TSpeedL = 0;
00063 float TSpeedR = 0;
00064 float Turn = 0.1;
00065 float aeW = 0;
00066 float eW = 0;
00067 float uW = 0; 
00068 float prevuW = 0;
00069 
00070 float Nominal = 35;
00071 float Kpos = 0.01;
00072 float Kor = 0.01;
00073 float KpW = 0.3;
00074 float KiW = 0.0;
00075 int autoSpeed = -1;
00076 
00077 Mutex Var_Lock;
00078 
00079 // global variables to eventually be removed
00080 short dPositionLeft, dTimeLeft, dPositionRight, dTimeRight;
00081 float fbSpeedL, fbSpeedR;
00082 float eL = 0;
00083 float eR = 0;
00084 
00085 // --- Processes and threads
00086 int32_t SignalPi, SignalWdt, SignalExtCollision;
00087 osThreadId PiControl,WdtFault,ExtCollision;
00088 osThreadDef(PiControlThread, osPriorityNormal, DEFAULT_STACK_SIZE);
00089 osThreadDef(ExtCollisionThread, osPriorityNormal, DEFAULT_STACK_SIZE);
00090 osTimerDef(Wdtimer, Watchdog);
00091 
00092 /* PIN-OUT
00093 
00094 MOSI Quad Enc    5|-|
00095 MISO Quad Enc    6|-|
00096 SCK Quad Enc     7|-|
00097 SPI Start Quad E 8|-|
00098 SPI Reset Quad E 9|-|
00099 Emergency Stop  10|-|
00100 Bluetooth tx    13|-|28
00101 Bluetooth rx    14|-|27
00102                 15|-|26 Brake, Left Motor, M1
00103                 16|-|25 Dir, Left Motor, M1
00104                 17|-|24 PWM, Left Motor, M1
00105                 18|-|23 Brake, Right Motor, M2
00106 Front IR        19|-|22 Dir, Right Motor, M2
00107 Rear  IR        20|-|21 PWM, Right Motor, M2
00108 */
00109 
00110 // --- IO Port Configuration
00111 DigitalOut led1(LED1);
00112 DigitalOut led2(LED2);
00113 DigitalOut led3(LED3);
00114 DigitalOut led4(LED4);
00115 DigitalOut dirR(p22);
00116 DigitalOut brakeR(p23);
00117 PwmOut PwmR(p21);
00118 DigitalOut dirL(p25);
00119 DigitalOut brakeL(p26);
00120 PwmOut PwmL(p24);
00121 Serial BtS(p13, p14); // (tx, rx) for PC serial channel
00122 Serial pc(USBTX, USBRX); // (tx, rx) for Parani/Promi Bluetooth serial channel
00123 SPI DE0(p5, p6, p7); // (mosi, miso, sclk) DE0 is the SPI channel with the DE0 FPGA 
00124 DigitalOut SpiReset(p9); // Reset for all devices within the slave SPI peripheral in the DE0 FPGA
00125 DigitalOut SpiStart(p8); // Places SPI interace on the DE0 FPGA into control mode
00126 //InterruptIn EmerStop(p10);  // External interrupt pin
00127 AnalogIn IRFront(p19); // Front IR Ranger Input
00128 AnalogIn IRRear(p20); // Rear IR Ranger Input
00129 Ticker PeriodicInt;                 
00130 DigitalIn EmergencyStop(p10);
00131 
00132 // ******** Main Thread ********
00133 int main() 
00134 {    
00135     char c;
00136     
00137     InitializeSystem();
00138     BtS.printf("\r\n --- Robot Initialization Complete --- \r\n");  
00139     pc.printf("\r\n --- Robot Initialization Complete --- \r\n"); 
00140     DisplayMenu();
00141     
00142     //BtS.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
00143     while(1)
00144     {        
00145         if(BtS.readable()) 
00146         {
00147             c = BtS.getc();
00148             
00149             // quit
00150             if (c == 'q')
00151             {
00152                 action = 0;
00153                 // erase errors
00154                 aeW = 0;
00155                 eW = 0;
00156                 aeL = 0;
00157                 aeR = 0;
00158                 Ramp(0, 800, 0);
00159                 DisplayMenu();
00160                 continue;
00161             }
00162             
00163             if (action == 0)
00164             {
00165                 // choose a menu selection
00166                 switch(c)
00167                 {
00168                     case 'd':
00169                         action = 1;
00170                         BtS.printf("\n\rTap w-a-s-d keys for differential speed control\r\nPress 'q' to quit \r\n");
00171                         break;
00172                     case 'w':
00173                         BtS.printf("\n\r Welcome to Wall Following, use w-s to control robot speed");
00174                         BtS.printf("\n\r Use a-d to increase/decrease turning radius");
00175                         BtS.printf("\n\r Use < and > to increase/decrease wall distance");
00176                         userSetL = 0.2;
00177                         userSetR = 0.2;
00178                         TSpeedL = 0.2;
00179                         TSpeedR = 0.2;
00180                         action = 2;
00181                         break;
00182                     case '0':
00183                         action = 3;
00184                         break;
00185                     case 'r':
00186                         action = 4;
00187                         userSetL = 0.2;
00188                         userSetR = 0.2;
00189                         break;
00190                     default:
00191                         BtS.printf("\n\r Command not recognized \n\r");
00192                         action = 0;
00193                         break;
00194                 }
00195                 continue;
00196             }
00197             
00198             if (action == 1)
00199             {
00200                 // keyboard input to drive robot using wasd 
00201                 switch(c)
00202                 {
00203                     case('w'):
00204                         userSetL = userSetL + 0.1;
00205                         userSetR = userSetR + 0.1;
00206                         break;
00207                     case('s'):
00208                         userSetL = userSetL - 0.1;
00209                         userSetR = userSetR - 0.1;
00210                         break;
00211                     case('a'):
00212                         userSetL = userSetL - 0.04;
00213                         userSetR = userSetR + 0.04;
00214                         break;
00215                     case('d'):
00216                         userSetL = userSetL + 0.04;
00217                         userSetR = userSetR - 0.04;
00218                         break;          
00219                     case('e'):
00220                         Ramp(0.5, 500, 0);
00221                         break;
00222                     case('r'):
00223                         Ramp(0, 500, 0);
00224                         break;
00225                 }
00226                 if (userSetL > 0.5)
00227                 {
00228                     userSetL = 0.5;
00229                 }
00230                 if (userSetL < -0.5)
00231                 {
00232                     userSetL = -0.5;
00233                 }
00234                 if (userSetR > 0.5)
00235                 {
00236                     userSetR = 0.5;
00237                 }
00238                 if (userSetR < -0.5)
00239                 {
00240                     userSetR = -0.5;
00241                 }
00242                 
00243                 continue;
00244             }
00245             
00246             if (action == 2)
00247             {
00248                 // keyboard input to wall following
00249                 switch(c)
00250                 {
00251                     case('w'):
00252                         TSpeedL = TSpeedL + 0.05;
00253                         TSpeedR = TSpeedR + 0.05;
00254                         break;
00255                     case('s'):
00256                         TSpeedL = TSpeedL - 0.05;
00257                         TSpeedR = TSpeedR - 0.05;
00258                         break;
00259                     case('a'):
00260                         Turn = Turn + 0.01;
00261                         break;
00262                     case('d'):
00263                         Turn = Turn - 0.01;
00264                         break;
00265                     case(','):
00266                         Nominal = Nominal - 2.5;
00267                         break;
00268                     case('.'):
00269                         Nominal = Nominal + 2.5;
00270                         break; 
00271                     case('g'):
00272                         autoSpeed = autoSpeed * -1;
00273                         break;
00274                     case('n'):
00275                         BtS.printf("\n\r Current constants: Ki %.3f:, Kp: %.3f, Kor: %.3f, Kpos: %.3f \n\r Select the constant you wish to change:", KiW, KpW, Kor, Kpos);    
00276                         char k;
00277                         float newConst;
00278                         while (1)
00279                         {
00280                             if (BtS.readable())
00281                             {
00282                                 k = BtS.getc();
00283                                 if (k == '1')
00284                                 {
00285                                     BtS.scanf("%f", &newConst);
00286                                     KiW = newConst;
00287                                     break;
00288                                 }
00289                                 if (k == '2')
00290                                 {
00291                                     BtS.scanf("%f", &newConst);
00292                                     KpW = newConst;
00293                                     break;
00294                                 }
00295                                 if (k == '3')
00296                                 {
00297                                     BtS.scanf("%f", &newConst);
00298                                     Kor = newConst;
00299                                     break;
00300                                 }
00301                                 if (k == '4')
00302                                 {
00303                                     BtS.scanf("%f", &newConst);
00304                                     Kpos = newConst;
00305                                     break;
00306                                 }
00307                                 
00308                                 printf("\n\r Pick a constant ya goof \n\r");
00309                             }
00310                         }
00311                 }
00312             }
00313             if (action == 3)
00314             {
00315                 // keyboard input to debug mode
00316                 float newSpeed;
00317                 BtS.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
00318                 BtS.scanf("%f", &newSpeed);
00319                 
00320                 BtS.printf("%f", newSpeed);
00321                 Ramp(newSpeed, 20, 0);
00322                 //userSetR = userSetL;
00323             }            
00324             
00325             if (action == 2 && c == 'r')
00326             {
00327                 if (RecCount == 200)
00328                 {
00329                     BtS.printf("\n\n\rRecV    RecU    RecX    RecE \n\r");
00330                     int i = 0;
00331                     for (i = 0; i < 200; i++)
00332                     {
00333                         BtS.printf("%f, %f, %f, %f \n\r", RecV[i], RecU[i], RecX[i], RecE[i]);
00334                     }
00335                 }
00336             }     
00337         }// close if(BtS.readable())
00338         if (action == 2)
00339         {
00340             // Wall Following
00341             //Var_Lock.lock();
00342             BtS.printf("IR F: %f cm R: %f cm \n\r", DF, DR);
00343             BtS.printf("Wall Error: %f \n\r", eW);
00344             BtS.printf("Acc Error: %f \n\r", aeW);
00345             BtS.printf("Diff. Setpoint: %f \n\r", uW);
00346             BtS.printf("Setpoint L: %f R: %f \n\n\r", userSetL, userSetR);
00347             //Var_Lock.unlock();
00348             Thread::wait(1000);
00349         }
00350         
00351         if (action == 3)
00352         {
00353             // Debug Mode
00354             
00355             float IRF, IRR;
00356             IRF = IRFront.read();
00357             IRR = IRRear.read();
00358             
00359             Var_Lock.lock();
00360             BtS.printf("Pos. L: %d R: %d \n\r", dPositionLeft, dPositionRight);
00361             BtS.printf("Time L: %d R: %d \n\r", dTimeLeft, dTimeRight);
00362             BtS.printf("fbs  L: %f R: %f \n\r", fbSpeedL, fbSpeedR);
00363             BtS.printf("e    L: %f R: %f \r\n", eL, eR);
00364             BtS.printf("Ae   L: %f R: %f \n\r", aeL, aeR);
00365             BtS.printf("cSP  L: %f R: %f \n\r", cSetL, cSetR);
00366             BtS.printf("IR   F: %f R: %f \n\r", IRF, IRR);
00367             Var_Lock.unlock();
00368             Thread::wait(2000);
00369         }          
00370     }
00371 }
00372 
00373 // ******** Control Thread ********
00374 void PiControlThread(void const *argument) 
00375 {
00376     float maxError = 1.0f;
00377     float maxAcc = 10.0f;
00378     
00379     while (1) 
00380     {
00381         // check for emergency stop
00382         if (EmergencyStop == 1)
00383         {
00384         userSetL = 0;
00385         userSetR = 0;
00386         SetLeftMotorSpeed(userSetL);
00387         SetRightMotorSpeed(userSetR);
00388         BtS.printf("\n\rEmergency Stop!!\n\r");
00389         Thread::wait(2000);
00390         }
00391         
00392         osSignalWait(SignalPi, osWaitForever); 
00393         led2= !led2; // Alive status
00394         
00395         float prevu1, prevu2;
00396 
00397         // Kp = 0.1, Ki = 0.5
00398         const float Kp = 0.5f;
00399         const float Ki = 0.8f;
00400         
00401         if (action == 2)
00402         {
00403             IRChecker();
00404         }
00405         prevu1 = u1;
00406         prevu2 = u2;
00407         
00408         // read encoder and calculate speed of both motors
00409         GetSpeeds();
00410         
00411         // calculate error
00412         eL = userSetL - fbSpeedL;
00413         eR = userSetR - fbSpeedR;
00414         
00415         /*
00416         // prevent overflow / bound the error    
00417         if (eL > 1)
00418         {
00419             eL = 1;
00420         }
00421         if (eL < -1);
00422         {
00423             eL = -1;
00424         }
00425         if (eR > maxError)
00426         {
00427             eR = maxError;
00428         }
00429         if (eR < -maxError);
00430         {
00431             eR = -maxError;
00432         } 
00433         */
00434         // accumulated error (integration)
00435         if (prevu1 < 1 && prevu1 > -1)
00436         {    
00437             aeL += eL;
00438         }
00439         if (prevu2 < 1 && prevu2 > -1)
00440         {
00441             aeR += eR;
00442         }
00443         
00444         /*
00445         // bound the accumulatd error
00446         if (aeL > maxAcc)
00447         {
00448             aeL = maxAcc;
00449         }
00450         if (aeL < -maxAcc)
00451         {
00452             aeL = -maxAcc;
00453         }
00454         if (aeR > maxAcc)
00455         {
00456             aeR = maxAcc;
00457         }
00458         if (aeR < -maxAcc)
00459         {
00460             aeR = -maxAcc;
00461         }
00462         */
00463         
00464         u1 = Kp*eL + Ki*aeL;
00465         u2 = Kp*eR + Ki*aeR;
00466         
00467         SetLeftMotorSpeed(u1);
00468         SetRightMotorSpeed(u2);
00469     } 
00470 }
00471 
00472 // ******** Collision Thread ********
00473 void ExtCollisionThread(void const *argument) 
00474 {
00475     while (1) 
00476     {
00477         osSignalWait(SignalExtCollision, osWaitForever);
00478         userSetL = 0;
00479         userSetR = 0;
00480         SetLeftMotorSpeed(userSetL);
00481         SetRightMotorSpeed(userSetR);
00482         mbed_reset();
00483     }
00484 }
00485 
00486 // ******** Watchdog Interrupt Handler ********
00487 void Watchdog(void const *n) 
00488 {
00489     led3=1;
00490     BtS.printf("\n\r Watchdog Timeout! Oh Shit!\n\r");
00491 }
00492 
00493 // ******** Period Timer Interrupt Handler ********
00494 void PiControllerISR(void)
00495 {
00496     osSignalSet(PiControl,0x1);
00497 }
00498     
00499 // ******** Collision Interrupt Handler ********
00500 void ExtCollisionISR(void) 
00501 {
00502     osSignalSet(ExtCollision,0x1);
00503 }
00504 
00505 
00506 // --- Initialization Functions
00507 void InitializeSystem()
00508 {
00509     led3=0;
00510     led4=0; 
00511     
00512     //EmerStop.rise(&ExtCollisionISR); // Atach the address of the interrupt handler to the rising edge of Bumper
00513     
00514     // Start execution of the Threads
00515     PiControl = osThreadCreate(osThread(PiControlThread), NULL);
00516     ExtCollision = osThreadCreate(osThread(ExtCollisionThread), NULL);
00517     osTimerId OneShot = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0);
00518     PeriodicInt.attach(&PiControllerISR, ControlUpdate); // Specify address of the TimerISR (Ticker) function and the interval between interrupts
00519     
00520     InitializePWM();
00521     InitializeEncoder();
00522     BTInit();
00523 }
00524 
00525 void InitializePWM()
00526 {
00527     PwmL.period(PWMPeriod);
00528     PwmR.period(PWMPeriod);
00529 }
00530 
00531 void InitializeEncoder()
00532 {
00533     // Initialization – to be executed once (normally)
00534     DE0.format(16,0);   // SPI format: 16-bit words, mode 0 protocol.
00535     DE0.frequency(1000000);
00536     SpiStart = 0;
00537     SpiReset = 1;
00538     wait_us(10);
00539     SpiReset = 0;
00540     DE0.write(0x8004);  // SPI slave control word to read (only) 4-word transactions
00541                             // starting at base address 0 within the peripheral.
00542 }
00543 
00544 void BTInit()
00545 {
00546     BtS.printf("AT+BTCANCEL\r\n");
00547     BtS.printf("AT+BTSCAN\r\n");
00548     
00549     // also send to the pc so we know whats going on
00550     pc.printf("AT+BTCANCEL\r\n");
00551     pc.printf("AT+BTSCAN\r\n");    
00552 }
00553 
00554 // --- Other Functions
00555 void SetLeftMotorSpeed(float u)
00556 {
00557     float T;
00558     float d;
00559     float onTime;
00560     float maxMotorSpeed = 0.5;
00561     
00562     // bound the input
00563     if (u > maxMotorSpeed)
00564     {
00565         u = maxMotorSpeed;
00566     }
00567                 
00568     if (u < -maxMotorSpeed)
00569     {
00570         u = -maxMotorSpeed;
00571     }
00572     
00573     // calculate duty cycle timing
00574     T = PWMPeriod;
00575     d = abs(u);
00576     onTime = d * T; 
00577 
00578     PwmL.pulsewidth(onTime);
00579     
00580     if (u > 0)
00581     {
00582         dirL = 1;
00583     }            
00584     else
00585     {
00586         dirL = 0;
00587     }
00588 }
00589 
00590 void SetRightMotorSpeed(float u)
00591 {
00592     float T;
00593     float d;
00594     float onTime;
00595     float maxMotorSpeed = 0.5;
00596     
00597     // bound the input
00598     if (u > maxMotorSpeed)
00599     {
00600         u = maxMotorSpeed;
00601     }
00602                 
00603     if (u < -maxMotorSpeed)
00604     {
00605         u = -maxMotorSpeed;
00606     }
00607     
00608     // calculate duty cycle timing
00609     T = PWMPeriod;
00610     d = abs(u);
00611     onTime = d * T; 
00612 
00613     PwmR.pulsewidth(onTime);
00614     
00615     if (u > 0)
00616     {
00617         dirR = 1;
00618     }            
00619     else
00620     {
00621         dirR = 0;
00622     }
00623 }
00624 
00625 void GetSpeeds()
00626 {
00627     // when using a interrupt period of 50ms: 1480
00628     float leftMaxPos = 1184.0f;
00629     float rightMaxPos = 1184.0f;
00630     
00631     // Restart the SPI module each time
00632     SpiStart = 1;
00633     wait_us(5);
00634     SpiStart = 0;
00635     DE0.write(0x8004);
00636     
00637     // read in 4 16-bit words
00638     Var_Lock.lock();
00639     dPositionLeft = DE0.write(Dummy);   // Read QEI-0 position register 
00640     dTimeLeft = DE0.write(Dummy);       // Read QE-0 time interval register
00641     dPositionRight = DE0.write(Dummy);  // Read QEI-1 position register 
00642     dTimeRight = DE0.write(Dummy);      // Read QEI-1 time interval register
00643     Var_Lock.unlock();
00644     
00645     // calcspeed
00646     fbSpeedL = ((float)dPositionLeft)/leftMaxPos;
00647     fbSpeedR = ((float)dPositionRight)/rightMaxPos;
00648     
00649     // bound the feedback speed
00650     if (fbSpeedL > 1)
00651     {
00652         fbSpeedL = 1;
00653     }
00654     if (fbSpeedL < -1)
00655     {
00656         fbSpeedL = -1;
00657     }
00658     if (fbSpeedR > 1)
00659     {
00660         fbSpeedR = 1;
00661     }
00662     if (fbSpeedR < -1)
00663     {
00664         fbSpeedR = -1;
00665     }
00666 } 
00667 
00668 void IRChecker()
00669 {
00670     float IRF, IRR, IRF1, IRR1, IRF2, IRR2, prevDF, prevDR;
00671     float speedSet;
00672     float aF = 22.6321; // 34.2911
00673     float bF = -0.1721; // -0.2608
00674     float aR = 22.6021; // 34.2456
00675     float bR = -0.0376; // -0.0569
00676     
00677     // Read Sensors
00678     IRF1 = 3.3*IRFront.read();
00679     IRR1 = 3.3*IRRear.read();
00680     IRF2 = 3.3*IRFront.read();
00681     IRR2 = 3.3*IRRear.read();
00682     
00683     // average
00684     IRF = (IRF1 + IRF2)/2;
00685     IRR = (IRR1 + IRR2)/2;
00686     
00687     prevDF = DF;
00688     prevDR = DR;
00689     
00690     // Calculate distance based on voltage
00691     DF = aF/(IRF+bF);
00692     DR = aR/(IRR+bR);
00693     
00694     prevuW = uW;
00695     
00696     // check for invalid data
00697     if (DF < 0)
00698     {
00699         DF = 80;
00700     }
00701     if (DR < 0)
00702     {
00703         DR = 80;
00704     }
00705     if (DF > 80)
00706     {
00707         DF = 80;
00708     }
00709     if (DR > 80)
00710     {
00711         DR = 80;
00712     }
00713     if (DF < 10)
00714     {
00715         DF = 10;
00716     }
00717     if (DR < 10)
00718     {
00719         DR = 10;
00720     }
00721     
00722     // calculate errors
00723     eW = Kpos*(Nominal - (DF + DR)/2) + Kor*(DR - DF);
00724     
00725     // accumulate error
00726     if (prevuW < Turn && prevuW > -Turn)
00727     {
00728         aeW = aeW + eW;
00729     }
00730     
00731     uW = KpW*eW + KiW*aeW;
00732     
00733     if (uW > Turn)
00734     {
00735         uW = Turn;
00736     }
00737     else if (uW < -Turn)
00738     {
00739         uW = -Turn;
00740     }
00741     
00742     // set speed using auto speed control
00743     if (autoSpeed == 1)
00744     {
00745         speedSet = (1 - (abs(eW)*5))*TSpeedL;
00746         if (speedSet < 0.05)
00747         {
00748             speedSet = 0.05;
00749         }
00750         userSetL = speedSet + uW;
00751         userSetR = speedSet - uW;   
00752     }
00753     else
00754     {
00755         // set differential speeds
00756         userSetL = TSpeedL + uW;
00757         userSetR = TSpeedR - uW;
00758     }   
00759     
00760     // data recording code
00761     if (action == 2)
00762     {
00763         if (RecCount < 200)
00764         {
00765             RecX[RecCount] = eW;
00766             RecU[RecCount] = uW;
00767             RecV[RecCount] = DF;
00768             RecE[RecCount] = DR;
00769             RecCount++;
00770         }
00771     }
00772 }
00773 
00774 void DisplayMenu()
00775 {
00776     BtS.printf("\r\n\nPress the corresponding key to do something:\r\n");
00777     BtS.printf("| Key | Action\n\r");
00778     BtS.printf("|-----|----------------------------\n\r");
00779     BtS.printf("|  d  | Drive the robot using wasd keys\n\r");
00780     BtS.printf("|  w  | Robot performs wall following\n\r");
00781     BtS.printf("|  0  | Debug mode\n\r");
00782     BtS.printf("|  r  | Record Data \n\r");
00783     BtS.printf("|  q  | Quit current action, stop the robot, and return to this menu\n\r\n");   
00784 }
00785 
00786 void Ramp(float speed, unsigned int time, unsigned short motor)
00787 {
00788     const unsigned short steps = 20;
00789     float changeL = (speed - userSetL)/steps;
00790     float changeR = (speed - userSetR)/steps;
00791     unsigned short i;
00792     // calculate wait time (we wont worry too much about rounding errors)
00793     unsigned int waitTime = time/steps;
00794     
00795     for (i = 0; i < steps; i++)
00796     {
00797         //Thread::wait(200);
00798         Thread::wait(waitTime);
00799         
00800         if (motor == 0)
00801         {
00802             // change both speeds
00803             userSetL += changeL;
00804             userSetR += changeR;
00805             continue;
00806         }
00807         if (motor == 1)
00808         {
00809             userSetL += changeL;
00810             continue;
00811         }
00812         if (motor == 2)
00813         {
00814             userSetR += changeR;
00815         }  
00816     }   
00817 }