main.cpp

00001 #include <mbed.h>
00002 #include <getGPS.h>
00003 
00004 DigitalOut pin(D7);
00005 DigitalIn pin1(D6);
00006 PwmOut pinAFin(D10);
00007 PwmOut pinARin(A1);
00008 PwmOut pinBFin(D11);
00009 PwmOut pinBRin(D12);
00010 GPS gps(D1, D0);// tx,rx
00011 Serial xbee(SERIAL_TX, SERIAL_RX);
00012 
00013 void driveMotor(double speedA,double speedB) {
00014   float outputA = abs(speedA);
00015   float outputB = abs(speedB);
00016   if (speedA > 0) {
00017     pinAFin=outputA;
00018     pinARin=0;
00019   } else if (speedA < 0) {
00020     pinAFin=0;
00021     pinARin=outputA;
00022   } else {
00023     pinAFin=0;
00024     pinARin=0;
00025   }
00026   if (speedB > 0) {
00027     pinBFin=outputB;
00028     pinBRin=0;
00029   } else if (speedB < 0) {
00030     pinBFin=0;
00031     pinBRin=outputB;
00032   } else {
00033     pinBFin=0;
00034     pinBRin=0;
00035   }
00036 }
00037 
00038 int main(){
00039     pin1.mode(PullUp);
00040     while(1){
00041         if(pin1 == 1){
00042             break;
00043             }
00044         }
00045         
00046     wait(50);
00047     pin = 1;
00048     wait(17);
00049     pin = 0;
00050     wait(10);
00051     
00052     
00053     double x,y,u,v,m,n,i,w,j,p,q,e,f,c;
00054     int r,s;
00055     u = 34.546385;
00056     v = 135.502085;
00057     s = 0;
00058     double t[2] = {0,0};
00059     
00060 //    xbee.printf("XBee Connected\r\n");
00061     while(gps.getgps() != true){
00062         }    
00063     
00064 //基準値を記録する
00065     gps.getgps();
00066     x = gps.latitude;
00067     y = gps.longitude;
00068 //    xbee.printf("x:%f,y:%f\r\n", x, y);
00069 
00070 //10秒前進する
00071     driveMotor(1,1);
00072     wait(5);//メモ：ただ直進してる
00073     driveMotor(0.8,0.8);
00074     wait(1);
00075     driveMotor(0.5,0.5);
00076     wait(2);
00077     driveMotor(0.2,0.2);
00078     wait(2);
00079     driveMotor(0,0);
00080 //m,nに現在の経度、緯度を記録する
00081     while(gps.getgps() != true){
00082     }
00083     gps.getgps();
00084     m=gps.latitude;
00085     n=gps.longitude;
00086     xbee.printf("m:%f,n:%f\r\n", m, n);
00087 //a,bの配列に、それぞれ緯度、経度の現在地と初期位置の差、jに目標までの距離を記録する        
00088     double a[2]={m-x,n-y};
00089     double b[2]={u-m,v-n};
00090     j=pow(b[0]/0.000008983148616*b[0]/0.000008983148616+b[1]/0.000010966382364*b[1]/0.000010966382364,0.5);
00091     xbee.printf("距離:%f\r\n", j);
00092     
00093     
00094     while(j>5){
00095 
00096 //cosと角度の値を出す        
00097         p=pow(a[0]*a[0]+a[1]*a[1],0.5);
00098         q=pow(b[0]*b[0]+b[1]*b[1],0.5);
00099         i=(a[0]*b[0]+a[1]*b[1])/p/q;
00100         xbee.printf("i:%f\r\n",i);
00101         w=acos(i);
00102 //tanの値を出す        
00103         e = (v-n)/(u-m);
00104         f = (n-y)/(m-x);
00105         c = (e-f)/(1+e*f);
00106         xbee.printf("c:%f\r\n",c);
00107 //t[0]は回る時間、t[1]はかかる時間、rに何回進むプログラムを動かすかを決める        
00108         t[0] = w/3.14159/0.9;
00109         t[1] = 2.0*j/3.0/1.2;
00110         xbee.printf("t[0]:%f\n",t[0]);
00111         xbee.printf("t[1]:%f\n",t[1]);
00112         wait(1);
00113         xbee.printf("c*i:%f\r\n", c*i);
00114         
00115 //実際に動くときのプログラム。1.2m/s計算、1.8s進み、0.4秒周りを見る。平均速度1.0m/s、基準を進行方向にしたとき見れる範囲:-43.5~43.5の予定
00116         
00117         if(m-x==0 && n-y==0){
00118             driveMotor(0,1); //直線上にある時は、18度だけ向きを変更
00119             wait(0.1);
00120             driveMotor(0,0);
00121             wait(3);
00122             }
00123         else if(c*i>0){
00124             driveMotor(0.5,0); //メモ：右回りがどっちか測定する
00125             wait(t[0]);
00126             driveMotor(0,0);
00127             wait(1);
00128             driveMotor(1,1);
00129             wait(t[1]);
00130             driveMotor(0.8,0.8);
00131             wait(1);
00132             driveMotor(0.5,0.5);
00133             wait(2);
00134             driveMotor(0.2,0.2);
00135             wait(2);
00136             driveMotor(0,0);
00137             }
00138         
00139         else if(c*i<0){
00140             driveMotor(0,0.5);
00141             wait(t[0]);
00142             driveMotor(0,0);
00143             wait(1);
00144             driveMotor(1,1);
00145             wait(t[1]);
00146             driveMotor(0.8,0.8);
00147             wait(1);
00148             driveMotor(0.5,0.5);
00149             wait(2);
00150             driveMotor(0.2,0.2);
00151             wait(2);
00152             driveMotor(0,0);
00153             }
00154             
00155 //0度と180度かの判断はあきらめて、0度と180度の時は、違う方角を向くようにした。
00156         else{
00157             driveMotor(1,0); //直線上にある時は、18度だけ向きを変更
00158             wait(0.1);
00159             driveMotor(0,0);
00160             wait(3);
00161             }
00162             
00163         //基準値を記録する
00164         while(gps.getgps() != true){
00165 //            xbee.printf("false\r\n");
00166         }
00167         gps.getgps();
00168         x = gps.latitude;
00169         y = gps.longitude;
00170 //5秒前進する
00171         driveMotor(0.8,0.8);
00172         wait(1);
00173         driveMotor(0.5,0.5);
00174         wait(2);
00175         driveMotor(0.2,0.2);
00176         wait(2);
00177         driveMotor(0,0);
00178 //m,nに現在の経度、緯度を記録する
00179 
00180         while(gps.getgps() != true){
00181 //            xbee.printf("false\r\n");
00182         }
00183         gps.getgps();
00184         m=gps.latitude;
00185         n=gps.longitude;
00186                     
00187 /*        xbee.printf("x:%f,y:%f\r\n", x, y);
00188         xbee.printf("m:%f,n:%f\r\n", m, n); 
00189 */
00190         
00191 //a,bの配列に、それぞれ緯度、経度の現在地と初期位置の差、jに目標までの距離を記録する        
00192         a[0]=m-x;
00193         a[1]=n-y;
00194         b[0]=u-m;
00195         b[1]=v-n;
00196         j=pow(b[0]/0.000008983148616*b[0]/0.000008983148616+b[1]/0.000010966382364*b[1]/0.000010966382364,0.5);
00197         
00198         }
00199      driveMotor(1,0);
00200      wait(3);
00201      driveMotor(0,0);
00202      }
00203     
00204