main.cpp

00001 /*-----------------------------------------
00002 Nest Egg Inc.
00003 White Wizard Board Android TANK Ver1.0
00004 
00005 How to connect?
00006 
00007 *Bluetooth module
00008  from p9 to p16.
00009 (you can select other serial pins)
00010 
00011 *Motor Driver Board
00012  p21 - IN_R1
00013  p22 - IN_R2
00014  p23 - IN_L2
00015  p24 - IN_L1
00016 
00017 -----------------------------------------*/
00018 
00019 #include "mbed.h"
00020 
00021 Serial rn42(p9,p10);
00022 DigitalOut myled(LED1);
00023 DigitalOut myled2(LED2);
00024 DigitalOut myled3(LED3);
00025 DigitalOut myled4(LED4);
00026 
00027 PwmOut motorR1(p21);
00028 PwmOut motorR2(p22);
00029 PwmOut motorL2(p23);
00030 PwmOut motorL1(p24);
00031 
00032 void read_string(char*);
00033 
00034 int main() {
00035     int i = 0;
00036     int j = 0;
00037     char input[255];
00038     rn42.baud(115200);
00039     
00040     motorR1.period(0.003*9);
00041     motorR2.period(0.003*9);
00042     motorL2.period(0.003*9);
00043     motorL1.period(0.003*9);
00044     
00045     float flg_width_a = 0.0;
00046     float flg_width_b = 0.0;
00047     
00048     int flg_error;
00049     
00050     while (1) {    
00051     
00052         i=0;
00053         flg_error = 0;
00054         myled2 = 1;
00055             
00056         if (rn42.readable()) {
00057             
00058             while ((i==0) || ( input[i-1] != 'G')) {
00059                 input[i] = rn42.getc();
00060                 //printf("%c;%d\n",input[i],i);
00061                 ++i;
00062                 myled != myled;
00063             }
00064                         
00065             for(j=0;j<i;++j){
00066                 rn42.putc(input[j]);
00067             }
00068             
00069             //check format L**R**G
00070             if((input[0]=='L') && (input[3]=='R') && (input[6]=='G')){
00071                 myled2 = 0;
00072                 
00073                 //check speed
00074                 switch(input[2]){
00075                     case '0':
00076                         flg_width_a = 0.0;
00077                         break;
00078                     case '1':
00079                         flg_width_a = 0.003;
00080                         break;
00081                     case '2':
00082                         flg_width_a = 0.003*2;
00083                         break;
00084                     case '3':
00085                         flg_width_a = 0.003*3;
00086                         break;
00087                     case '4':
00088                         flg_width_a = 0.003*4;
00089                         break;
00090                     case '5':
00091                         flg_width_a = 0.003*5;
00092                         break;
00093                     case '6':
00094                         flg_width_a = 0.003*6;
00095                         break;
00096                     case '7':
00097                         flg_width_a = 0.003*7;
00098                         break;
00099                     case '8':
00100                         flg_width_a = 0.003*8;
00101                         break;
00102                     case '9':
00103                         flg_width_a = 0.003*9;
00104                         break;
00105                     default:
00106                         flg_error = 1;
00107                         
00108                 }
00109                 
00110                 
00111                 switch(input[1]){
00112                     case 'F':
00113                         motorL2.pulsewidth(flg_width_a);
00114                         motorL1.pulsewidth(0);
00115                         break;
00116                     case 'B':
00117                         motorL2.pulsewidth(0);
00118                         motorL1.pulsewidth(flg_width_a);
00119                         break;
00120                     default:
00121                         flg_error = 1;
00122                         
00123                 }
00124                 
00125                 //check speed
00126                 switch(input[5]){
00127                     case '0':
00128                         flg_width_b = 0.0;
00129                         break;
00130                     case '1':
00131                         flg_width_b = 0.003;
00132                         break;
00133                     case '2':
00134                         flg_width_b = 0.003*2;
00135                         break;
00136                     case '3':
00137                         flg_width_b = 0.003*3;
00138                         break;
00139                     case '4':
00140                         flg_width_b = 0.003*4;
00141                         break;
00142                     case '5':
00143                         flg_width_b = 0.003*5;
00144                         break;
00145                     case '6':
00146                         flg_width_b = 0.003*6;
00147                         break;
00148                     case '7':
00149                         flg_width_b = 0.003*7;
00150                         break;
00151                     case '8':
00152                         flg_width_b = 0.003*8;
00153                         break;
00154                     case '9':
00155                         flg_width_b = 0.003*9;
00156                         break;
00157                     default:
00158                         flg_error = 1;
00159                         
00160                 }
00161                 
00162                 switch(input[4]){
00163                         
00164                     case 'F':
00165                         motorR2.pulsewidth(flg_width_b);
00166                         motorR1.pulsewidth(0);
00167                         myled3 = 0;
00168                         break;
00169                     case 'B':
00170                         motorR2.pulsewidth(0);
00171                         motorR1.pulsewidth(flg_width_b);
00172                         myled3 = 0;
00173                         break;
00174                     default:
00175                         flg_error = 1;
00176                         
00177                 }
00178             }
00179         }
00180         
00181         if(flg_error == 1){
00182             myled2 = 1;
00183             motorL2.pulsewidth(0);
00184             motorL1.pulsewidth(0);
00185             motorR2.pulsewidth(0);
00186             motorR1.pulsewidth(0);
00187             
00188         }
00189         //wait(0.05);
00190         
00191     }
00192 }