cansat-e_2021
/
GPSDRV8833CanSat
connected GPS and motor driver
main.cpp@3:b4f715167119, 2021-10-20 (annotated)
- Committer:
- kaipon
- Date:
- Wed Oct 20 07:34:03 2021 +0000
- Revision:
- 3:b4f715167119
- Parent:
- 2:591877e2c7d1
- Child:
- 4:fca767ac5f69
a
Who changed what in which revision?
User | Revision | Line number | New 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; |
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); |
keepyourselfalive | 0:a56e760b5d55 | 49 | m=gps.latitude; //現在の経度 |
keepyourselfalive | 0:a56e760b5d55 | 50 | n=gps.longitude; //現在の緯度 |
keepyourselfalive | 0:a56e760b5d55 | 51 | x=m-x; |
keepyourselfalive | 0:a56e760b5d55 | 52 | y=n-y; |
keepyourselfalive | 0:a56e760b5d55 | 53 | double a[2]={x,y}; |
keepyourselfalive | 0:a56e760b5d55 | 54 | double b[2]={0-m,0-n}; //目的地の座標を(0,0)とした |
kaipon | 3:b4f715167119 | 55 | k=b[0]/0.000008983148616; |
kaipon | 3:b4f715167119 | 56 | l=b[1]/0.000010966382364; |
kaipon | 3:b4f715167119 | 57 | j=pow(k*k+l*l,0.5); |
keepyourselfalive | 0:a56e760b5d55 | 58 | double p=a[0]*a[0]+a[1]*a[1]; |
keepyourselfalive | 0:a56e760b5d55 | 59 | double q=b[0]*b[0]+b[1]*b[1]; |
keepyourselfalive | 0:a56e760b5d55 | 60 | double pp=pow(p,0.5); |
keepyourselfalive | 0:a56e760b5d55 | 61 | double qq=pow(q,0.5); |
keepyourselfalive | 0:a56e760b5d55 | 62 | i=(a[0]*b[0]+a[1]*b[1])/pp*qq; |
keepyourselfalive | 0:a56e760b5d55 | 63 | w=acos(i); |
kaipon | 2:591877e2c7d1 | 64 | double e = (0-n)/(0-m);//目的地の座標を(0,0)とした |
kaipon | 2:591877e2c7d1 | 65 | double f = (n-y)/(m-x); |
kaipon | 2:591877e2c7d1 | 66 | double c = (e-f)/(1+e*f); |
kaipon | 1:c9f35ca3f74f | 67 | double d = m*m+n*n; |
kaipon | 1:c9f35ca3f74f | 68 | double dd = pow(d,0.5); //目的地までの距離 |
kaipon | 3:b4f715167119 | 69 | double t[2] = {5*w/3.14159,2*j/3}; //方向転換は5秒以内、1はモーターの1m/sで進むための時間。再度図るのは、全体の2/3だけ進んだときにしました。 |
kaipon | 2:591877e2c7d1 | 70 | if(c*w>0){ |
kaipon | 2:591877e2c7d1 | 71 | driveMotor(1,0); //方向転換、単位は1秒あたり36度。 |
kaipon | 2:591877e2c7d1 | 72 | wait(t[0]); |
kaipon | 2:591877e2c7d1 | 73 | driveMotor(0,0); |
kaipon | 2:591877e2c7d1 | 74 | wait(0.5); |
kaipon | 2:591877e2c7d1 | 75 | driveMotor(1,1); |
kaipon | 2:591877e2c7d1 | 76 | wait(t[1]); |
kaipon | 2:591877e2c7d1 | 77 | } |
kaipon | 2:591877e2c7d1 | 78 | else if(c*w<0){ |
kaipon | 2:591877e2c7d1 | 79 | driveMotor(0,1); |
kaipon | 2:591877e2c7d1 | 80 | wait(t[0]); |
kaipon | 2:591877e2c7d1 | 81 | driveMotor(0,0); |
kaipon | 2:591877e2c7d1 | 82 | wait(0.5); |
kaipon | 2:591877e2c7d1 | 83 | driveMotor(1,1); |
kaipon | 2:591877e2c7d1 | 84 | wait(t[1]); |
kaipon | 2:591877e2c7d1 | 85 | } |
kaipon | 3:b4f715167119 | 86 | //ちゃんと前後確認するプログラムを書く。いったん、0度と180度かの判断はあきらめて、0度と180度の時は、違う方角を向くようにしました。 |
kaipon | 2:591877e2c7d1 | 87 | else{ |
kaipon | 3:b4f715167119 | 88 | driveMotor(1,0); //直線上にある時は、18度だけ向きを変更します。 |
kaipon | 3:b4f715167119 | 89 | wait(0.5); |
kaipon | 3:b4f715167119 | 90 | driveMotor(0,0); |
kaipon | 2:591877e2c7d1 | 91 | } |
keepyourselfalive | 0:a56e760b5d55 | 92 | } |
keepyourselfalive | 0:a56e760b5d55 | 93 | } |
keepyourselfalive | 0:a56e760b5d55 | 94 | |
keepyourselfalive | 0:a56e760b5d55 | 95 |