cansat-e_2021 / Mbed 2 deprecated GPSDRV8833CanSat

connected GPS and motor driver

Dependencies:   mbed getGPS

main.cpp@2:591877e2c7d1, 2021-10-20 (annotated)

Committer:
kaipon
Date:
Wed Oct 20 02:55:52 2021 +0000
Revision:
2:591877e2c7d1
Parent:
1:c9f35ca3f74f
Child:
3:b4f715167119
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(){
keepyourselfalive 0:a56e760b5d55 38 double x, y;
keepyourselfalive 0:a56e760b5d55 39 double m, n;
keepyourselfalive 0:a56e760b5d55 40 double i,w;
keepyourselfalive 0:a56e760b5d55 41 gps.getgps();
keepyourselfalive 0:a56e760b5d55 42 x = gps.latitude; //経度の初期値
keepyourselfalive 0:a56e760b5d55 43 y = gps.longitude; //緯度の初期値
keepyourselfalive 0:a56e760b5d55 44 while(1){
keepyourselfalive 0:a56e760b5d55 45 driveMotor(1,1);
keepyourselfalive 0:a56e760b5d55 46 wait(30);
keepyourselfalive 0:a56e760b5d55 47 driveMotor(0,0);
keepyourselfalive 0:a56e760b5d55 48 m=gps.latitude; //現在の経度
keepyourselfalive 0:a56e760b5d55 49 n=gps.longitude; //現在の緯度
keepyourselfalive 0:a56e760b5d55 50 x=m-x;
keepyourselfalive 0:a56e760b5d55 51 y=n-y;
keepyourselfalive 0:a56e760b5d55 52 double a[2]={x,y};
keepyourselfalive 0:a56e760b5d55 53 double b[2]={0-m,0-n}; //目的地の座標を(0,0)とした
keepyourselfalive 0:a56e760b5d55 54 double p=a[0]*a[0]+a[1]*a[1];
keepyourselfalive 0:a56e760b5d55 55 double q=b[0]*b[0]+b[1]*b[1];
keepyourselfalive 0:a56e760b5d55 56 double pp=pow(p,0.5);
keepyourselfalive 0:a56e760b5d55 57 double qq=pow(q,0.5);
keepyourselfalive 0:a56e760b5d55 58 i=(a[0]*b[0]+a[1]*b[1])/pp*qq;
keepyourselfalive 0:a56e760b5d55 59 w=acos(i);
kaipon 2:591877e2c7d1 60 double e = (0-n)/(0-m);//目的地の座標を(0,0)とした
kaipon 2:591877e2c7d1 61 double f = (n-y)/(m-x);
kaipon 2:591877e2c7d1 62 double c = (e-f)/(1+e*f);
kaipon 1:c9f35ca3f74f 63 double d = m*m+n*n;
kaipon 1:c9f35ca3f74f 64 double dd = pow(d,0.5); //目的地までの距離
kaipon 1:c9f35ca3f74f 65 double t[2] = {5*w/3.14159,2*dd/3*1}; //方向転換は5秒以内、1はモーターの1m/sを緯度、経度に変換した値。再度図るのは、全体の2/3だけ進んだときにしました。
kaipon 2:591877e2c7d1 66 if(c*w>0){
kaipon 2:591877e2c7d1 67 driveMotor(1,0); //方向転換、単位は1秒あたり36度。
kaipon 2:591877e2c7d1 68 wait(t[0]);
kaipon 2:591877e2c7d1 69 driveMotor(0,0);
kaipon 2:591877e2c7d1 70 wait(0.5);
kaipon 2:591877e2c7d1 71 driveMotor(1,1);
kaipon 2:591877e2c7d1 72 wait(t[1]);
kaipon 2:591877e2c7d1 73 }
kaipon 2:591877e2c7d1 74 else if(c*w<0){
kaipon 2:591877e2c7d1 75 driveMotor(0,1);
kaipon 2:591877e2c7d1 76 wait(t[0]);
kaipon 2:591877e2c7d1 77 driveMotor(0,0);
kaipon 2:591877e2c7d1 78 wait(0.5);
kaipon 2:591877e2c7d1 79 driveMotor(1,1);
kaipon 2:591877e2c7d1 80 wait(t[1]);
kaipon 2:591877e2c7d1 81 }
kaipon 2:591877e2c7d1 82 else{
kaipon 2:591877e2c7d1 83 return 0;
kaipon 2:591877e2c7d1 84 }
keepyourselfalive 0:a56e760b5d55 85 }
keepyourselfalive 0:a56e760b5d55 86 }
keepyourselfalive 0:a56e760b5d55 87
keepyourselfalive 0:a56e760b5d55 88