足立 快仁 / Mbed 2 deprecated driving_1_2

zahyou kime hukume

Dependencies:   mbed getGPS

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?

UserRevisionLine numberNew 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で測れなかった時５回だけ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