CanSat-C 2021 / Mbed 2 deprecated MoveTest-Function_ver

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

Dependencies:   mbed TB6612FNG HMC6352 getGPS

Function.h@5:9f9cf3912fda, 2021-10-27 (annotated)

Committer:
ushiroji
Date:
Wed Oct 27 12:26:48 2021 +0000
Revision:
5:9f9cf3912fda
Parent:
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 4:135619de1646 17 double theta;
ushiroji 4:135619de1646 18 double delta;
ushiroji 2:0c73afb20925 19
ushiroji 2:0c73afb20925 20 int FrontGet()
ushiroji 2:0c73afb20925 21 {
ushiroji 2:0c73afb20925 22 Ultra = 1; //超音波on
ushiroji 2:0c73afb20925 23 hs.TrigerOut();
ushiroji 2:0c73afb20925 24 wait(1);
ushiroji 2:0c73afb20925 25 int distance;
ushiroji 2:0c73afb20925 26 distance = hs.GetDistance();
ushiroji 2:0c73afb20925 27 pc.printf("distance=%d\r\n", distance); //距離出力
ushiroji 2:0c73afb20925 28 Ultra=0;//超音波off
ushiroji 2:0c73afb20925 29
ushiroji 2:0c73afb20925 30 if(distance < 200) {
ushiroji 2:0c73afb20925 31 return 1;
ushiroji 2:0c73afb20925 32 }
ushiroji 2:0c73afb20925 33 else {
ushiroji 2:0c73afb20925 34 return 0;
ushiroji 2:0c73afb20925 35 }
ushiroji 2:0c73afb20925 36 }
ushiroji 2:0c73afb20925 37
ushiroji 2:0c73afb20925 38 int catchGPS()
ushiroji 2:0c73afb20925 39 {
ushiroji 4:135619de1646 40 pc.printf("GPS Start\r\n");
ushiroji 2:0c73afb20925 41 /* 1秒ごとに現在地を取得してターミナル出力 */
ushiroji 2:0c73afb20925 42 while(1) {
ushiroji 2:0c73afb20925 43 if(gps.getgps()) { //現在地取得
ushiroji 2:0c73afb20925 44 pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
ushiroji 2:0c73afb20925 45 *pGPS_x = gps.latitude;
ushiroji 2:0c73afb20925 46 *pGPS_y = gps.longitude;
ushiroji 2:0c73afb20925 47 break;
ushiroji 2:0c73afb20925 48 }
ushiroji 2:0c73afb20925 49 else {
ushiroji 4:135619de1646 50 pc.printf("No data\r\n");//データ取得に失敗した場合
ushiroji 2:0c73afb20925 51 wait(1);
ushiroji 2:0c73afb20925 52 }
ushiroji 2:0c73afb20925 53 }
ushiroji 2:0c73afb20925 54 return 0;
ushiroji 2:0c73afb20925 55 }
ushiroji 2:0c73afb20925 56
ushiroji 2:0c73afb20925 57
ushiroji 4:135619de1646 58 double AngleGet()
ushiroji 2:0c73afb20925 59 {
ushiroji 2:0c73afb20925 60 double angle;
ushiroji 2:0c73afb20925 61 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
ushiroji 2:0c73afb20925 62 angle = compass.sample() / 10;
ushiroji 2:0c73afb20925 63
ushiroji 4:135619de1646 64 double next_x;
ushiroji 4:135619de1646 65 double next_y;
ushiroji 4:135619de1646 66 double theta;
ushiroji 4:135619de1646 67 double delta;
ushiroji 4:135619de1646 68 //if(gps.getgps()) {
ushiroji 4:135619de1646 69 pc.printf("%f, %f\r\n", 0, 0); //gps.latitude=0, gps.longitude=0
ushiroji 4:135619de1646 70 next_x = 0;
ushiroji 4:135619de1646 71 next_y = 100;
ushiroji 4:135619de1646 72 theta = atan(next_x - 0 / next_y - 0) * 180 / M_PI;
ushiroji 4:135619de1646 73 printf("theta=%f\r\n", theta);
ushiroji 4:135619de1646 74 if(theta >= 0) {
ushiroji 4:135619de1646 75 delta = angle - theta;
ushiroji 4:135619de1646 76 }
ushiroji 4:135619de1646 77 else {
ushiroji 4:135619de1646 78 theta = theta + 360;
ushiroji 4:135619de1646 79 delta = angle - theta;
ushiroji 4:135619de1646 80 }
ushiroji 4:135619de1646 81 printf("%f-%f=%f\r\n", angle, theta, delta);
ushiroji 4:135619de1646 82 //}
ushiroji 4:135619de1646 83 /*
ushiroji 4:135619de1646 84 else {
ushiroji 4:135619de1646 85 pc.printf("No data\r\n");//データ取得に失敗した場合
ushiroji 4:135619de1646 86 wait(1);
ushiroji 4:135619de1646 87 }
ushiroji 4:135619de1646 88 */
ushiroji 2:0c73afb20925 89 wait(2);
ushiroji 4:135619de1646 90 return delta;
ushiroji 4:135619de1646 91 }
ushiroji 4:135619de1646 92 /*
ushiroji 4:135619de1646 93 double angle;
ushiroji 4:135619de1646 94 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
ushiroji 4:135619de1646 95 angle = compass.sample() / 10;
ushiroji 4:135619de1646 96
ushiroji 4:135619de1646 97 if(gps.getgps()) {
ushiroji 4:135619de1646 98 pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
ushiroji 4:135619de1646 99 next_CP_x = gps.latitude;
ushiroji 4:135619de1646 100 next_CP_y = gps.longitude;
ushiroji 4:135619de1646 101 theta = atan(next_CP_x - 0 / next_CP_y - 0) * 180 / M_PI;
ushiroji 4:135619de1646 102 printf("%f", theta);
ushiroji 4:135619de1646 103 delta = theta - angle;
ushiroji 4:135619de1646 104 printf("%f-%f=%f\r\n", theta, angle, delta);
ushiroji 4:135619de1646 105 return delta;
ushiroji 2:0c73afb20925 106 }
ushiroji 4:135619de1646 107 else {
ushiroji 4:135619de1646 108 pc.printf("No data\r\n");//データ取得に失敗した場合
ushiroji 4:135619de1646 109 wait(1);
ushiroji 4:135619de1646 110 }
ushiroji 4:135619de1646 111 }
ushiroji 4:135619de1646 112 */