cansat-e_2021
/
GPSDRV8833CanSat
connected GPS and motor driver
main.cpp
- Committer:
- kaipon
- Date:
- 2021-10-19
- Revision:
- 1:c9f35ca3f74f
- Parent:
- 0:a56e760b5d55
- Child:
- 2:591877e2c7d1
File content as of revision 1:c9f35ca3f74f:
#include <mbed.h> #include <getGPS.h> PwmOut pinAFin(A1); PwmOut pinARin(D10); PwmOut pinBFin(D11); PwmOut pinBRin(D12); Serial pc(SERIAL_TX, SERIAL_RX); GPS gps(D1, D0);// tx,rx void driveMotor(int speedA,int speedB) { float outputA = abs(speedA); float outputB = abs(speedB); if (speedA > 0) { pinAFin=outputA; pinARin=0; } else if (speedA < 0) { pinAFin=0; pinARin=outputA; } else { pinAFin=0; pinARin=0; } if (speedB > 0) { pinBFin=outputB; pinBRin=0; } else if (speedB < 0) { pinBFin=0; pinBRin=outputB; } else { pinBFin=0; pinBRin=0; } } int main(){ double x, y; double m, n; double i,w; gps.getgps(); x = gps.latitude; //経度の初期値 y = gps.longitude; //緯度の初期値 while(1){ driveMotor(1,1); wait(30); driveMotor(0,0); m=gps.latitude; //現在の経度 n=gps.longitude; //現在の緯度 x=m-x; y=n-y; double a[2]={x,y}; double b[2]={0-m,0-n}; //目的地の座標を(0,0)とした double p=a[0]*a[0]+a[1]*a[1]; double q=b[0]*b[0]+b[1]*b[1]; double pp=pow(p,0.5); double qq=pow(q,0.5); i=(a[0]*b[0]+a[1]*b[1])/pp*qq; w=acos(i); double d = m*m+n*n; double dd = pow(d,0.5); //目的地までの距離 double t[2] = {5*w/3.14159,2*dd/3*1}; //方向転換は5秒以内、1はモーターの1m/sを緯度、経度に変換した値。再度図るのは、全体の2/3だけ進んだときにしました。 driveMotor(1,0); //方向転換、単位は1秒あたり72度。左回転にしたい。 wait(t[0]); driveMotor(0,0); wait(0.5); driveMotor(1,1); wait(t[1]); } }