main.cpp

00001 #include "ArduMotoShield.h"
00002 #include "mbed.h"
00003 
00004 ArduMotoShield Motor;
00005 AnalogIn current(A2),RV_set(A3),RV2(A4);
00006 DigitalIn mybutton(USER_BUTTON);
00007 
00008 Serial Device(D8, D2);
00009 Serial pc(SERIAL_TX, SERIAL_RX);
00010 
00011 float max(float ,float );
00012 float min(float ,float );
00013 
00014 int main(void){
00015     
00016     Device.baud(9600);
00017     pc.baud(9600);
00018     float value =0;
00019     float Speed = 0;
00020     float Left = 0,Right = 0;
00021     float x=0,y=0;
00022     char word;
00023     int i=0,count=0;
00024     float maxAmps = 0,minAmps = 0;
00025     
00026     
00027     pc.printf("Hello PROJECT_CAR.\n");
00028     while(1)
00029     {
00030        // printf("Valuenery = %.2f. %d\n", RV2.read(),abs(i));
00031         //wait(0.75);
00032         //Motor.forward(1.0,1.0);
00033         //pc.printf(" %.2f\n ",(current.read()*1000+10));
00034         //wait(0.1);
00035         /*float amps = (501 - (current.read()*1000))*27.03/1023;
00036          maxAmps = max(maxAmps , amps);
00037          minAmps = min(minAmps , amps);
00038         float noise = maxAmps - minAmps;
00039         pc.printf("Curent is  %.2f A.      %.2f ",amps,abs(amps));
00040         pc.printf("Noise is %.2f\n",noise);
00041         if (pc.readable() != -1){maxAmps = amps; minAmps =  amps;}
00042         wait(0.1);*/
00043         
00044         
00045         
00046         
00047         
00048         if(count<=0)
00049         {
00050             if(Device.readable())
00051             {
00052                 word = Device.getc();
00053                 pc.printf(" %c.\n",word);
00054                 if(word == '%')
00055                 {
00056                     Speed = RV_set.read()*3.3;
00057                     if(Speed >=0 && Speed < 0.825)//0-0.825
00058                     {
00059                         x=0;y=0.824;
00060                         pc.printf("Level %d is = %.3f \t %.3f \t %.3f \t %.3f.\n",i=0,x,y,RV_set.read(),Speed);
00061                     }
00062                     else if(Speed >= 0.825 && Speed < 1.650)//0.825-1.650
00063                     {
00064                         x=0.825;y=1.649;
00065                         pc.printf("Level %d is = %.3f \t %.3f \t %.3f \t %.3f.\n",i=1,x,y,RV_set.read(),Speed);
00066                     }
00067                     else if(Speed >= 1.650 && Speed < 2.475)//1.650-2.475
00068                     {
00069                         x=1.650;y=2.474;
00070                         pc.printf("Level %d is = %.3f \t %.3f \t %.3f \t %.3f.\n",i=2,x,y,RV_set.read(),Speed);
00071                     }
00072                     else//2.475-3.300
00073                     {
00074                         x=2.475;y=3.300;
00075                         pc.printf("Level %d is = %.3f \t %.3f \t %.3f \t %.3f.\n",i=3,x,y,RV_set.read(),Speed);
00076                     }
00077                     count++;
00078                 }
00079                 if(word == '#')
00080                 { 
00081                 //Device.printf("24.3,324.5,234.4,\n");
00082                     Device.printf("%.2f,",0);//ความเร็ว
00083                     Device.printf("%.2f,",0);//กระแส
00084                     if(RV_set.read()>=0&&RV_set.read()<0.25)
00085                         Device.printf("Kange,\n");//โหมด
00086                     else if(RV_set.read()>=0.25&&RV_set.read()<0.50)
00087                         Device.printf("Thin,\n");//โหมด
00088                     else if(RV_set.read()>=0.50&&RV_set.read()<0.75)
00089                         Device.printf("Long,\n");//โหมด
00090                     else
00091                         Device.printf("FAT,\n");//โหมด
00092                         
00093                 }    
00094             }
00095             wait(0.5);
00096         }
00097         else
00098         {
00099             if(Device.readable())
00100             {
00101                 if(RV_set.read()>=0.03)
00102                 {
00103                     if(RV2.read()<0.49)
00104                     {
00105                         Right = y/3.3;
00106                         Left = RV2.read()/2+(0.25*i);
00107                         pc.printf("Value = %.2f , %.2f , %.2f.\n",RV2.read(),Left,Right);  
00108                     }
00109                     else if(RV2.read()>=0.49&&RV2.read()<=0.51)
00110                     {
00111                         Left = (y/3.3)-0.072;
00112                         Right = y/3.3;
00113                         pc.printf("CentF = %.2f , %.2f , %.2f\n",Left,Right,RV2.read());   
00114                     }
00115                     else 
00116                     {
00117                         Left = y/3.3;
00118                         Right = (((RV2.read()*2)-2)*(-1)/4)+(0.25*i);
00119                         pc.printf("Right = %.2f , %.2f , %.2f.\n",RV2.read(),Left,Right);
00120                     }
00121                     Motor.forward(Left,Right); 
00122                         float amps = (501 - (current.read()*1000))*27.03/1023;
00123                         maxAmps = max(maxAmps , amps);
00124                         minAmps = min(minAmps , amps);
00125                         float noise = maxAmps - minAmps;
00126                         Device.printf("%.2f,",abs(amps));//กระแส
00127                         Device.printf("%.2f,",((Left+Right)/2));//ความเร็ว
00128                         pc.printf("Current is %.2f\n",abs(amps));
00129                         if(1==0)
00130                             Device.printf("Khang ,\n");//โหมด
00131                         else if(i==1)
00132                             Device.printf("Thin,\n");//โหมด
00133                         else if(i==2)
00134                             Device.printf("Long,\n");//โหมด
00135                         else
00136                             Device.printf("FAT,\n");//โหมด
00137                    if (pc.readable() != -1){maxAmps = amps; minAmps =  amps;}
00138                     wait(0.8);
00139                 }
00140             }
00141         }
00142     }
00143     //Motor.stop();
00144 }
00145 float max(float a,float b)
00146 {
00147     if(a>b)
00148         return a;    
00149     else
00150         return b;
00151 }
00152 float min(float a,float b)
00153 {
00154     if(a<b)
00155         return a;    
00156     else
00157         return b;
00158 }