main.cpp

00001 #include "mbed.h"
00002 
00003 LocalFileSystem local("local");
00004 
00005 DigitalOut led1(LED1);
00006 DigitalOut led2(LED2);
00007 DigitalOut led3(LED3);
00008 DigitalOut led4(LED4);
00009 
00010 /////////////////////////motor
00011 PwmOut rm_pwm(p21);
00012 DigitalOut rm_in1(p5);
00013 DigitalOut rm_in2(p6);
00014 DigitalOut rm_en(p7);
00015 
00016 PwmOut lm_pwm(p22);
00017 DigitalOut lm_in1(p26);
00018 DigitalOut lm_in2(p25);
00019 DigitalOut lm_en(p24);
00020 
00021 //0 : stop, 1: forward, 2: backward, 3: leftturn, 4: rightturn
00022 unsigned char moving = 0;
00023 unsigned char moving_command = '2';
00024 unsigned char pre_moving = 0;
00025 
00026 ///////////////////////////
00027 
00028 ///////////////////////////com
00029 Serial pc(USBTX, USBRX);
00030 
00031 Serial mx(p28,p27);
00032 
00033 DigitalOut water(p14);
00034 unsigned char demostate = 0;
00035 unsigned char fire_x = 0;
00036 unsigned char fire_y = 0;
00037 unsigned char moving_state = 0;
00038 unsigned char i, i2, i3, i4;
00039 int target_x = 0, target_y = 0;
00040 int error_x = 0, error_y = 0;
00041 int pre_error_x = 0, pre_error_y = 0;
00042 unsigned char fire_camera;
00043 unsigned char demo = 1;
00044 int dy_x = 2048;
00045 int dy_y = 1748;
00046 
00047 unsigned char k;
00048 //depth_value = depth.read();
00049 
00050 ///////////////////////////sense
00051 AnalogIn distance(p20);
00052 AnalogIn gas1(p19);
00053 AnalogIn gas2(p18);
00054 AnalogIn fire(p17);
00055 AnalogIn distance_l(p15);
00056 AnalogIn distance_r(p16);
00057 
00058 Serial mysenl(p9, p10);
00059 
00060 float distance_value = 0, gas1_value = 0, gas2_value = 0, fire_value = 0;
00061 float distance_r_value = 0;
00062 float distance_l_value = 0;
00063 
00064 int roll, pitch, yaw, temp, check, r_check;
00065 int r,p,y;
00066 unsigned char input, input2, startb;
00067 /////////////////////////
00068 
00069 ///////////////////////////////////////////////////
00070 void mx_write(unsigned char address, int data);
00071 void bulk_write(int speed1, int position1, int speed2, int position2);
00072 //////////////////////////////////
00073 void motor_thread(void const *args)
00074 {
00075     rm_pwm = 0.5;
00076     lm_pwm = 0.5;
00077     
00078     rm_pwm.period_ms(20);
00079     rm_pwm.pulsewidth_ms(10);
00080 
00081     lm_pwm.period_ms(20);
00082     lm_pwm.pulsewidth_ms(10);
00083 
00084     rm_in1 = 0;
00085     rm_in2 = 0;
00086     rm_en = 0;
00087 
00088     lm_in1 = 0;
00089     lm_in2 = 0;
00090     lm_en = 0;
00091 
00092     while (true) {
00093         if(pre_moving!=moving)
00094         {
00095             if(moving==0)
00096             {
00097                 
00098                 rm_in1 = 0;
00099                 rm_in2 = 0;
00100                 rm_en = 0;
00101 
00102                 lm_in1 = 0;
00103                 lm_in2 = 0;
00104                 lm_en = 0;
00105             }
00106             else if(moving==1)
00107             {
00108                 rm_pwm.period_ms(20);
00109                 rm_pwm.pulsewidth_ms(10);
00110             
00111                 lm_pwm.period_ms(20);
00112                 lm_pwm.pulsewidth_ms(10);
00113                 rm_in1 = 0;
00114                 rm_in2 = 1;
00115                 rm_en = 1;
00116 
00117                 lm_in1 = 1;
00118                 lm_in2 = 0;
00119                 lm_en = 1;
00120             }
00121             else if(moving==2)
00122             {
00123                 rm_pwm.period_ms(20);
00124         rm_pwm.pulsewidth_ms(10);
00125 
00126     lm_pwm.period_ms(20);
00127     lm_pwm.pulsewidth_ms(10);          
00128                 rm_in1 = 1;
00129                 rm_in2 = 0;
00130                 rm_en = 1;
00131 
00132                 lm_in1 = 0;
00133                 lm_in2 = 1;
00134                 lm_en = 1;
00135             }
00136             else if(moving==3)
00137             {
00138                 rm_pwm.period_ms(20);
00139     rm_pwm.pulsewidth_ms(10);
00140 
00141     lm_pwm.period_ms(20);
00142     lm_pwm.pulsewidth_ms(10);
00143                 rm_in1 = 0;
00144                 rm_in2 = 1;
00145                 rm_en = 1;
00146 
00147                 lm_in1 = 0;
00148                 lm_in2 = 1;
00149                 lm_en = 1;
00150             }
00151             else if(moving==4)
00152             {
00153                 rm_pwm.period_ms(20);
00154     rm_pwm.pulsewidth_ms(10);
00155 
00156     lm_pwm.period_ms(20);
00157     lm_pwm.pulsewidth_ms(10);
00158                 rm_in1 = 1;
00159                 rm_in2 = 0;
00160                 rm_en = 1;
00161 
00162                 lm_in1 = 1;
00163                 lm_in2 = 0;
00164                 lm_en = 1;
00165             }
00166             else if(moving==5)
00167             {
00168                 rm_pwm.period_ms(20);
00169     rm_pwm.pulsewidth_ms(7);
00170 
00171     lm_pwm.period_ms(20);
00172     lm_pwm.pulsewidth_ms(7);
00173                 rm_in1 = 0;
00174                 rm_in2 = 1;
00175                 rm_en = 1;
00176 
00177                 lm_in1 = 1;
00178                 lm_in2 = 0;
00179                 lm_en = 1;
00180             }
00181             else if(moving==7)
00182             {
00183                 rm_pwm.period_ms(20);
00184     rm_pwm.pulsewidth_ms(7);
00185 
00186     lm_pwm.period_ms(20);
00187     lm_pwm.pulsewidth_ms(7);
00188                 rm_in1 = 0;
00189                 rm_in2 = 1;
00190                 rm_en = 1;
00191 
00192                 lm_in1 = 0;
00193                 lm_in2 = 1;
00194                 lm_en = 1;
00195             }
00196             else if(moving==6)
00197             {
00198                 rm_pwm.period_ms(20);
00199     rm_pwm.pulsewidth_ms(7);
00200 
00201     lm_pwm.period_ms(20);
00202     lm_pwm.pulsewidth_ms(7);
00203                rm_in1 = 1;
00204                 rm_in2 = 0;
00205                 rm_en = 1;
00206 
00207                 lm_in1 = 1;
00208                 lm_in2 = 0;
00209                 lm_en = 1;
00210             }
00211         }
00212        //pc.printf("%d    %d %d %d %d %d %d\r\n",moving, rm_in1.read(), rm_in2.read(), rm_en.read(), lm_in1.read(), lm_in2.read(), lm_en.read());
00213         pre_moving = moving;
00214 
00215         led2 = !led2;
00216         Thread::wait(500);
00217     }
00218 }
00219 //////////////////////////////////
00220 void com_thread(void const *args)
00221 {
00222     bulk_write(5, 2048, 5, 2048);
00223     while (true) {
00224        if(pc.readable())
00225         {
00226         i = pc.getc();
00227         
00228         if(i==100)
00229         {
00230             i = pc.getc();
00231             i = pc.getc();
00232             i2 = pc.getc();
00233             i3 = pc.getc();
00234             i4 = pc.getc();
00235             
00236             fire_camera = pc.getc();
00237             moving_command = pc.getc();
00238             pc.printf("%d\r\n%f\r\n%d\r\n%d\r\n%d\r\n%d\r\n",temp,gas2_value,r,p,y,fire_camera);
00239             
00240             target_x = (int)i*256+i2;
00241             target_y = (int)i3*256+i4;
00242         }
00243         if(fire_camera == 1)
00244         {
00245             error_x = 320-target_x;
00246             error_y = 240-target_y;
00247             
00248             dy_x += error_x*0.1+(pre_error_x-error_x)*0.02;
00249             dy_y += error_y*0.1+(pre_error_y-error_y)*0.02;
00250             
00251             pre_error_x = error_x;
00252             pre_error_y = error_y;
00253             
00254             bulk_write(25, dy_x, 25, dy_y);
00255         }
00256         else
00257         {
00258             //dy_x = 2048;
00259             //dy_y = 1748;
00260             //bulk_write(10, dy_x, 10, dy_y);
00261         }
00262        
00263         }
00264         
00265         
00266         
00267         led3 = !led3;
00268         Thread::wait(10);
00269     }
00270 }
00271 //////////////////////////////////
00272 void sense_thread(void const *args)
00273 {
00274     
00275     while (true) {
00276         if(mysenl.readable())
00277             {
00278             startb = mysenl.getc();
00279             }
00280             if(startb==0x02)
00281              {
00282             r_check = 0;
00283             input = mysenl.getc();
00284             input2 = mysenl.getc();
00285             r_check+=input;
00286             r_check+=input2;
00287             roll = (input*256+input2)/100-180;
00288             
00289             input = mysenl.getc();
00290             input2 = mysenl.getc();
00291             r_check+=input;
00292             r_check+=input2;
00293             pitch = (input*256+input2)/100-90;
00294             
00295             input = mysenl.getc();
00296             input2 = mysenl.getc();
00297             r_check+=input;
00298             r_check+=input2;
00299             yaw = (input*256+input2)/100-180;
00300             
00301             input = mysenl.getc();
00302             input2 = mysenl.getc();
00303             
00304             temp = (input*256+input2)/100-100;
00305             r_check+=input;
00306             r_check+=input2;
00307             
00308             
00309             input = mysenl.getc();
00310             input2 = mysenl.getc();
00311             
00312             input = mysenl.getc();
00313             input2 = mysenl.getc();
00314            if(input==44&&input2==3)
00315            {
00316             r=roll;
00317             p=pitch;
00318             y=yaw;
00319             distance_value = distance.read();
00320             distance_r_value = distance_r.read();
00321             distance_l_value = distance_l.read();
00322         gas1_value = gas1.read();
00323         gas2_value = gas2.read();
00324         fire_value = fire.read();
00325            // pc.printf("\t%d\t%d\t%d\n\r\t%d\t%d\t%d\t%d\t%d\r\n",r, p, y, temp, r_check,check, input, input2);
00326             //pc.printf("\tdis%f\tdis_r%f\tdis_l%f\r\n\tgas %f\tgas %f\tfire %f\r\n",distance_value,distance_r_value,distance_l_value, gas1_value, gas2_value, fire_value);
00327             }
00328             }
00329         distance_value = distance.read();
00330             distance_r_value = distance_r.read();
00331             distance_l_value = distance_l.read();
00332         gas1_value = gas1.read();
00333         gas2_value = gas2.read();
00334         fire_value = fire.read();
00335         if(demo==1)
00336         {
00337             if(fire_camera==0&&demostate==0)
00338             {
00339                 bulk_write(5, 2048, 5, 2048);
00340                 moving = 1;   
00341                 Thread::wait(1000);
00342                 moving = 2; 
00343                 Thread::wait(1000);
00344                 
00345             }
00346             else if((distance_r_value>0.5||distance_l_value>0.5||distance_value>0.2)&&fire_camera&&demostate==1)
00347             {
00348                 moving = 0;
00349                 Thread::wait(2000); 
00350                    water = 1;
00351                    Thread::wait(1300);
00352                    water = 0;
00353                    demostate = 0;
00354             }
00355             else if(fire_camera==1)
00356             {
00357                 demostate = 1;
00358                 if(dy_x>2248)
00359             {
00360                 moving = 7;
00361                  Thread::wait(100);
00362              }
00363             else if(dy_x<1748) 
00364             {
00365                moving = 6;
00366                 Thread::wait(100);
00367                 
00368             }
00369                 moving = 5;
00370                 
00371             }
00372             
00373         }
00374         else
00375         {
00376         if(moving_command==1)
00377         {
00378             if(moving_state==0)
00379             {
00380                 moving = 2;
00381                 
00382                 if(fire_camera==1)
00383                     moving_state = 1;
00384             }
00385             else if(moving_state==1)
00386             {
00387                 moving_state = 1;
00388                 //moving
00389             if(distance_r_value>0.7&&distance_l_value>0.7)
00390                 moving = 2;
00391             else if(distance_r_value>0.7)
00392                 moving = 3;
00393             else if(distance_l_value>0.7)
00394                 moving = 4;
00395             else
00396                 moving = 1;
00397                 
00398             if(dy_x>2148)
00399             {
00400                 moving = 4;
00401                 Thread::wait(500);
00402              }
00403             else if(dy_x<1948) 
00404             {
00405                moving = 3;
00406                 Thread::wait(500); 
00407                 
00408             } 
00409                 if(distance_value>0.5)
00410                     moving_state = 2;
00411             }
00412             else if(moving_state == 2);
00413             {
00414                    Thread::wait(5000); 
00415                    water = 1;
00416                    Thread::wait(1300);
00417                    water = 0; 
00418                    
00419                    if(fire_camera==0)
00420                     moving_state = 0;
00421             }
00422             
00423         }
00424         else
00425         {
00426            // moving_state = 0;
00427             //bulk_write(5, 2048, 5, 2048);
00428             //water = 0;
00429             //moving = 0;
00430             //moving = moving_command;   
00431         }
00432         }
00433         led4 = !led4;
00434         Thread::wait(50);
00435     }
00436 }
00437 // main() runs in its own thread in the OS
00438 // (note the calls to Thread::wait below for delays)
00439 int main()
00440 {
00441     pc.baud(57600);
00442     mx.baud(57600);
00443     mysenl.baud(115200);
00444 
00445     Thread::wait(1000);
00446 
00447     Thread thread1(motor_thread);
00448     Thread thread2(com_thread);
00449     Thread thread3(sense_thread);
00450 
00451     while (true) {
00452         
00453         
00454         
00455         /*
00456         
00457         if(pc.readable())
00458         {
00459         i = pc.getc();
00460         
00461         
00462         if(i=='w')
00463             moving_command = 1;
00464         else if(i=='s')
00465             moving_command = 2;
00466         else if(i=='a')
00467             moving_command = 3;
00468         else if(i=='d')
00469             moving_command = 4;
00470         else if(i=='q')
00471             moving_command = 0;
00472         else if(i=='t')
00473             moving_command = 5;
00474 
00475         else if(i=='z')
00476             dy_x += 10;
00477         else if(i=='x')
00478             dy_x -= 10;
00479         else if(i=='c')
00480             dy_y += 10;
00481         else if(i=='v')
00482             dy_y -= 10;
00483         else if(i=='e')
00484             water = 1;
00485         else if(i=='r')
00486             water = 0;
00487         
00488         }
00489         */
00490         led1 = !led1;
00491         Thread::wait(500);
00492 
00493     }
00494 }
00495 
00496 void mx_write(unsigned char address, int data)
00497 {
00498 
00499     unsigned char k = 0x07+0x05+0x03+address+((data&0xff00)>>8)+(data&0xff);
00500     k = 0xff-k;
00501 
00502     mx.putc(0xff);
00503     mx.putc(0xff);
00504     mx.putc(0x07);
00505     mx.putc(0x05);
00506     mx.putc(0x03);
00507     mx.putc(address);
00508     mx.putc((unsigned char)(data&0xff));
00509     mx.putc((unsigned char)((data&0xff00)>>8));
00510     mx.putc(k);
00511 
00512     pc.printf("%d %d %d %d %d %d %d %d %d\n\r",0xff,0xff,0x07,0x05,0x03,address,data&0xff,(data&0xff00)>>8,k);
00513 }
00514 
00515 void bulk_write(int speed1, int position1, int speed2, int position2)
00516 {
00517     k = 0xfe;
00518     k += 0x0e;
00519     k += 0x83;
00520     k += 0x1e;
00521     k += 0x04;
00522     //k = 0xfe+0x0e+0x83+0x1e+0x04;
00523     k+=0x04+((position1&0xff00)>>8)+(position1&0xff)+((speed1&0xff00)>>8)+(speed1&0xff);
00524     k+=0x08+((position2&0xff00)>>8)+(position2&0xff)+((speed2&0xff00)>>8)+(speed2&0xff);
00525     k = 0xff-k;
00526 
00527     mx.putc(0xff);
00528     mx.putc(0xff);
00529     mx.putc(0xfe);
00530     mx.putc(0x0e);
00531     mx.putc(0x83);
00532     mx.putc(0x1E);
00533     mx.putc(0x04);
00534     mx.putc(0x04);
00535     mx.putc((unsigned char)(position1&0xff));
00536     mx.putc((unsigned char)((position1&0xff00)>>8));
00537     mx.putc((unsigned char)(speed1&0xff));
00538     mx.putc((unsigned char)((speed1&0xff00)>>8));
00539     mx.putc(0x08);
00540     mx.putc((unsigned char)(position2&0xff));
00541     mx.putc((unsigned char)((position2&0xff00)>>8));
00542     mx.putc((unsigned char)(speed2&0xff));
00543     mx.putc((unsigned char)((speed2&0xff00)>>8));
00544     mx.putc(k);
00545 }