![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
zahyou kime hukume
main.cpp@2:3b1b6e825546, 2021-12-01 (annotated)
- Committer:
- kaipon
- Date:
- Wed Dec 01 05:42:15 2021 +0000
- Revision:
- 2:3b1b6e825546
- Parent:
- 1:5f317aa38c12
- Child:
- 3:6033c3517dc7
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kaipon | 0:c51bac912336 | 1 | #include <mbed.h> |
kaipon | 0:c51bac912336 | 2 | #include <getGPS.h> |
kaipon | 0:c51bac912336 | 3 | |
kaipon | 1:5f317aa38c12 | 4 | PwmOut pinAFin(D10); |
kaipon | 1:5f317aa38c12 | 5 | PwmOut pinARin(A1); |
kaipon | 0:c51bac912336 | 6 | PwmOut pinBFin(D11); |
kaipon | 0:c51bac912336 | 7 | PwmOut pinBRin(D12); |
kaipon | 0:c51bac912336 | 8 | GPS gps(D1, D0);// tx,rx |
kaipon | 1:5f317aa38c12 | 9 | Serial xbee(A7,A2); |
kaipon | 0:c51bac912336 | 10 | |
kaipon | 0:c51bac912336 | 11 | void driveMotor(int speedA,int speedB) { |
kaipon | 0:c51bac912336 | 12 | float outputA = abs(speedA); |
kaipon | 0:c51bac912336 | 13 | float outputB = abs(speedB); |
kaipon | 0:c51bac912336 | 14 | if (speedA > 0) { |
kaipon | 0:c51bac912336 | 15 | pinAFin=outputA; |
kaipon | 0:c51bac912336 | 16 | pinARin=0; |
kaipon | 0:c51bac912336 | 17 | } else if (speedA < 0) { |
kaipon | 0:c51bac912336 | 18 | pinAFin=0; |
kaipon | 0:c51bac912336 | 19 | pinARin=outputA; |
kaipon | 0:c51bac912336 | 20 | } else { |
kaipon | 0:c51bac912336 | 21 | pinAFin=0; |
kaipon | 0:c51bac912336 | 22 | pinARin=0; |
kaipon | 0:c51bac912336 | 23 | } |
kaipon | 0:c51bac912336 | 24 | if (speedB > 0) { |
kaipon | 0:c51bac912336 | 25 | pinBFin=outputB; |
kaipon | 0:c51bac912336 | 26 | pinBRin=0; |
kaipon | 0:c51bac912336 | 27 | } else if (speedB < 0) { |
kaipon | 0:c51bac912336 | 28 | pinBFin=0; |
kaipon | 0:c51bac912336 | 29 | pinBRin=outputB; |
kaipon | 0:c51bac912336 | 30 | } else { |
kaipon | 0:c51bac912336 | 31 | pinBFin=0; |
kaipon | 0:c51bac912336 | 32 | pinBRin=0; |
kaipon | 0:c51bac912336 | 33 | } |
kaipon | 0:c51bac912336 | 34 | } |
kaipon | 0:c51bac912336 | 35 | |
kaipon | 0:c51bac912336 | 36 | int main(){ |
kaipon | 0:c51bac912336 | 37 | double x,y,u,v,m,n,i,w,j,p,q,e,f,c; |
kaipon | 0:c51bac912336 | 38 | int r,s; |
kaipon | 1:5f317aa38c12 | 39 | u = 34.546402; |
kaipon | 1:5f317aa38c12 | 40 | v = 135.502126; |
kaipon | 1:5f317aa38c12 | 41 | s = 0; |
kaipon | 1:5f317aa38c12 | 42 | |
kaipon | 1:5f317aa38c12 | 43 | xbee.printf("XBee Connected\r\n"); |
kaipon | 1:5f317aa38c12 | 44 | while(gps.getgps() != true){ |
kaipon | 1:5f317aa38c12 | 45 | } |
kaipon | 1:5f317aa38c12 | 46 | |
kaipon | 1:5f317aa38c12 | 47 | //基準値を記録する |
kaipon | 0:c51bac912336 | 48 | gps.getgps(); |
kaipon | 0:c51bac912336 | 49 | x = gps.latitude; |
kaipon | 0:c51bac912336 | 50 | y = gps.longitude; |
kaipon | 2:3b1b6e825546 | 51 | xbee.printf("x:%f,y:%f\r\n", x, y); |
kaipon | 1:5f317aa38c12 | 52 | //10秒前進する |
kaipon | 1:5f317aa38c12 | 53 | driveMotor(1,1); |
kaipon | 1:5f317aa38c12 | 54 | wait(10);//メモ:ただ直進してる |
kaipon | 1:5f317aa38c12 | 55 | driveMotor(0,0); |
kaipon | 2:3b1b6e825546 | 56 | //m,nに現在の経度、緯度を記録する |
kaipon | 2:3b1b6e825546 | 57 | while(gps.getgps() != true){ |
kaipon | 2:3b1b6e825546 | 58 | xbee.printf("false\r\n"); |
kaipon | 2:3b1b6e825546 | 59 | } |
kaipon | 2:3b1b6e825546 | 60 | |
kaipon | 1:5f317aa38c12 | 61 | m=gps.latitude; |
kaipon | 1:5f317aa38c12 | 62 | n=gps.longitude; |
kaipon | 2:3b1b6e825546 | 63 | xbee.printf("m:%f,n:%f\r\n", m, n); |
kaipon | 0:c51bac912336 | 64 | //a,bの配列に、それぞれ緯度、経度の現在地と初期位置の差、jに目標までの距離を記録する |
kaipon | 1:5f317aa38c12 | 65 | double a[2]={m-x,n-y}; |
kaipon | 1:5f317aa38c12 | 66 | double b[2]={u-m,v-n}; |
kaipon | 1:5f317aa38c12 | 67 | j=pow(b[0]/0.000008983148616*b[0]/0.000008983148616+b[1]/0.000010966382364*b[1]/0.000010966382364,0.5); |
kaipon | 2:3b1b6e825546 | 68 | xbee.printf("距離:%f\r\n", j); |
kaipon | 1:5f317aa38c12 | 69 | |
kaipon | 1:5f317aa38c12 | 70 | |
kaipon | 2:3b1b6e825546 | 71 | while(j>5){ |
kaipon | 1:5f317aa38c12 | 72 | |
kaipon | 0:c51bac912336 | 73 | //cosと角度の値を出す |
kaipon | 0:c51bac912336 | 74 | p=pow(a[0]*a[0]+a[1]*a[1],0.5); |
kaipon | 0:c51bac912336 | 75 | q=pow(b[0]*b[0]+b[1]*b[1],0.5); |
kaipon | 1:5f317aa38c12 | 76 | i=(a[0]*b[0]+a[1]*b[1])/p/q; |
kaipon | 2:3b1b6e825546 | 77 | xbee.printf("i:%f\r\n",i); |
kaipon | 0:c51bac912336 | 78 | w=acos(i); |
kaipon | 0:c51bac912336 | 79 | //tanの値を出す |
kaipon | 0:c51bac912336 | 80 | e = (v-n)/(u-m); |
kaipon | 0:c51bac912336 | 81 | f = (n-y)/(m-x); |
kaipon | 0:c51bac912336 | 82 | c = (e-f)/(1+e*f); |
kaipon | 2:3b1b6e825546 | 83 | xbee.printf("c:%f\r\n",c); |
kaipon | 0:c51bac912336 | 84 | //t[0]は回る時間、t[1]はかかる時間、rに何回進むプログラムを動かすかを決める |
kaipon | 0:c51bac912336 | 85 | double t[2] = {w/3.14159/2,2*j/3/1.2}; //再度図るのは、全体の2/3だけ進んだとき |
kaipon | 0:c51bac912336 | 86 | r = t[1]/2.2; |
kaipon | 1:5f317aa38c12 | 87 | wait(1); |
kaipon | 2:3b1b6e825546 | 88 | xbee.printf("c*i:%f\r\n", c*i); |
kaipon | 0:c51bac912336 | 89 | |
kaipon | 0:c51bac912336 | 90 | //実際に動くときのプログラム。1.2m/s計算、1.8s進み、0.4秒周りを見る。平均速度1.0m/s、基準を進行方向にしたとき見れる範囲:-43.5~43.5の予定 |
kaipon | 0:c51bac912336 | 91 | |
kaipon | 0:c51bac912336 | 92 | if(c*i>0){ |
kaipon | 0:c51bac912336 | 93 | driveMotor(1,0); //メモ:右回りがどっちか測定する |
kaipon | 0:c51bac912336 | 94 | wait(t[0]); |
kaipon | 0:c51bac912336 | 95 | driveMotor(0,0); |
kaipon | 0:c51bac912336 | 96 | while(s == r){ |
kaipon | 0:c51bac912336 | 97 | driveMotor(1,1); |
kaipon | 0:c51bac912336 | 98 | wait(1.8); |
kaipon | 0:c51bac912336 | 99 | driveMotor(0,0); |
kaipon | 1:5f317aa38c12 | 100 | /*超音波センサの起動。首を振る |
kaipon | 0:c51bac912336 | 101 | driveMotor(0,1); |
kaipon | 0:c51bac912336 | 102 | wait(0.1); |
kaipon | 0:c51bac912336 | 103 | driveMotor(1,0); |
kaipon | 0:c51bac912336 | 104 | wait(0.2); |
kaipon | 0:c51bac912336 | 105 | driveMotor(0,1); |
kaipon | 0:c51bac912336 | 106 | wait(0.1); |
kaipon | 1:5f317aa38c12 | 107 | //超音波センサ終了*/ |
kaipon | 0:c51bac912336 | 108 | s++; |
kaipon | 0:c51bac912336 | 109 | } |
kaipon | 0:c51bac912336 | 110 | } |
kaipon | 0:c51bac912336 | 111 | |
kaipon | 0:c51bac912336 | 112 | else if(c*i<0){ |
kaipon | 0:c51bac912336 | 113 | driveMotor(0,1); |
kaipon | 0:c51bac912336 | 114 | wait(t[0]); |
kaipon | 0:c51bac912336 | 115 | driveMotor(0,0); |
kaipon | 0:c51bac912336 | 116 | while(s == r){ |
kaipon | 0:c51bac912336 | 117 | driveMotor(1,1); |
kaipon | 0:c51bac912336 | 118 | wait(1.8); |
kaipon | 0:c51bac912336 | 119 | driveMotor(0,0); |
kaipon | 1:5f317aa38c12 | 120 | /*超音波センサの起動。首を振る |
kaipon | 0:c51bac912336 | 121 | driveMotor(0,1); |
kaipon | 0:c51bac912336 | 122 | wait(0.1); |
kaipon | 0:c51bac912336 | 123 | driveMotor(1,0); |
kaipon | 0:c51bac912336 | 124 | wait(0.2); |
kaipon | 0:c51bac912336 | 125 | driveMotor(0,1); |
kaipon | 0:c51bac912336 | 126 | wait(0.1); |
kaipon | 1:5f317aa38c12 | 127 | //超音波センサ終了*/ |
kaipon | 0:c51bac912336 | 128 | s++; |
kaipon | 0:c51bac912336 | 129 | } |
kaipon | 0:c51bac912336 | 130 | } |
kaipon | 0:c51bac912336 | 131 | |
kaipon | 0:c51bac912336 | 132 | //0度と180度かの判断はあきらめて、0度と180度の時は、違う方角を向くようにした。 |
kaipon | 0:c51bac912336 | 133 | else{ |
kaipon | 0:c51bac912336 | 134 | driveMotor(1,0); //直線上にある時は、18度だけ向きを変更 |
kaipon | 0:c51bac912336 | 135 | wait(0.1); |
kaipon | 0:c51bac912336 | 136 | driveMotor(0,0); |
kaipon | 2:3b1b6e825546 | 137 | wait(3); |
kaipon | 0:c51bac912336 | 138 | } |
kaipon | 1:5f317aa38c12 | 139 | |
kaipon | 1:5f317aa38c12 | 140 | //基準値を記録する |
kaipon | 2:3b1b6e825546 | 141 | while(gps.getgps() != true){ |
kaipon | 2:3b1b6e825546 | 142 | // xbee.printf("false\r\n"); |
kaipon | 2:3b1b6e825546 | 143 | } |
kaipon | 1:5f317aa38c12 | 144 | x = gps.latitude; |
kaipon | 1:5f317aa38c12 | 145 | y = gps.longitude; |
kaipon | 1:5f317aa38c12 | 146 | //5秒前進する |
kaipon | 1:5f317aa38c12 | 147 | driveMotor(1,1); |
kaipon | 1:5f317aa38c12 | 148 | wait(5);//メモ:ただ直進してる |
kaipon | 1:5f317aa38c12 | 149 | driveMotor(0,0); |
kaipon | 1:5f317aa38c12 | 150 | //m,nに現在の経度、緯度を記録する |
kaipon | 2:3b1b6e825546 | 151 | |
kaipon | 2:3b1b6e825546 | 152 | while(gps.getgps() != true){ |
kaipon | 2:3b1b6e825546 | 153 | xbee.printf("false\r\n"); |
kaipon | 2:3b1b6e825546 | 154 | } |
kaipon | 1:5f317aa38c12 | 155 | m=gps.latitude; |
kaipon | 1:5f317aa38c12 | 156 | n=gps.longitude; |
kaipon | 1:5f317aa38c12 | 157 | |
kaipon | 2:3b1b6e825546 | 158 | xbee.printf("x:%f,y:%f\r\n", x, y); |
kaipon | 2:3b1b6e825546 | 159 | xbee.printf("m:%f,n:%f\r\n", m, n); |
kaipon | 1:5f317aa38c12 | 160 | |
kaipon | 1:5f317aa38c12 | 161 | //a,bの配列に、それぞれ緯度、経度の現在地と初期位置の差、jに目標までの距離を記録する |
kaipon | 1:5f317aa38c12 | 162 | a[0]=m-x; |
kaipon | 1:5f317aa38c12 | 163 | a[1]=n-y; |
kaipon | 1:5f317aa38c12 | 164 | b[0]=u-m; |
kaipon | 1:5f317aa38c12 | 165 | b[1]=v-n; |
kaipon | 1:5f317aa38c12 | 166 | j=pow(b[0]/0.000008983148616*b[0]/0.000008983148616+b[1]/0.000010966382364*b[1]/0.000010966382364,0.5); |
kaipon | 1:5f317aa38c12 | 167 | |
kaipon | 0:c51bac912336 | 168 | } |
kaipon | 0:c51bac912336 | 169 | } |
kaipon | 0:c51bac912336 | 170 | |
kaipon | 0:c51bac912336 | 171 |