cansat-e_2021
/
GPSDRV8833CanSat
connected GPS and motor driver
Diff: main.cpp
- Revision:
- 0:a56e760b5d55
- Child:
- 1:c9f35ca3f74f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Oct 19 06:24:19 2021 +0000 @@ -0,0 +1,62 @@ +#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); + } + } + + \ No newline at end of file