cansat-e_2021 / Mbed 2 deprecated GPSDRV8833CanSat

connected GPS and motor driver

Dependencies:   mbed getGPS

main.cpp@4:fca767ac5f69, 2021-10-20 (annotated)

Committer:
kaipon
Date:
Wed Oct 20 07:46:17 2021 +0000
Revision:
4:fca767ac5f69
Parent:
3:b4f715167119
Child:
5:978542e79cb7
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
keepyourselfalive 0:a56e760b5d55 1 #include <mbed.h>
keepyourselfalive 0:a56e760b5d55 2 #include <getGPS.h>
kaipon 2:591877e2c7d1 3 //イレギュラーへの対応がまだできてません
keepyourselfalive 0:a56e760b5d55 4
keepyourselfalive 0:a56e760b5d55 5 PwmOut pinAFin(A1);
keepyourselfalive 0:a56e760b5d55 6 PwmOut pinARin(D10);
keepyourselfalive 0:a56e760b5d55 7 PwmOut pinBFin(D11);
keepyourselfalive 0:a56e760b5d55 8 PwmOut pinBRin(D12);
keepyourselfalive 0:a56e760b5d55 9 Serial pc(SERIAL_TX, SERIAL_RX);
keepyourselfalive 0:a56e760b5d55 10 GPS gps(D1, D0);// tx,rx
keepyourselfalive 0:a56e760b5d55 11
keepyourselfalive 0:a56e760b5d55 12 void driveMotor(int speedA,int speedB) {
keepyourselfalive 0:a56e760b5d55 13 float outputA = abs(speedA);
keepyourselfalive 0:a56e760b5d55 14 float outputB = abs(speedB);
keepyourselfalive 0:a56e760b5d55 15 if (speedA > 0) {
keepyourselfalive 0:a56e760b5d55 16 pinAFin=outputA;
keepyourselfalive 0:a56e760b5d55 17 pinARin=0;
keepyourselfalive 0:a56e760b5d55 18 } else if (speedA < 0) {
keepyourselfalive 0:a56e760b5d55 19 pinAFin=0;
keepyourselfalive 0:a56e760b5d55 20 pinARin=outputA;
keepyourselfalive 0:a56e760b5d55 21 } else {
keepyourselfalive 0:a56e760b5d55 22 pinAFin=0;
keepyourselfalive 0:a56e760b5d55 23 pinARin=0;
keepyourselfalive 0:a56e760b5d55 24 }
keepyourselfalive 0:a56e760b5d55 25 if (speedB > 0) {
keepyourselfalive 0:a56e760b5d55 26 pinBFin=outputB;
keepyourselfalive 0:a56e760b5d55 27 pinBRin=0;
keepyourselfalive 0:a56e760b5d55 28 } else if (speedB < 0) {
keepyourselfalive 0:a56e760b5d55 29 pinBFin=0;
keepyourselfalive 0:a56e760b5d55 30 pinBRin=outputB;
keepyourselfalive 0:a56e760b5d55 31 } else {
keepyourselfalive 0:a56e760b5d55 32 pinBFin=0;
keepyourselfalive 0:a56e760b5d55 33 pinBRin=0;
keepyourselfalive 0:a56e760b5d55 34 }
keepyourselfalive 0:a56e760b5d55 35 }
keepyourselfalive 0:a56e760b5d55 36
keepyourselfalive 0:a56e760b5d55 37 int main(){
kaipon 4:fca767ac5f69 38 double v,w,x,y;
keepyourselfalive 0:a56e760b5d55 39 double m, n;
keepyourselfalive 0:a56e760b5d55 40 double i,w;
kaipon 3:b4f715167119 41 double j,k,l;
keepyourselfalive 0:a56e760b5d55 42 gps.getgps();
keepyourselfalive 0:a56e760b5d55 43 x = gps.latitude; //経度の初期値
keepyourselfalive 0:a56e760b5d55 44 y = gps.longitude; //緯度の初期値
kaipon 3:b4f715167119 45 while(j>5){
keepyourselfalive 0:a56e760b5d55 46 driveMotor(1,1);
keepyourselfalive 0:a56e760b5d55 47 wait(30);
keepyourselfalive 0:a56e760b5d55 48 driveMotor(0,0);
kaipon 4:fca767ac5f69 49 gps.getgps();
keepyourselfalive 0:a56e760b5d55 50 m=gps.latitude; //現在の経度
keepyourselfalive 0:a56e760b5d55 51 n=gps.longitude; //現在の緯度
kaipon 4:fca767ac5f69 52 v=m-x;
kaipon 4:fca767ac5f69 53 w=n-y;
kaipon 4:fca767ac5f69 54 double a[2]={v,w};
keepyourselfalive 0:a56e760b5d55 55 double b[2]={0-m,0-n}; //目的地の座標を(0,0)とした
kaipon 3:b4f715167119 56 k=b[0]/0.000008983148616;
kaipon 3:b4f715167119 57 l=b[1]/0.000010966382364;
kaipon 3:b4f715167119 58 j=pow(k*k+l*l,0.5);
keepyourselfalive 0:a56e760b5d55 59 double p=a[0]*a[0]+a[1]*a[1];
keepyourselfalive 0:a56e760b5d55 60 double q=b[0]*b[0]+b[1]*b[1];
keepyourselfalive 0:a56e760b5d55 61 double pp=pow(p,0.5);
keepyourselfalive 0:a56e760b5d55 62 double qq=pow(q,0.5);
keepyourselfalive 0:a56e760b5d55 63 i=(a[0]*b[0]+a[1]*b[1])/pp*qq;
keepyourselfalive 0:a56e760b5d55 64 w=acos(i);
kaipon 2:591877e2c7d1 65 double e = (0-n)/(0-m);//目的地の座標を(0,0)とした
kaipon 2:591877e2c7d1 66 double f = (n-y)/(m-x);
kaipon 2:591877e2c7d1 67 double c = (e-f)/(1+e*f);
kaipon 1:c9f35ca3f74f 68 double d = m*m+n*n;
kaipon 1:c9f35ca3f74f 69 double dd = pow(d,0.5); //目的地までの距離
kaipon 3:b4f715167119 70 double t[2] = {5*w/3.14159,2*j/3}; //方向転換は5秒以内、1はモーターの1m/sで進むための時間。再度図るのは、全体の2/3だけ進んだときにしました。
kaipon 2:591877e2c7d1 71 if(c*w>0){
kaipon 4:fca767ac5f69 72 driveMotor(1,0); //方向転換、単位は1秒あたり36度。どっちを1にしたら、右回りになるかなどはわかっていない
kaipon 2:591877e2c7d1 73 wait(t[0]);
kaipon 2:591877e2c7d1 74 driveMotor(0,0);
kaipon 2:591877e2c7d1 75 wait(0.5);
kaipon 2:591877e2c7d1 76 driveMotor(1,1);
kaipon 2:591877e2c7d1 77 wait(t[1]);
kaipon 2:591877e2c7d1 78 }
kaipon 2:591877e2c7d1 79 else if(c*w<0){
kaipon 2:591877e2c7d1 80 driveMotor(0,1);
kaipon 2:591877e2c7d1 81 wait(t[0]);
kaipon 2:591877e2c7d1 82 driveMotor(0,0);
kaipon 2:591877e2c7d1 83 wait(0.5);
kaipon 2:591877e2c7d1 84 driveMotor(1,1);
kaipon 2:591877e2c7d1 85 wait(t[1]);
kaipon 2:591877e2c7d1 86 }
kaipon 3:b4f715167119 87 //ちゃんと前後確認するプログラムを書く。いったん、0度と180度かの判断はあきらめて、0度と180度の時は、違う方角を向くようにしました。
kaipon 2:591877e2c7d1 88 else{
kaipon 3:b4f715167119 89 driveMotor(1,0); //直線上にある時は、18度だけ向きを変更します。
kaipon 3:b4f715167119 90 wait(0.5);
kaipon 3:b4f715167119 91 driveMotor(0,0);
kaipon 2:591877e2c7d1 92 }
keepyourselfalive 0:a56e760b5d55 93 }
keepyourselfalive 0:a56e760b5d55 94 }
keepyourselfalive 0:a56e760b5d55 95
keepyourselfalive 0:a56e760b5d55 96