cansat-e_2021 / Mbed 2 deprecated GPSDRV8833CanSat

connected GPS and motor driver

Dependencies:   mbed getGPS

main.cpp@1:c9f35ca3f74f, 2021-10-19 (annotated)

Committer:
kaipon
Date:
Tue Oct 19 07:51:56 2021 +0000
Revision:
1:c9f35ca3f74f
Parent:
0:a56e760b5d55
Child:
2:591877e2c7d1
aaa

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>
keepyourselfalive 0:a56e760b5d55 3
keepyourselfalive 0:a56e760b5d55 4 PwmOut pinAFin(A1);
keepyourselfalive 0:a56e760b5d55 5 PwmOut pinARin(D10);
keepyourselfalive 0:a56e760b5d55 6 PwmOut pinBFin(D11);
keepyourselfalive 0:a56e760b5d55 7 PwmOut pinBRin(D12);
keepyourselfalive 0:a56e760b5d55 8 Serial pc(SERIAL_TX, SERIAL_RX);
keepyourselfalive 0:a56e760b5d55 9 GPS gps(D1, D0);// tx,rx
keepyourselfalive 0:a56e760b5d55 10
keepyourselfalive 0:a56e760b5d55 11 void driveMotor(int speedA,int speedB) {
keepyourselfalive 0:a56e760b5d55 12 float outputA = abs(speedA);
keepyourselfalive 0:a56e760b5d55 13 float outputB = abs(speedB);
keepyourselfalive 0:a56e760b5d55 14 if (speedA > 0) {
keepyourselfalive 0:a56e760b5d55 15 pinAFin=outputA;
keepyourselfalive 0:a56e760b5d55 16 pinARin=0;
keepyourselfalive 0:a56e760b5d55 17 } else if (speedA < 0) {
keepyourselfalive 0:a56e760b5d55 18 pinAFin=0;
keepyourselfalive 0:a56e760b5d55 19 pinARin=outputA;
keepyourselfalive 0:a56e760b5d55 20 } else {
keepyourselfalive 0:a56e760b5d55 21 pinAFin=0;
keepyourselfalive 0:a56e760b5d55 22 pinARin=0;
keepyourselfalive 0:a56e760b5d55 23 }
keepyourselfalive 0:a56e760b5d55 24 if (speedB > 0) {
keepyourselfalive 0:a56e760b5d55 25 pinBFin=outputB;
keepyourselfalive 0:a56e760b5d55 26 pinBRin=0;
keepyourselfalive 0:a56e760b5d55 27 } else if (speedB < 0) {
keepyourselfalive 0:a56e760b5d55 28 pinBFin=0;
keepyourselfalive 0:a56e760b5d55 29 pinBRin=outputB;
keepyourselfalive 0:a56e760b5d55 30 } else {
keepyourselfalive 0:a56e760b5d55 31 pinBFin=0;
keepyourselfalive 0:a56e760b5d55 32 pinBRin=0;
keepyourselfalive 0:a56e760b5d55 33 }
keepyourselfalive 0:a56e760b5d55 34 }
keepyourselfalive 0:a56e760b5d55 35
keepyourselfalive 0:a56e760b5d55 36 int main(){
keepyourselfalive 0:a56e760b5d55 37 double x, y;
keepyourselfalive 0:a56e760b5d55 38 double m, n;
keepyourselfalive 0:a56e760b5d55 39 double i,w;
keepyourselfalive 0:a56e760b5d55 40 gps.getgps();
keepyourselfalive 0:a56e760b5d55 41 x = gps.latitude; //経度の初期値
keepyourselfalive 0:a56e760b5d55 42 y = gps.longitude; //緯度の初期値
keepyourselfalive 0:a56e760b5d55 43 while(1){
keepyourselfalive 0:a56e760b5d55 44 driveMotor(1,1);
keepyourselfalive 0:a56e760b5d55 45 wait(30);
keepyourselfalive 0:a56e760b5d55 46 driveMotor(0,0);
keepyourselfalive 0:a56e760b5d55 47 m=gps.latitude; //現在の経度
keepyourselfalive 0:a56e760b5d55 48 n=gps.longitude; //現在の緯度
keepyourselfalive 0:a56e760b5d55 49 x=m-x;
keepyourselfalive 0:a56e760b5d55 50 y=n-y;
keepyourselfalive 0:a56e760b5d55 51 double a[2]={x,y};
keepyourselfalive 0:a56e760b5d55 52 double b[2]={0-m,0-n}; //目的地の座標を(0,0)とした
keepyourselfalive 0:a56e760b5d55 53 double p=a[0]*a[0]+a[1]*a[1];
keepyourselfalive 0:a56e760b5d55 54 double q=b[0]*b[0]+b[1]*b[1];
keepyourselfalive 0:a56e760b5d55 55 double pp=pow(p,0.5);
keepyourselfalive 0:a56e760b5d55 56 double qq=pow(q,0.5);
keepyourselfalive 0:a56e760b5d55 57 i=(a[0]*b[0]+a[1]*b[1])/pp*qq;
keepyourselfalive 0:a56e760b5d55 58 w=acos(i);
kaipon 1:c9f35ca3f74f 59 double d = m*m+n*n;
kaipon 1:c9f35ca3f74f 60 double dd = pow(d,0.5); //目的地までの距離
kaipon 1:c9f35ca3f74f 61 double t[2] = {5*w/3.14159,2*dd/3*1}; //方向転換は5秒以内、1はモーターの1m/sを緯度、経度に変換した値。再度図るのは、全体の2/3だけ進んだときにしました。
kaipon 1:c9f35ca3f74f 62 driveMotor(1,0); //方向転換、単位は1秒あたり72度。左回転にしたい。
kaipon 1:c9f35ca3f74f 63 wait(t[0]);
kaipon 1:c9f35ca3f74f 64 driveMotor(0,0);
kaipon 1:c9f35ca3f74f 65 wait(0.5);
kaipon 1:c9f35ca3f74f 66 driveMotor(1,1);
kaipon 1:c9f35ca3f74f 67 wait(t[1]);
keepyourselfalive 0:a56e760b5d55 68 }
keepyourselfalive 0:a56e760b5d55 69 }
keepyourselfalive 0:a56e760b5d55 70
keepyourselfalive 0:a56e760b5d55 71