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