CanSat-C 2021 / Mbed 2 deprecated MoveTest-Function_ver

移動系の統合試験用プログラムです。

Dependencies:   mbed TB6612FNG HMC6352 getGPS

Function.h@2:0c73afb20925, 2021-10-24 (annotated)

Committer:
ushiroji
Date:
Sun Oct 24 14:46:02 2021 +0000
Revision:
2:0c73afb20925
Child:
4:135619de1646
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ushiroji 2:0c73afb20925 1 #include "mbed.h"
ushiroji 2:0c73afb20925 2 #include "getGPS.h"
ushiroji 2:0c73afb20925 3 #include "us015.h"
ushiroji 2:0c73afb20925 4 #include "HMC6352.h"
ushiroji 2:0c73afb20925 5 #include "Math.h"
ushiroji 2:0c73afb20925 6
ushiroji 2:0c73afb20925 7 US015 hs(D2, D3);
ushiroji 2:0c73afb20925 8 DigitalOut Ultra(D2);
ushiroji 2:0c73afb20925 9 GPS gps(D1, D0);
ushiroji 2:0c73afb20925 10 HMC6352 compass(D4, D5);
ushiroji 2:0c73afb20925 11 Serial pc(USBTX, USBRX);
ushiroji 2:0c73afb20925 12 double GPS_x, GPS_y; // 現在地の座標
ushiroji 2:0c73afb20925 13 double next_CP_x, next_CP_y;
ushiroji 2:0c73afb20925 14 double CPs_x[100]; // = []; //CPリスト(x座標)
ushiroji 2:0c73afb20925 15 double CPs_y[100]; // = []; // CPリスト(y座標)
ushiroji 2:0c73afb20925 16 double *pGPS_x = &GPS_x, *pGPS_y = &GPS_y;
ushiroji 2:0c73afb20925 17
ushiroji 2:0c73afb20925 18 int FrontGet()
ushiroji 2:0c73afb20925 19 {
ushiroji 2:0c73afb20925 20 Ultra = 1; //超音波on
ushiroji 2:0c73afb20925 21 hs.TrigerOut();
ushiroji 2:0c73afb20925 22 wait(1);
ushiroji 2:0c73afb20925 23 int distance;
ushiroji 2:0c73afb20925 24 distance = hs.GetDistance();
ushiroji 2:0c73afb20925 25 pc.printf("distance=%d\r\n", distance); //距離出力
ushiroji 2:0c73afb20925 26 Ultra=0;//超音波off
ushiroji 2:0c73afb20925 27
ushiroji 2:0c73afb20925 28 if(distance < 200) {
ushiroji 2:0c73afb20925 29 return 1;
ushiroji 2:0c73afb20925 30 }
ushiroji 2:0c73afb20925 31 else {
ushiroji 2:0c73afb20925 32 return 0;
ushiroji 2:0c73afb20925 33 }
ushiroji 2:0c73afb20925 34 }
ushiroji 2:0c73afb20925 35
ushiroji 2:0c73afb20925 36 int catchGPS()
ushiroji 2:0c73afb20925 37 {
ushiroji 2:0c73afb20925 38 pc.printf("GPS Start\n");
ushiroji 2:0c73afb20925 39 /* 1秒ごとに現在地を取得してターミナル出力 */
ushiroji 2:0c73afb20925 40 while(1) {
ushiroji 2:0c73afb20925 41 if(gps.getgps()) { //現在地取得
ushiroji 2:0c73afb20925 42 pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
ushiroji 2:0c73afb20925 43 *pGPS_x = gps.latitude;
ushiroji 2:0c73afb20925 44 *pGPS_y = gps.longitude;
ushiroji 2:0c73afb20925 45 break;
ushiroji 2:0c73afb20925 46 }
ushiroji 2:0c73afb20925 47 else {
ushiroji 2:0c73afb20925 48 pc.printf("No data\n");//データ取得に失敗した場合
ushiroji 2:0c73afb20925 49 wait(1);
ushiroji 2:0c73afb20925 50 }
ushiroji 2:0c73afb20925 51 }
ushiroji 2:0c73afb20925 52 return 0;
ushiroji 2:0c73afb20925 53 }
ushiroji 2:0c73afb20925 54
ushiroji 2:0c73afb20925 55
ushiroji 2:0c73afb20925 56 int AngleGet()
ushiroji 2:0c73afb20925 57 {
ushiroji 2:0c73afb20925 58 while(1) {
ushiroji 2:0c73afb20925 59 double angle;
ushiroji 2:0c73afb20925 60 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
ushiroji 2:0c73afb20925 61 angle = compass.sample() / 10;
ushiroji 2:0c73afb20925 62
ushiroji 2:0c73afb20925 63 double theta;
ushiroji 2:0c73afb20925 64 double delta;
ushiroji 2:0c73afb20925 65
ushiroji 2:0c73afb20925 66 if(gps.getgps()) {
ushiroji 2:0c73afb20925 67 pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
ushiroji 2:0c73afb20925 68 next_CP_x = gps.latitude;
ushiroji 2:0c73afb20925 69 next_CP_y = gps.longitude;
ushiroji 2:0c73afb20925 70 theta = atan(next_CP_x - 0 / next_CP_y - 0) * 180 / M_PI;
ushiroji 2:0c73afb20925 71 printf("%f", theta);
ushiroji 2:0c73afb20925 72 delta = theta - angle;
ushiroji 2:0c73afb20925 73 printf("%f-%f=%f\r\n", theta, angle, delta);
ushiroji 2:0c73afb20925 74 }
ushiroji 2:0c73afb20925 75 else {
ushiroji 2:0c73afb20925 76 pc.printf("No data\r\n");//データ取得に失敗した場合
ushiroji 2:0c73afb20925 77 wait(1);
ushiroji 2:0c73afb20925 78 }
ushiroji 2:0c73afb20925 79 wait(2);
ushiroji 2:0c73afb20925 80 }
ushiroji 2:0c73afb20925 81 }