main.cpp

00001 // test code, this demonstrates working motor drivers. 
00002 // full reverse to full stop, dynamicaly brake and switch off.
00003 #include "mbed.h"
00004 #include "Motor.h"
00005 Motor B(p23, p6, p5); // pwm, fwd, rev, can brake //right?
00006 Motor A(p24, p7, p8); // pwm, fwd, rev, can brake //left?
00007 
00008 
00009 //End Device Setup for XBEE End Device
00010 Serial xbee2(p9,p10);
00011 DigitalOut rst1(p11);
00012 
00013 //Movement Indicators
00014 DigitalOut myled1(LED1);
00015 DigitalOut myled2(LED2);
00016 DigitalOut myled3(LED3);
00017 DigitalOut myled4(LED4);
00018 
00019 //debug
00020 Serial pc(USBTX, USBRX);
00021 
00022 // Default Value for command to wait
00023 char command = 'W';
00024 
00025 int main() {
00026     rst1 = 0;   //Set reset pin to 0
00027     myled1 = 0;
00028     myled2 = 0;
00029     myled3 = 0;
00030     myled4 = 0;
00031     wait_ms(1);
00032     rst1 = 1;   //Set reset pin to 1
00033     wait_ms(1);
00034     
00035     while (1){
00036         if(xbee2.readable()) {//if new command readable
00037             command = xbee2.getc();
00038             //pc.printf("Command: %c\n\r",command);
00039         }
00040         
00041 //        pc.printf("%c",command);
00042 //        pc.printf("\n\r");
00043         
00044         switch (command){
00045             case 'A': // Move back speed 1
00046                 A.speed(-0.5);
00047                 B.speed(-0.5);
00048                 
00049                 myled1 = 1;
00050                 myled2 = 0;
00051                 myled3 = 0;
00052                 myled4 = 0;
00053                 
00054                 //wait(0.5);
00055                 break;
00056             case 'B': // Move back speed 2
00057                 A.speed(-1);
00058                 B.speed(-1);
00059                 
00060                 myled1 = 1;
00061                 myled2 = 0;
00062                 myled3 = 0;
00063                 myled4 = 0;
00064                 
00065                 //wait(0.5);
00066                 break;
00067             case 'C': // Move right speed 1
00068                 A.speed(0.5);
00069                 B.speed(0);
00070                 
00071                 myled1 = 0;
00072                 myled2 = 1;
00073                 myled3 = 0;
00074                 myled4 = 0;
00075                 
00076                 //wait(0.5);
00077                 break;
00078             case 'D': // Move right speed 2
00079                 A.speed(1);
00080                 B.speed(0);
00081                 
00082                 myled1 = 0;
00083                 myled2 = 1;
00084                 myled3 = 0;
00085                 myled4 = 0;
00086                 
00087                 //wait(0.5);
00088                 break;
00089             case 'E': // Move forward speed 1
00090                 A.speed(0.5);
00091                 B.speed(0.5);
00092                 
00093                 myled1 = 0;
00094                 myled2 = 0;
00095                 myled3 = 1;
00096                 myled4 = 0;
00097                 
00098                 //wait(0.5);
00099                 break;
00100             case 'F': // Move forward speed 2
00101                 A.speed(1);
00102                 B.speed(1);
00103                 
00104                 myled1 = 0;
00105                 myled2 = 0;
00106                 myled3 = 1;
00107                 myled4 = 0;
00108                 
00109                 //wait(0.5);
00110                 break;
00111             case 'G': // Move left speed 1
00112                 A.speed(0);
00113                 B.speed(0.5);
00114                 
00115                 myled1 = 0;
00116                 myled2 = 0;
00117                 myled3 = 0;
00118                 myled4 = 1;
00119                 
00120                 //wait(0.5);
00121                 break;
00122             case 'H': // Move left speed 2
00123                 A.speed(0);
00124                 B.speed(1);
00125                 
00126                 myled1 = 0;
00127                 myled2 = 0;
00128                 myled3 = 0;
00129                 myled4 = 1;
00130                 
00131                 //wait(0.5);
00132                 break;
00133             case 'I': // Stop
00134                 A.speed(0.0);
00135                 B.speed(0.0);
00136                 
00137                 myled1 = 1;
00138                 myled2 = 1;
00139                 myled3 = 1;
00140                 myled4 = 1;
00141                 break;
00142                 
00143                 //wait(0.5);
00144                 
00145             case 'W': // Stop if nothing else is coming in 
00146                 A.speed(0.0);
00147                 B.speed(0.0);
00148                 
00149                 myled1 = 0;
00150                 myled2 = 0;
00151                 myled3 = 0;
00152                 myled4 = 0;
00153                 //wait(0.5);
00154 
00155             }
00156         }
00157 }