cansat-e_2021 / Mbed 2 deprecated driving_1

for testing about driving 1

Dependencies:   mbed getGPS

main.cpp@0:c51bac912336, 2021-11-17 (annotated)

Committer:
kaipon
Date:
Wed Nov 17 07:55:22 2021 +0000
Revision:
0:c51bac912336
Child:
1:5f317aa38c12
for testing about driving 1

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 0:c51bac912336 4 PwmOut pinAFin(A1);
kaipon 0:c51bac912336 5 PwmOut pinARin(D10);
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 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 0:c51bac912336 39 gps.getgps();
kaipon 0:c51bac912336 40 x = gps.latitude;
kaipon 0:c51bac912336 41 y = gps.longitude;
kaipon 0:c51bac912336 42 while(j>10){
kaipon 0:c51bac912336 43 //30秒前進する
kaipon 0:c51bac912336 44 driveMotor(1,1);
kaipon 0:c51bac912336 45 wait(30);//メモ：ただ直進してる
kaipon 0:c51bac912336 46 driveMotor(0,0);
kaipon 0:c51bac912336 47 //m,nに現在の経度、緯度を記録する
kaipon 0:c51bac912336 48 gps.getgps();
kaipon 0:c51bac912336 49 m=gps.latitude;
kaipon 0:c51bac912336 50 n=gps.longitude;
kaipon 0:c51bac912336 51 //a,bの配列に、それぞれ緯度、経度の現在地と初期位置の差、jに目標までの距離を記録する
kaipon 0:c51bac912336 52 double a[2]={m-x,n-y};
kaipon 0:c51bac912336 53 double b[2]={u-m,v-n};
kaipon 0:c51bac912336 54 j=pow(b[0]*b[0]/0.000008983148616/0.000008983148616+b[1]*b[1]/0.000010966382364/0.000010966382364,0.5);
kaipon 0:c51bac912336 55 //cosと角度の値を出す
kaipon 0:c51bac912336 56 p=pow(a[0]*a[0]+a[1]*a[1],0.5);
kaipon 0:c51bac912336 57 q=pow(b[0]*b[0]+b[1]*b[1],0.5);
kaipon 0:c51bac912336 58 i=(a[0]*b[0]+a[1]*b[1])/p*q;
kaipon 0:c51bac912336 59 w=acos(i);
kaipon 0:c51bac912336 60 //tanの値を出す
kaipon 0:c51bac912336 61 e = (v-n)/(u-m);
kaipon 0:c51bac912336 62 f = (n-y)/(m-x);
kaipon 0:c51bac912336 63 c = (e-f)/(1+e*f);
kaipon 0:c51bac912336 64 //t[0]は回る時間、t[1]はかかる時間、rに何回進むプログラムを動かすかを決める
kaipon 0:c51bac912336 65 double t[2] = {w/3.14159/2,2*j/3/1.2}; //再度図るのは、全体の2/3だけ進んだとき
kaipon 0:c51bac912336 66 r = t[1]/2.2;
kaipon 0:c51bac912336 67
kaipon 0:c51bac912336 68 //実際に動くときのプログラム。1.2m/s計算、1.8s進み、0.4秒周りを見る。平均速度1.0m/s、基準を進行方向にしたとき見れる範囲:-43.5~43.5の予定
kaipon 0:c51bac912336 69
kaipon 0:c51bac912336 70 if(c*i>0){
kaipon 0:c51bac912336 71 driveMotor(1,0); //メモ：右回りがどっちか測定する
kaipon 0:c51bac912336 72 wait(t[0]);
kaipon 0:c51bac912336 73 driveMotor(0,0);
kaipon 0:c51bac912336 74 while(s == r){
kaipon 0:c51bac912336 75 driveMotor(1,1);
kaipon 0:c51bac912336 76 wait(1.8);
kaipon 0:c51bac912336 77 driveMotor(0,0);
kaipon 0:c51bac912336 78 //超音波センサの起動。首を振る
kaipon 0:c51bac912336 79 driveMotor(0,1);
kaipon 0:c51bac912336 80 wait(0.1);
kaipon 0:c51bac912336 81 driveMotor(1,0);
kaipon 0:c51bac912336 82 wait(0.2);
kaipon 0:c51bac912336 83 driveMotor(0,1);
kaipon 0:c51bac912336 84 wait(0.1);
kaipon 0:c51bac912336 85 //超音波センサ終了
kaipon 0:c51bac912336 86 s++;
kaipon 0:c51bac912336 87 }
kaipon 0:c51bac912336 88 }
kaipon 0:c51bac912336 89
kaipon 0:c51bac912336 90 else if(c*i<0){
kaipon 0:c51bac912336 91 driveMotor(0,1);
kaipon 0:c51bac912336 92 wait(t[0]);
kaipon 0:c51bac912336 93 driveMotor(0,0);
kaipon 0:c51bac912336 94 while(s == r){
kaipon 0:c51bac912336 95 driveMotor(1,1);
kaipon 0:c51bac912336 96 wait(1.8);
kaipon 0:c51bac912336 97 driveMotor(0,0);
kaipon 0:c51bac912336 98 //超音波センサの起動。首を振る
kaipon 0:c51bac912336 99 driveMotor(0,1);
kaipon 0:c51bac912336 100 wait(0.1);
kaipon 0:c51bac912336 101 driveMotor(1,0);
kaipon 0:c51bac912336 102 wait(0.2);
kaipon 0:c51bac912336 103 driveMotor(0,1);
kaipon 0:c51bac912336 104 wait(0.1);
kaipon 0:c51bac912336 105 //超音波センサ終了
kaipon 0:c51bac912336 106 s++;
kaipon 0:c51bac912336 107 }
kaipon 0:c51bac912336 108 }
kaipon 0:c51bac912336 109
kaipon 0:c51bac912336 110 //0度と180度かの判断はあきらめて、0度と180度の時は、違う方角を向くようにした。
kaipon 0:c51bac912336 111 else{
kaipon 0:c51bac912336 112 driveMotor(1,0); //直線上にある時は、18度だけ向きを変更
kaipon 0:c51bac912336 113 wait(0.1);
kaipon 0:c51bac912336 114 driveMotor(0,0);
kaipon 0:c51bac912336 115 }
kaipon 0:c51bac912336 116 }
kaipon 0:c51bac912336 117 }
kaipon 0:c51bac912336 118
kaipon 0:c51bac912336 119