cansat-e_2021 / Mbed 2 deprecated driving_1

for testing about driving 1

Dependencies:   mbed getGPS

main.cpp@8:8aca0c7419ab, 2021-12-18 (annotated)

Committer:
kaipon
Date:
Sat Dec 18 23:52:04 2021 +0000
Revision:
8:8aca0c7419ab
Parent:
7:9ec83a204ae5 
nemu

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 8:8aca0c7419ab 4 DigitalOut pin(D7);
kaipon 8:8aca0c7419ab 5 DigitalIn pin1(D6);
kaipon 1:5f317aa38c12 6 PwmOut pinAFin(D10);
kaipon 1:5f317aa38c12 7 PwmOut pinARin(A1);
kaipon 0:c51bac912336 8 PwmOut pinBFin(D11);
kaipon 0:c51bac912336 9 PwmOut pinBRin(D12);
kaipon 0:c51bac912336 10 GPS gps(D1, D0);// tx,rx
kaipon 8:8aca0c7419ab 11 Serial xbee(SERIAL_TX, SERIAL_RX);
kaipon 0:c51bac912336 12
kaipon 3:6033c3517dc7 13 void driveMotor(double speedA,double speedB) {
kaipon 0:c51bac912336 14 float outputA = abs(speedA);
kaipon 0:c51bac912336 15 float outputB = abs(speedB);
kaipon 0:c51bac912336 16 if (speedA > 0) {
kaipon 0:c51bac912336 17 pinAFin=outputA;
kaipon 0:c51bac912336 18 pinARin=0;
kaipon 0:c51bac912336 19 } else if (speedA < 0) {
kaipon 0:c51bac912336 20 pinAFin=0;
kaipon 0:c51bac912336 21 pinARin=outputA;
kaipon 0:c51bac912336 22 } else {
kaipon 0:c51bac912336 23 pinAFin=0;
kaipon 0:c51bac912336 24 pinARin=0;
kaipon 0:c51bac912336 25 }
kaipon 0:c51bac912336 26 if (speedB > 0) {
kaipon 0:c51bac912336 27 pinBFin=outputB;
kaipon 0:c51bac912336 28 pinBRin=0;
kaipon 0:c51bac912336 29 } else if (speedB < 0) {
kaipon 0:c51bac912336 30 pinBFin=0;
kaipon 0:c51bac912336 31 pinBRin=outputB;
kaipon 0:c51bac912336 32 } else {
kaipon 0:c51bac912336 33 pinBFin=0;
kaipon 0:c51bac912336 34 pinBRin=0;
kaipon 0:c51bac912336 35 }
kaipon 0:c51bac912336 36 }
kaipon 0:c51bac912336 37
kaipon 0:c51bac912336 38 int main(){
kaipon 8:8aca0c7419ab 39 pin1.mode(PullUp);
kaipon 8:8aca0c7419ab 40 while(1){
kaipon 8:8aca0c7419ab 41 if(pin1 == 1){
kaipon 8:8aca0c7419ab 42 break;
kaipon 8:8aca0c7419ab 43 }
kaipon 8:8aca0c7419ab 44 }
kaipon 8:8aca0c7419ab 45
kaipon 8:8aca0c7419ab 46 wait(50);
kaipon 8:8aca0c7419ab 47 pin = 1;
kaipon 8:8aca0c7419ab 48 wait(17);
kaipon 8:8aca0c7419ab 49 pin = 0;
kaipon 8:8aca0c7419ab 50 wait(10);
kaipon 8:8aca0c7419ab 51
kaipon 8:8aca0c7419ab 52
kaipon 0:c51bac912336 53 double x,y,u,v,m,n,i,w,j,p,q,e,f,c;
kaipon 8:8aca0c7419ab 54 int r,s;
kaipon 8:8aca0c7419ab 55 u = 34.546385;
kaipon 8:8aca0c7419ab 56 v = 135.502085;
kaipon 1:5f317aa38c12 57 s = 0;
kaipon 8:8aca0c7419ab 58 double t[2] = {0,0};
kaipon 1:5f317aa38c12 59
kaipon 8:8aca0c7419ab 60 // xbee.printf("XBee Connected\r\n");
kaipon 1:5f317aa38c12 61 while(gps.getgps() != true){
kaipon 1:5f317aa38c12 62 }
kaipon 1:5f317aa38c12 63
kaipon 1:5f317aa38c12 64 //基準値を記録する
kaipon 0:c51bac912336 65 gps.getgps();
kaipon 0:c51bac912336 66 x = gps.latitude;
kaipon 0:c51bac912336 67 y = gps.longitude;
kaipon 8:8aca0c7419ab 68 // xbee.printf("x:%f,y:%f\r\n", x, y);
kaipon 8:8aca0c7419ab 69
kaipon 1:5f317aa38c12 70 //10秒前進する
kaipon 1:5f317aa38c12 71 driveMotor(1,1);
kaipon 3:6033c3517dc7 72 wait(5);//メモ：ただ直進してる
kaipon 3:6033c3517dc7 73 driveMotor(0.8,0.8);
kaipon 3:6033c3517dc7 74 wait(1);
kaipon 3:6033c3517dc7 75 driveMotor(0.5,0.5);
kaipon 3:6033c3517dc7 76 wait(2);
kaipon 3:6033c3517dc7 77 driveMotor(0.2,0.2);
kaipon 3:6033c3517dc7 78 wait(2);
kaipon 1:5f317aa38c12 79 driveMotor(0,0);
kaipon 2:3b1b6e825546 80 //m,nに現在の経度、緯度を記録する
kaipon 2:3b1b6e825546 81 while(gps.getgps() != true){
kaipon 2:3b1b6e825546 82 }
kaipon 8:8aca0c7419ab 83 gps.getgps();
kaipon 1:5f317aa38c12 84 m=gps.latitude;
kaipon 1:5f317aa38c12 85 n=gps.longitude;
kaipon 2:3b1b6e825546 86 xbee.printf("m:%f,n:%f\r\n", m, n);
kaipon 0:c51bac912336 87 //a,bの配列に、それぞれ緯度、経度の現在地と初期位置の差、jに目標までの距離を記録する
kaipon 1:5f317aa38c12 88 double a[2]={m-x,n-y};
kaipon 1:5f317aa38c12 89 double b[2]={u-m,v-n};
kaipon 1:5f317aa38c12 90 j=pow(b[0]/0.000008983148616*b[0]/0.000008983148616+b[1]/0.000010966382364*b[1]/0.000010966382364,0.5);
kaipon 2:3b1b6e825546 91 xbee.printf("距離:%f\r\n", j);
kaipon 1:5f317aa38c12 92
kaipon 1:5f317aa38c12 93
kaipon 2:3b1b6e825546 94 while(j>5){
kaipon 1:5f317aa38c12 95
kaipon 0:c51bac912336 96 //cosと角度の値を出す
kaipon 0:c51bac912336 97 p=pow(a[0]*a[0]+a[1]*a[1],0.5);
kaipon 0:c51bac912336 98 q=pow(b[0]*b[0]+b[1]*b[1],0.5);
kaipon 1:5f317aa38c12 99 i=(a[0]*b[0]+a[1]*b[1])/p/q;
kaipon 2:3b1b6e825546 100 xbee.printf("i:%f\r\n",i);
kaipon 0:c51bac912336 101 w=acos(i);
kaipon 0:c51bac912336 102 //tanの値を出す
kaipon 0:c51bac912336 103 e = (v-n)/(u-m);
kaipon 0:c51bac912336 104 f = (n-y)/(m-x);
kaipon 0:c51bac912336 105 c = (e-f)/(1+e*f);
kaipon 2:3b1b6e825546 106 xbee.printf("c:%f\r\n",c);
kaipon 0:c51bac912336 107 //t[0]は回る時間、t[1]はかかる時間、rに何回進むプログラムを動かすかを決める
kaipon 8:8aca0c7419ab 108 t[0] = w/3.14159/0.9;
kaipon 8:8aca0c7419ab 109 t[1] = 2.0*j/3.0/1.2;
kaipon 8:8aca0c7419ab 110 xbee.printf("t[0]:%f\n",t[0]);
kaipon 8:8aca0c7419ab 111 xbee.printf("t[1]:%f\n",t[1]);
kaipon 1:5f317aa38c12 112 wait(1);
kaipon 2:3b1b6e825546 113 xbee.printf("c*i:%f\r\n", c*i);
kaipon 0:c51bac912336 114
kaipon 0:c51bac912336 115 //実際に動くときのプログラム。1.2m/s計算、1.8s進み、0.4秒周りを見る。平均速度1.0m/s、基準を進行方向にしたとき見れる範囲:-43.5~43.5の予定
kaipon 0:c51bac912336 116
kaipon 8:8aca0c7419ab 117 if(m-x==0 && n-y==0){
kaipon 8:8aca0c7419ab 118 driveMotor(0,1); //直線上にある時は、18度だけ向きを変更
kaipon 8:8aca0c7419ab 119 wait(0.1);
kaipon 8:8aca0c7419ab 120 driveMotor(0,0);
kaipon 8:8aca0c7419ab 121 wait(3);
kaipon 8:8aca0c7419ab 122 }
kaipon 8:8aca0c7419ab 123 else if(c*i>0){
kaipon 4:962ecf61540b 124 driveMotor(0.5,0); //メモ：右回りがどっちか測定する
kaipon 0:c51bac912336 125 wait(t[0]);
kaipon 0:c51bac912336 126 driveMotor(0,0);
kaipon 8:8aca0c7419ab 127 wait(1);
kaipon 8:8aca0c7419ab 128 driveMotor(1,1);
kaipon 8:8aca0c7419ab 129 wait(t[1]);
kaipon 3:6033c3517dc7 130 driveMotor(0.8,0.8);
kaipon 3:6033c3517dc7 131 wait(1);
kaipon 3:6033c3517dc7 132 driveMotor(0.5,0.5);
kaipon 3:6033c3517dc7 133 wait(2);
kaipon 3:6033c3517dc7 134 driveMotor(0.2,0.2);
kaipon 3:6033c3517dc7 135 wait(2);
kaipon 3:6033c3517dc7 136 driveMotor(0,0);
kaipon 0:c51bac912336 137 }
kaipon 0:c51bac912336 138
kaipon 0:c51bac912336 139 else if(c*i<0){
kaipon 4:962ecf61540b 140 driveMotor(0,0.5);
kaipon 0:c51bac912336 141 wait(t[0]);
kaipon 0:c51bac912336 142 driveMotor(0,0);
kaipon 8:8aca0c7419ab 143 wait(1);
kaipon 8:8aca0c7419ab 144 driveMotor(1,1);
kaipon 8:8aca0c7419ab 145 wait(t[1]);
kaipon 3:6033c3517dc7 146 driveMotor(0.8,0.8);
kaipon 3:6033c3517dc7 147 wait(1);
kaipon 3:6033c3517dc7 148 driveMotor(0.5,0.5);
kaipon 3:6033c3517dc7 149 wait(2);
kaipon 3:6033c3517dc7 150 driveMotor(0.2,0.2);
kaipon 3:6033c3517dc7 151 wait(2);
kaipon 3:6033c3517dc7 152 driveMotor(0,0);
kaipon 0:c51bac912336 153 }
kaipon 0:c51bac912336 154
kaipon 0:c51bac912336 155 //0度と180度かの判断はあきらめて、0度と180度の時は、違う方角を向くようにした。
kaipon 0:c51bac912336 156 else{
kaipon 0:c51bac912336 157 driveMotor(1,0); //直線上にある時は、18度だけ向きを変更
kaipon 0:c51bac912336 158 wait(0.1);
kaipon 0:c51bac912336 159 driveMotor(0,0);
kaipon 2:3b1b6e825546 160 wait(3);
kaipon 0:c51bac912336 161 }
kaipon 1:5f317aa38c12 162
kaipon 1:5f317aa38c12 163 //基準値を記録する
kaipon 2:3b1b6e825546 164 while(gps.getgps() != true){
kaipon 2:3b1b6e825546 165 // xbee.printf("false\r\n");
kaipon 2:3b1b6e825546 166 }
kaipon 8:8aca0c7419ab 167 gps.getgps();
kaipon 1:5f317aa38c12 168 x = gps.latitude;
kaipon 1:5f317aa38c12 169 y = gps.longitude;
kaipon 1:5f317aa38c12 170 //5秒前進する
kaipon 3:6033c3517dc7 171 driveMotor(0.8,0.8);
kaipon 3:6033c3517dc7 172 wait(1);
kaipon 3:6033c3517dc7 173 driveMotor(0.5,0.5);
kaipon 3:6033c3517dc7 174 wait(2);
kaipon 3:6033c3517dc7 175 driveMotor(0.2,0.2);
kaipon 3:6033c3517dc7 176 wait(2);
kaipon 1:5f317aa38c12 177 driveMotor(0,0);
kaipon 3:6033c3517dc7 178 //m,nに現在の経度、緯度を記録する
kaipon 3:6033c3517dc7 179
kaipon 8:8aca0c7419ab 180 while(gps.getgps() != true){
kaipon 8:8aca0c7419ab 181 // xbee.printf("false\r\n");
kaipon 8:8aca0c7419ab 182 }
kaipon 8:8aca0c7419ab 183 gps.getgps();
kaipon 1:5f317aa38c12 184 m=gps.latitude;
kaipon 1:5f317aa38c12 185 n=gps.longitude;
kaipon 3:6033c3517dc7 186
kaipon 8:8aca0c7419ab 187 /* xbee.printf("x:%f,y:%f\r\n", x, y);
kaipon 2:3b1b6e825546 188 xbee.printf("m:%f,n:%f\r\n", m, n);
kaipon 8:8aca0c7419ab 189 */
kaipon 1:5f317aa38c12 190
kaipon 1:5f317aa38c12 191 //a,bの配列に、それぞれ緯度、経度の現在地と初期位置の差、jに目標までの距離を記録する
kaipon 1:5f317aa38c12 192 a[0]=m-x;
kaipon 1:5f317aa38c12 193 a[1]=n-y;
kaipon 1:5f317aa38c12 194 b[0]=u-m;
kaipon 1:5f317aa38c12 195 b[1]=v-n;
kaipon 1:5f317aa38c12 196 j=pow(b[0]/0.000008983148616*b[0]/0.000008983148616+b[1]/0.000010966382364*b[1]/0.000010966382364,0.5);
kaipon 1:5f317aa38c12 197
kaipon 0:c51bac912336 198 }
kaipon 3:6033c3517dc7 199 driveMotor(1,0);
kaipon 3:6033c3517dc7 200 wait(3);
kaipon 3:6033c3517dc7 201 driveMotor(0,0);
kaipon 0:c51bac912336 202 }
kaipon 0:c51bac912336 203
kaipon 0:c51bac912336 204