cansat-e_2021 / Mbed 2 deprecated driving_1

for testing about driving 1

Dependencies:   mbed getGPS

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?

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 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