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 A(p22, p6, p5); // pwm, fwd, rev, can brake 
00006 Motor B(p21, p7, p8); // pwm, fwd, rev, can brake
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(LED2);
00017 DigitalOut myled4(LED2);
00018 
00019 // Default Value for command to wait
00020 char command = 'W';
00021 
00022 int main() {
00023     rst1 = 0;   //Set reset pin to 0
00024     myled1 = 0;
00025     myled2 = 0;
00026     myled3 = 0;
00027     myled4 = 0;
00028     wait_ms(1);
00029     rst1 = 1;   //Set reset pin to 1
00030     wait_ms(1);
00031     
00032     while (1){
00033         if(xbee2.readable()) {//if new command readable
00034             command = xbee2.getc();
00035             //pc.printf("Command: %c\n\r",command);
00036         }
00037         
00038         switch (command){
00039             case 'A': // Move back speed 1
00040                 A.speed(-0.5);
00041                 B.speed(-0.5);
00042                 
00043                 myled1 = 1;
00044                 myled2 = 0;
00045                 myled3 = 0;
00046                 myled4 = 0;
00047                 
00048                 wait(0.5);
00049                 break;
00050             case 'B': // Move back speed 2
00051                 A.speed(-1);
00052                 B.speed(-1);
00053                 
00054                 myled1 = 1;
00055                 myled2 = 0;
00056                 myled3 = 0;
00057                 myled4 = 0;
00058                 
00059                 wait(0.5);
00060                 break;
00061             case 'C': // Move right speed 1
00062                 A.speed(0.5);
00063                 B.speed(0);
00064                 
00065                 myled1 = 0;
00066                 myled2 = 1;
00067                 myled3 = 0;
00068                 myled4 = 0;
00069                 
00070                 wait(0.5);
00071                 break;
00072             case 'D': // Move right speed 2
00073                 A.speed(1);
00074                 B.speed(0);
00075                 
00076                 myled1 = 0;
00077                 myled2 = 1;
00078                 myled3 = 0;
00079                 myled4 = 0;
00080                 
00081                 wait(0.5);
00082                 break;
00083             case 'E': // Move forward speed 1
00084                 A.speed(0.5);
00085                 B.speed(0.5);
00086                 
00087                 myled1 = 0;
00088                 myled2 = 0;
00089                 myled3 = 1;
00090                 myled4 = 0;
00091                 
00092                 wait(0.5);
00093                 break;
00094             case 'F': // Move forward speed 2
00095                 A.speed(1);
00096                 B.speed(1);
00097                 
00098                 myled1 = 0;
00099                 myled2 = 0;
00100                 myled3 = 1;
00101                 myled4 = 0;
00102                 
00103                 wait(0.5);
00104                 break;
00105             case 'G': // Move left speed 1
00106                 A.speed(0);
00107                 B.speed(0.5);
00108                 
00109                 myled1 = 0;
00110                 myled2 = 0;
00111                 myled3 = 0;
00112                 myled4 = 1;
00113                 
00114                 wait(0.5);
00115                 break;
00116             case 'H': // Move left speed 2
00117                 A.speed(0);
00118                 B.speed(1);
00119                 
00120                 myled1 = 0;
00121                 myled2 = 0;
00122                 myled3 = 0;
00123                 myled4 = 1;
00124                 
00125                 wait(0.5);
00126                 break;
00127             case 'I': // Stop
00128                 A.speed(-0.5);
00129                 B.speed(-0.5);
00130                 
00131                 myled1 = 1;
00132                 myled2 = 1;
00133                 myled3 = 1;
00134                 myled4 = 1;
00135                 
00136                 wait(0.5);
00137                 
00138             case 'W': // Stop if nothing else is coming in 
00139                 A.speed(0);
00140                 B.speed(0);
00141                 
00142                 myled1 = 0;
00143                 myled2 = 0;
00144                 myled3 = 0;
00145                 myled4 = 0;
00146                 
00147                 wait(0.5);
00148 
00149             }
00150         }
00151 }