cansat-e_2021 / Mbed 2 deprecated driving_1

for testing about driving 1

Dependencies:   mbed getGPS

main.cpp@1:5f317aa38c12, 2021-11-24 (annotated)

Committer:
kaipon
Date:
Wed Nov 24 06:49:52 2021 +0000
Revision:
1:5f317aa38c12
Parent:
0:c51bac912336
Child:
2:3b1b6e825546
2

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 Serial pc(SERIAL_TX, SERIAL_RX);
kaipon 0:c51bac912336 9 GPS gps(D1, D0);// tx,rx
kaipon 1:5f317aa38c12 10 Serial xbee(A7,A2);
kaipon 0:c51bac912336 11
kaipon 0:c51bac912336 12 void driveMotor(int speedA,int speedB) {
kaipon 0:c51bac912336 13 float outputA = abs(speedA);
kaipon 0:c51bac912336 14 float outputB = abs(speedB);
kaipon 0:c51bac912336 15 if (speedA > 0) {
kaipon 0:c51bac912336 16 pinAFin=outputA;
kaipon 0:c51bac912336 17 pinARin=0;
kaipon 0:c51bac912336 18 } else if (speedA < 0) {
kaipon 0:c51bac912336 19 pinAFin=0;
kaipon 0:c51bac912336 20 pinARin=outputA;
kaipon 0:c51bac912336 21 } else {
kaipon 0:c51bac912336 22 pinAFin=0;
kaipon 0:c51bac912336 23 pinARin=0;
kaipon 0:c51bac912336 24 }
kaipon 0:c51bac912336 25 if (speedB > 0) {
kaipon 0:c51bac912336 26 pinBFin=outputB;
kaipon 0:c51bac912336 27 pinBRin=0;
kaipon 0:c51bac912336 28 } else if (speedB < 0) {
kaipon 0:c51bac912336 29 pinBFin=0;
kaipon 0:c51bac912336 30 pinBRin=outputB;
kaipon 0:c51bac912336 31 } else {
kaipon 0:c51bac912336 32 pinBFin=0;
kaipon 0:c51bac912336 33 pinBRin=0;
kaipon 0:c51bac912336 34 }
kaipon 0:c51bac912336 35 }
kaipon 0:c51bac912336 36
kaipon 0:c51bac912336 37 int main(){
kaipon 0:c51bac912336 38 double x,y,u,v,m,n,i,w,j,p,q,e,f,c;
kaipon 0:c51bac912336 39 int r,s;
kaipon 1:5f317aa38c12 40 u = 34.546402;
kaipon 1:5f317aa38c12 41 v = 135.502126;
kaipon 1:5f317aa38c12 42 s = 0;
kaipon 1:5f317aa38c12 43
kaipon 1:5f317aa38c12 44 xbee.printf("XBee Connected\r\n");
kaipon 1:5f317aa38c12 45 while(gps.getgps() != true){
kaipon 1:5f317aa38c12 46 }
kaipon 1:5f317aa38c12 47
kaipon 1:5f317aa38c12 48 //基準値を記録する
kaipon 0:c51bac912336 49 gps.getgps();
kaipon 0:c51bac912336 50 x = gps.latitude;
kaipon 0:c51bac912336 51 y = gps.longitude;
kaipon 1:5f317aa38c12 52 xbee.printf("%f,%f\n", x, y);
kaipon 1:5f317aa38c12 53 //10秒前進する
kaipon 1:5f317aa38c12 54 driveMotor(1,1);
kaipon 1:5f317aa38c12 55 wait(10);//メモ：ただ直進してる
kaipon 1:5f317aa38c12 56 driveMotor(0,0);
kaipon 0:c51bac912336 57 //m,nに現在の経度、緯度を記録する
kaipon 1:5f317aa38c12 58 gps.getgps();
kaipon 1:5f317aa38c12 59 m=gps.latitude;
kaipon 1:5f317aa38c12 60 n=gps.longitude;
kaipon 1:5f317aa38c12 61 xbee.printf("%f,%f\n", m, n);
kaipon 0:c51bac912336 62 //a,bの配列に、それぞれ緯度、経度の現在地と初期位置の差、jに目標までの距離を記録する
kaipon 1:5f317aa38c12 63 double a[2]={m-x,n-y};
kaipon 1:5f317aa38c12 64 double b[2]={u-m,v-n};
kaipon 1:5f317aa38c12 65 j=pow(b[0]/0.000008983148616*b[0]/0.000008983148616+b[1]/0.000010966382364*b[1]/0.000010966382364,0.5);
kaipon 1:5f317aa38c12 66
kaipon 1:5f317aa38c12 67
kaipon 1:5f317aa38c12 68 while(j>10){
kaipon 1:5f317aa38c12 69
kaipon 0:c51bac912336 70 //cosと角度の値を出す
kaipon 0:c51bac912336 71 p=pow(a[0]*a[0]+a[1]*a[1],0.5);
kaipon 0:c51bac912336 72 q=pow(b[0]*b[0]+b[1]*b[1],0.5);
kaipon 1:5f317aa38c12 73 i=(a[0]*b[0]+a[1]*b[1])/p/q;
kaipon 0:c51bac912336 74 w=acos(i);
kaipon 0:c51bac912336 75 //tanの値を出す
kaipon 0:c51bac912336 76 e = (v-n)/(u-m);
kaipon 0:c51bac912336 77 f = (n-y)/(m-x);
kaipon 0:c51bac912336 78 c = (e-f)/(1+e*f);
kaipon 0:c51bac912336 79 //t[0]は回る時間、t[1]はかかる時間、rに何回進むプログラムを動かすかを決める
kaipon 0:c51bac912336 80 double t[2] = {w/3.14159/2,2*j/3/1.2}; //再度図るのは、全体の2/3だけ進んだとき
kaipon 0:c51bac912336 81 r = t[1]/2.2;
kaipon 1:5f317aa38c12 82 wait(1);
kaipon 0:c51bac912336 83
kaipon 0:c51bac912336 84 //実際に動くときのプログラム。1.2m/s計算、1.8s進み、0.4秒周りを見る。平均速度1.0m/s、基準を進行方向にしたとき見れる範囲:-43.5~43.5の予定
kaipon 0:c51bac912336 85
kaipon 0:c51bac912336 86 if(c*i>0){
kaipon 0:c51bac912336 87 driveMotor(1,0); //メモ：右回りがどっちか測定する
kaipon 0:c51bac912336 88 wait(t[0]);
kaipon 0:c51bac912336 89 driveMotor(0,0);
kaipon 0:c51bac912336 90 while(s == r){
kaipon 0:c51bac912336 91 driveMotor(1,1);
kaipon 0:c51bac912336 92 wait(1.8);
kaipon 0:c51bac912336 93 driveMotor(0,0);
kaipon 1:5f317aa38c12 94 /*超音波センサの起動。首を振る
kaipon 0:c51bac912336 95 driveMotor(0,1);
kaipon 0:c51bac912336 96 wait(0.1);
kaipon 0:c51bac912336 97 driveMotor(1,0);
kaipon 0:c51bac912336 98 wait(0.2);
kaipon 0:c51bac912336 99 driveMotor(0,1);
kaipon 0:c51bac912336 100 wait(0.1);
kaipon 1:5f317aa38c12 101 //超音波センサ終了*/
kaipon 0:c51bac912336 102 s++;
kaipon 0:c51bac912336 103 }
kaipon 0:c51bac912336 104 }
kaipon 0:c51bac912336 105
kaipon 0:c51bac912336 106 else if(c*i<0){
kaipon 0:c51bac912336 107 driveMotor(0,1);
kaipon 0:c51bac912336 108 wait(t[0]);
kaipon 0:c51bac912336 109 driveMotor(0,0);
kaipon 0:c51bac912336 110 while(s == r){
kaipon 0:c51bac912336 111 driveMotor(1,1);
kaipon 0:c51bac912336 112 wait(1.8);
kaipon 0:c51bac912336 113 driveMotor(0,0);
kaipon 1:5f317aa38c12 114 /*超音波センサの起動。首を振る
kaipon 0:c51bac912336 115 driveMotor(0,1);
kaipon 0:c51bac912336 116 wait(0.1);
kaipon 0:c51bac912336 117 driveMotor(1,0);
kaipon 0:c51bac912336 118 wait(0.2);
kaipon 0:c51bac912336 119 driveMotor(0,1);
kaipon 0:c51bac912336 120 wait(0.1);
kaipon 1:5f317aa38c12 121 //超音波センサ終了*/
kaipon 0:c51bac912336 122 s++;
kaipon 0:c51bac912336 123 }
kaipon 0:c51bac912336 124 }
kaipon 0:c51bac912336 125
kaipon 0:c51bac912336 126 //0度と180度かの判断はあきらめて、0度と180度の時は、違う方角を向くようにした。
kaipon 0:c51bac912336 127 else{
kaipon 0:c51bac912336 128 driveMotor(1,0); //直線上にある時は、18度だけ向きを変更
kaipon 0:c51bac912336 129 wait(0.1);
kaipon 0:c51bac912336 130 driveMotor(0,0);
kaipon 0:c51bac912336 131 }
kaipon 1:5f317aa38c12 132
kaipon 1:5f317aa38c12 133 //基準値を記録する
kaipon 1:5f317aa38c12 134 gps.getgps();
kaipon 1:5f317aa38c12 135 x = gps.latitude;
kaipon 1:5f317aa38c12 136 y = gps.longitude;
kaipon 1:5f317aa38c12 137 //5秒前進する
kaipon 1:5f317aa38c12 138 driveMotor(1,1);
kaipon 1:5f317aa38c12 139 wait(5);//メモ：ただ直進してる
kaipon 1:5f317aa38c12 140 driveMotor(0,0);
kaipon 1:5f317aa38c12 141 //m,nに現在の経度、緯度を記録する
kaipon 1:5f317aa38c12 142 gps.getgps();
kaipon 1:5f317aa38c12 143 m=gps.latitude;
kaipon 1:5f317aa38c12 144 n=gps.longitude;
kaipon 1:5f317aa38c12 145
kaipon 1:5f317aa38c12 146 xbee.printf("%f,%f\n", x, y);
kaipon 1:5f317aa38c12 147 xbee.printf("%f,%f\n", m, n);
kaipon 1:5f317aa38c12 148
kaipon 1:5f317aa38c12 149 //a,bの配列に、それぞれ緯度、経度の現在地と初期位置の差、jに目標までの距離を記録する
kaipon 1:5f317aa38c12 150 a[0]=m-x;
kaipon 1:5f317aa38c12 151 a[1]=n-y;
kaipon 1:5f317aa38c12 152 b[0]=u-m;
kaipon 1:5f317aa38c12 153 b[1]=v-n;
kaipon 1:5f317aa38c12 154 j=pow(b[0]/0.000008983148616*b[0]/0.000008983148616+b[1]/0.000010966382364*b[1]/0.000010966382364,0.5);
kaipon 1:5f317aa38c12 155
kaipon 0:c51bac912336 156 }
kaipon 0:c51bac912336 157 }
kaipon 0:c51bac912336 158
kaipon 0:c51bac912336 159