cansat-e_2021 / Mbed 2 deprecated driving_1

for testing about driving 1

Dependencies:   mbed getGPS

main.cpp@2:3b1b6e825546, 2021-12-01 (annotated)

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