![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
zahyou kime hukume
main.cpp@3:6033c3517dc7, 2021-12-01 (annotated)
- Committer:
- kaipon
- Date:
- Wed Dec 01 07:10:14 2021 +0000
- Revision:
- 3:6033c3517dc7
- Parent:
- 2:3b1b6e825546
- Child:
- 4:962ecf61540b
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 | 3:6033c3517dc7 | 11 | void driveMotor(double speedA,double 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 | 3:6033c3517dc7 | 38 | int r,s,k; |
kaipon | 3:6033c3517dc7 | 39 | u = 34.546366; |
kaipon | 3:6033c3517dc7 | 40 | v = 135.502015; |
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 | 3:6033c3517dc7 | 54 | wait(5);//メモ:ただ直進してる |
kaipon | 3:6033c3517dc7 | 55 | driveMotor(0.8,0.8); |
kaipon | 3:6033c3517dc7 | 56 | wait(1); |
kaipon | 3:6033c3517dc7 | 57 | driveMotor(0.5,0.5); |
kaipon | 3:6033c3517dc7 | 58 | wait(2); |
kaipon | 3:6033c3517dc7 | 59 | driveMotor(0.2,0.2); |
kaipon | 3:6033c3517dc7 | 60 | wait(2); |
kaipon | 1:5f317aa38c12 | 61 | driveMotor(0,0); |
kaipon | 2:3b1b6e825546 | 62 | //m,nに現在の経度、緯度を記録する |
kaipon | 2:3b1b6e825546 | 63 | while(gps.getgps() != true){ |
kaipon | 2:3b1b6e825546 | 64 | xbee.printf("false\r\n"); |
kaipon | 2:3b1b6e825546 | 65 | } |
kaipon | 2:3b1b6e825546 | 66 | |
kaipon | 1:5f317aa38c12 | 67 | m=gps.latitude; |
kaipon | 1:5f317aa38c12 | 68 | n=gps.longitude; |
kaipon | 2:3b1b6e825546 | 69 | xbee.printf("m:%f,n:%f\r\n", m, n); |
kaipon | 0:c51bac912336 | 70 | //a,bの配列に、それぞれ緯度、経度の現在地と初期位置の差、jに目標までの距離を記録する |
kaipon | 1:5f317aa38c12 | 71 | double a[2]={m-x,n-y}; |
kaipon | 1:5f317aa38c12 | 72 | double b[2]={u-m,v-n}; |
kaipon | 1:5f317aa38c12 | 73 | j=pow(b[0]/0.000008983148616*b[0]/0.000008983148616+b[1]/0.000010966382364*b[1]/0.000010966382364,0.5); |
kaipon | 2:3b1b6e825546 | 74 | xbee.printf("距離:%f\r\n", j); |
kaipon | 1:5f317aa38c12 | 75 | |
kaipon | 1:5f317aa38c12 | 76 | |
kaipon | 2:3b1b6e825546 | 77 | while(j>5){ |
kaipon | 1:5f317aa38c12 | 78 | |
kaipon | 0:c51bac912336 | 79 | //cosと角度の値を出す |
kaipon | 0:c51bac912336 | 80 | p=pow(a[0]*a[0]+a[1]*a[1],0.5); |
kaipon | 0:c51bac912336 | 81 | q=pow(b[0]*b[0]+b[1]*b[1],0.5); |
kaipon | 1:5f317aa38c12 | 82 | i=(a[0]*b[0]+a[1]*b[1])/p/q; |
kaipon | 2:3b1b6e825546 | 83 | xbee.printf("i:%f\r\n",i); |
kaipon | 0:c51bac912336 | 84 | w=acos(i); |
kaipon | 0:c51bac912336 | 85 | //tanの値を出す |
kaipon | 0:c51bac912336 | 86 | e = (v-n)/(u-m); |
kaipon | 0:c51bac912336 | 87 | f = (n-y)/(m-x); |
kaipon | 0:c51bac912336 | 88 | c = (e-f)/(1+e*f); |
kaipon | 2:3b1b6e825546 | 89 | xbee.printf("c:%f\r\n",c); |
kaipon | 0:c51bac912336 | 90 | //t[0]は回る時間、t[1]はかかる時間、rに何回進むプログラムを動かすかを決める |
kaipon | 0:c51bac912336 | 91 | double t[2] = {w/3.14159/2,2*j/3/1.2}; //再度図るのは、全体の2/3だけ進んだとき |
kaipon | 0:c51bac912336 | 92 | r = t[1]/2.2; |
kaipon | 1:5f317aa38c12 | 93 | wait(1); |
kaipon | 2:3b1b6e825546 | 94 | xbee.printf("c*i:%f\r\n", c*i); |
kaipon | 0:c51bac912336 | 95 | |
kaipon | 0:c51bac912336 | 96 | //実際に動くときのプログラム。1.2m/s計算、1.8s進み、0.4秒周りを見る。平均速度1.0m/s、基準を進行方向にしたとき見れる範囲:-43.5~43.5の予定 |
kaipon | 0:c51bac912336 | 97 | |
kaipon | 0:c51bac912336 | 98 | if(c*i>0){ |
kaipon | 0:c51bac912336 | 99 | driveMotor(1,0); //メモ:右回りがどっちか測定する |
kaipon | 0:c51bac912336 | 100 | wait(t[0]); |
kaipon | 0:c51bac912336 | 101 | driveMotor(0,0); |
kaipon | 3:6033c3517dc7 | 102 | while(s == r-5){ |
kaipon | 0:c51bac912336 | 103 | driveMotor(1,1); |
kaipon | 0:c51bac912336 | 104 | wait(1.8); |
kaipon | 0:c51bac912336 | 105 | driveMotor(0,0); |
kaipon | 1:5f317aa38c12 | 106 | /*超音波センサの起動。首を振る |
kaipon | 0:c51bac912336 | 107 | driveMotor(0,1); |
kaipon | 0:c51bac912336 | 108 | wait(0.1); |
kaipon | 0:c51bac912336 | 109 | driveMotor(1,0); |
kaipon | 0:c51bac912336 | 110 | wait(0.2); |
kaipon | 0:c51bac912336 | 111 | driveMotor(0,1); |
kaipon | 0:c51bac912336 | 112 | wait(0.1); |
kaipon | 1:5f317aa38c12 | 113 | //超音波センサ終了*/ |
kaipon | 0:c51bac912336 | 114 | s++; |
kaipon | 0:c51bac912336 | 115 | } |
kaipon | 3:6033c3517dc7 | 116 | driveMotor(0.8,0.8); |
kaipon | 3:6033c3517dc7 | 117 | wait(1); |
kaipon | 3:6033c3517dc7 | 118 | driveMotor(0.5,0.5); |
kaipon | 3:6033c3517dc7 | 119 | wait(2); |
kaipon | 3:6033c3517dc7 | 120 | driveMotor(0.2,0.2); |
kaipon | 3:6033c3517dc7 | 121 | wait(2); |
kaipon | 3:6033c3517dc7 | 122 | driveMotor(0,0); |
kaipon | 0:c51bac912336 | 123 | } |
kaipon | 0:c51bac912336 | 124 | |
kaipon | 0:c51bac912336 | 125 | else if(c*i<0){ |
kaipon | 0:c51bac912336 | 126 | driveMotor(0,1); |
kaipon | 0:c51bac912336 | 127 | wait(t[0]); |
kaipon | 0:c51bac912336 | 128 | driveMotor(0,0); |
kaipon | 3:6033c3517dc7 | 129 | while(s == r-5){ |
kaipon | 0:c51bac912336 | 130 | driveMotor(1,1); |
kaipon | 0:c51bac912336 | 131 | wait(1.8); |
kaipon | 0:c51bac912336 | 132 | driveMotor(0,0); |
kaipon | 1:5f317aa38c12 | 133 | /*超音波センサの起動。首を振る |
kaipon | 0:c51bac912336 | 134 | driveMotor(0,1); |
kaipon | 0:c51bac912336 | 135 | wait(0.1); |
kaipon | 0:c51bac912336 | 136 | driveMotor(1,0); |
kaipon | 0:c51bac912336 | 137 | wait(0.2); |
kaipon | 0:c51bac912336 | 138 | driveMotor(0,1); |
kaipon | 0:c51bac912336 | 139 | wait(0.1); |
kaipon | 1:5f317aa38c12 | 140 | //超音波センサ終了*/ |
kaipon | 0:c51bac912336 | 141 | s++; |
kaipon | 0:c51bac912336 | 142 | } |
kaipon | 3:6033c3517dc7 | 143 | driveMotor(0.8,0.8); |
kaipon | 3:6033c3517dc7 | 144 | wait(1); |
kaipon | 3:6033c3517dc7 | 145 | driveMotor(0.5,0.5); |
kaipon | 3:6033c3517dc7 | 146 | wait(2); |
kaipon | 3:6033c3517dc7 | 147 | driveMotor(0.2,0.2); |
kaipon | 3:6033c3517dc7 | 148 | wait(2); |
kaipon | 3:6033c3517dc7 | 149 | driveMotor(0,0); |
kaipon | 0:c51bac912336 | 150 | } |
kaipon | 0:c51bac912336 | 151 | |
kaipon | 0:c51bac912336 | 152 | //0度と180度かの判断はあきらめて、0度と180度の時は、違う方角を向くようにした。 |
kaipon | 0:c51bac912336 | 153 | else{ |
kaipon | 0:c51bac912336 | 154 | driveMotor(1,0); //直線上にある時は、18度だけ向きを変更 |
kaipon | 0:c51bac912336 | 155 | wait(0.1); |
kaipon | 0:c51bac912336 | 156 | driveMotor(0,0); |
kaipon | 2:3b1b6e825546 | 157 | wait(3); |
kaipon | 0:c51bac912336 | 158 | } |
kaipon | 1:5f317aa38c12 | 159 | |
kaipon | 1:5f317aa38c12 | 160 | //基準値を記録する |
kaipon | 2:3b1b6e825546 | 161 | while(gps.getgps() != true){ |
kaipon | 2:3b1b6e825546 | 162 | // xbee.printf("false\r\n"); |
kaipon | 2:3b1b6e825546 | 163 | } |
kaipon | 1:5f317aa38c12 | 164 | x = gps.latitude; |
kaipon | 1:5f317aa38c12 | 165 | y = gps.longitude; |
kaipon | 1:5f317aa38c12 | 166 | //5秒前進する |
kaipon | 3:6033c3517dc7 | 167 | driveMotor(0.8,0.8); |
kaipon | 3:6033c3517dc7 | 168 | wait(1); |
kaipon | 3:6033c3517dc7 | 169 | driveMotor(0.5,0.5); |
kaipon | 3:6033c3517dc7 | 170 | wait(2); |
kaipon | 3:6033c3517dc7 | 171 | driveMotor(0.2,0.2); |
kaipon | 3:6033c3517dc7 | 172 | wait(2); |
kaipon | 1:5f317aa38c12 | 173 | driveMotor(0,0); |
kaipon | 3:6033c3517dc7 | 174 | //m,nに現在の経度、緯度を記録する |
kaipon | 3:6033c3517dc7 | 175 | |
kaipon | 1:5f317aa38c12 | 176 | m=gps.latitude; |
kaipon | 1:5f317aa38c12 | 177 | n=gps.longitude; |
kaipon | 3:6033c3517dc7 | 178 | a[0]=m-x; |
kaipon | 3:6033c3517dc7 | 179 | a[1]=n-y; |
kaipon | 3:6033c3517dc7 | 180 | k = 0; |
kaipon | 3:6033c3517dc7 | 181 | while(a[0] == 0 || a[1] == 0 || k < 5){ |
kaipon | 3:6033c3517dc7 | 182 | driveMotor(1,1); |
kaipon | 3:6033c3517dc7 | 183 | wait(1); |
kaipon | 3:6033c3517dc7 | 184 | driveMotor(0,0); |
kaipon | 3:6033c3517dc7 | 185 | wait(1); |
kaipon | 3:6033c3517dc7 | 186 | while(gps.getgps() != true){ |
kaipon | 3:6033c3517dc7 | 187 | xbee.printf("false\r\n"); |
kaipon | 3:6033c3517dc7 | 188 | } |
kaipon | 3:6033c3517dc7 | 189 | m=gps.latitude; |
kaipon | 3:6033c3517dc7 | 190 | n=gps.longitude; |
kaipon | 3:6033c3517dc7 | 191 | a[0]=m-x; |
kaipon | 3:6033c3517dc7 | 192 | a[1]=n-y; |
kaipon | 3:6033c3517dc7 | 193 | k++; |
kaipon | 3:6033c3517dc7 | 194 | } |
kaipon | 3:6033c3517dc7 | 195 | |
kaipon | 2:3b1b6e825546 | 196 | xbee.printf("x:%f,y:%f\r\n", x, y); |
kaipon | 2:3b1b6e825546 | 197 | xbee.printf("m:%f,n:%f\r\n", m, n); |
kaipon | 1:5f317aa38c12 | 198 | |
kaipon | 1:5f317aa38c12 | 199 | //a,bの配列に、それぞれ緯度、経度の現在地と初期位置の差、jに目標までの距離を記録する |
kaipon | 1:5f317aa38c12 | 200 | a[0]=m-x; |
kaipon | 1:5f317aa38c12 | 201 | a[1]=n-y; |
kaipon | 1:5f317aa38c12 | 202 | b[0]=u-m; |
kaipon | 1:5f317aa38c12 | 203 | b[1]=v-n; |
kaipon | 1:5f317aa38c12 | 204 | j=pow(b[0]/0.000008983148616*b[0]/0.000008983148616+b[1]/0.000010966382364*b[1]/0.000010966382364,0.5); |
kaipon | 1:5f317aa38c12 | 205 | |
kaipon | 0:c51bac912336 | 206 | } |
kaipon | 3:6033c3517dc7 | 207 | driveMotor(1,0); |
kaipon | 3:6033c3517dc7 | 208 | wait(3); |
kaipon | 3:6033c3517dc7 | 209 | driveMotor(0,0); |
kaipon | 0:c51bac912336 | 210 | } |
kaipon | 0:c51bac912336 | 211 | |
kaipon | 0:c51bac912336 | 212 |