CanSat-C 2021 / Mbed 2 deprecated Run_test

走行試験用のプログラムです。

Dependencies:   mbed TB6612FNG HMC6352 US015 getGPS

Function.h@0:0454d5b5905b, 2021-11-06 (annotated)

Committer:
ushiroji
Date:
Sat Nov 06 01:27:45 2021 +0000
Revision:
0:0454d5b5905b
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ushiroji 0:0454d5b5905b 1 #include "mbed.h"
ushiroji 0:0454d5b5905b 2 #include "getGPS.h"
ushiroji 0:0454d5b5905b 3 #include "us015.h"
ushiroji 0:0454d5b5905b 4 #include "HMC6352.h"
ushiroji 0:0454d5b5905b 5 #include "Math.h"
ushiroji 0:0454d5b5905b 6 #include "TB6612.h"
ushiroji 0:0454d5b5905b 7
ushiroji 0:0454d5b5905b 8 US015 hs(D2, D3);
ushiroji 0:0454d5b5905b 9 DigitalOut Ultra(D2);
ushiroji 0:0454d5b5905b 10 GPS gps(D1, D0);
ushiroji 0:0454d5b5905b 11 HMC6352 compass(D4, D5);
ushiroji 0:0454d5b5905b 12 TB6612 motor_a(D10,D6,D7); //モータA制御用(pwma,ain1,ain2)
ushiroji 0:0454d5b5905b 13 TB6612 motor_b(D11,D8,D9); //モータB制御用(pwmb,bin1,bin2)
ushiroji 0:0454d5b5905b 14 Serial pc(USBTX, USBRX);
ushiroji 0:0454d5b5905b 15
ushiroji 0:0454d5b5905b 16 double GPS_x, GPS_y; // 現在地の座標
ushiroji 0:0454d5b5905b 17 double next_CP_x, next_CP_y;
ushiroji 0:0454d5b5905b 18 double CPs_x[100]; // = []; //CPリスト(x座標)
ushiroji 0:0454d5b5905b 19 double CPs_y[100]; // = []; // CPリスト(y座標)
ushiroji 0:0454d5b5905b 20 double *pGPS_x = &GPS_x, *pGPS_y = &GPS_y;
ushiroji 0:0454d5b5905b 21 double theta;
ushiroji 0:0454d5b5905b 22 double delta;
ushiroji 0:0454d5b5905b 23
ushiroji 0:0454d5b5905b 24 int FrontGet()
ushiroji 0:0454d5b5905b 25 {
ushiroji 0:0454d5b5905b 26 Ultra = 1; //超音波on
ushiroji 0:0454d5b5905b 27 hs.TrigerOut();
ushiroji 0:0454d5b5905b 28 wait(1);
ushiroji 0:0454d5b5905b 29 int distance;
ushiroji 0:0454d5b5905b 30 distance = hs.GetDistance();
ushiroji 0:0454d5b5905b 31 pc.printf("distance=%d\r\n", distance); //距離出力
ushiroji 0:0454d5b5905b 32 Ultra=0;//超音波off
ushiroji 0:0454d5b5905b 33
ushiroji 0:0454d5b5905b 34 if(distance < 200) {
ushiroji 0:0454d5b5905b 35 return 1;
ushiroji 0:0454d5b5905b 36 }
ushiroji 0:0454d5b5905b 37 else {
ushiroji 0:0454d5b5905b 38 return 0;
ushiroji 0:0454d5b5905b 39 }
ushiroji 0:0454d5b5905b 40 }
ushiroji 0:0454d5b5905b 41
ushiroji 0:0454d5b5905b 42
ushiroji 0:0454d5b5905b 43 int catchGPS()
ushiroji 0:0454d5b5905b 44 {
ushiroji 0:0454d5b5905b 45 pc.printf("GPS Start\r\n");
ushiroji 0:0454d5b5905b 46 /* 1秒ごとに現在地を取得してターミナル出力 */
ushiroji 0:0454d5b5905b 47 while(1) {
ushiroji 0:0454d5b5905b 48 if(gps.getgps()) { //現在地取得
ushiroji 0:0454d5b5905b 49 pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
ushiroji 0:0454d5b5905b 50 *pGPS_x = gps.latitude;
ushiroji 0:0454d5b5905b 51 *pGPS_y = gps.longitude;
ushiroji 0:0454d5b5905b 52 break;
ushiroji 0:0454d5b5905b 53 }
ushiroji 0:0454d5b5905b 54 else {
ushiroji 0:0454d5b5905b 55 pc.printf("No data\r\n");//データ取得に失敗した場合
ushiroji 0:0454d5b5905b 56 wait(1);
ushiroji 0:0454d5b5905b 57 }
ushiroji 0:0454d5b5905b 58 }
ushiroji 0:0454d5b5905b 59 return 0;
ushiroji 0:0454d5b5905b 60 }
ushiroji 0:0454d5b5905b 61
ushiroji 0:0454d5b5905b 62
ushiroji 0:0454d5b5905b 63 void Move(char input_data, float motor_speed) {
ushiroji 0:0454d5b5905b 64 switch (input_data) {
ushiroji 0:0454d5b5905b 65 case '1': // 停止
ushiroji 0:0454d5b5905b 66 motor_a = 0;
ushiroji 0:0454d5b5905b 67 motor_b = 0;
ushiroji 0:0454d5b5905b 68 break;
ushiroji 0:0454d5b5905b 69 case '2': // 前進
ushiroji 0:0454d5b5905b 70 motor_a = motor_speed;
ushiroji 0:0454d5b5905b 71 motor_b = motor_speed;
ushiroji 0:0454d5b5905b 72 break;
ushiroji 0:0454d5b5905b 73 case '3': // 後退
ushiroji 0:0454d5b5905b 74 motor_a = -motor_speed;
ushiroji 0:0454d5b5905b 75 motor_b = -motor_speed;
ushiroji 0:0454d5b5905b 76 break;
ushiroji 0:0454d5b5905b 77 case '4': // 時計回りに回転
ushiroji 0:0454d5b5905b 78 motor_a = motor_speed;
ushiroji 0:0454d5b5905b 79 motor_b = -motor_speed;
ushiroji 0:0454d5b5905b 80 break;
ushiroji 0:0454d5b5905b 81 case '5': // 反時計回りに回転
ushiroji 0:0454d5b5905b 82 motor_a = -motor_speed;
ushiroji 0:0454d5b5905b 83 motor_b = motor_speed;
ushiroji 0:0454d5b5905b 84 break;
ushiroji 0:0454d5b5905b 85 case '6': // Aのみ正転
ushiroji 0:0454d5b5905b 86 motor_a = motor_speed;
ushiroji 0:0454d5b5905b 87 break;
ushiroji 0:0454d5b5905b 88 case '7': // Bのみ正転
ushiroji 0:0454d5b5905b 89 motor_b = motor_speed;
ushiroji 0:0454d5b5905b 90 break;
ushiroji 0:0454d5b5905b 91 case '8': // Aのみ逆転
ushiroji 0:0454d5b5905b 92 motor_a = -motor_speed;
ushiroji 0:0454d5b5905b 93 break;
ushiroji 0:0454d5b5905b 94 case '9': // Bのみ逆転
ushiroji 0:0454d5b5905b 95 motor_b = -motor_speed;
ushiroji 0:0454d5b5905b 96 break;
ushiroji 0:0454d5b5905b 97 }
ushiroji 0:0454d5b5905b 98 pc.printf("input_data=%d\r\n", input_data);
ushiroji 0:0454d5b5905b 99 }
ushiroji 0:0454d5b5905b 100
ushiroji 0:0454d5b5905b 101
ushiroji 0:0454d5b5905b 102 void Rotate(double rot){
ushiroji 0:0454d5b5905b 103 // double time = angle; // 試験で調整
ushiroji 0:0454d5b5905b 104 int time = 3; // 試験用
ushiroji 0:0454d5b5905b 105 if(rot > 0) {
ushiroji 0:0454d5b5905b 106 Move('5', 1);
ushiroji 0:0454d5b5905b 107 wait(time);
ushiroji 0:0454d5b5905b 108 Move('1', 0);
ushiroji 0:0454d5b5905b 109 }
ushiroji 0:0454d5b5905b 110 else {
ushiroji 0:0454d5b5905b 111 Move('6', 1);
ushiroji 0:0454d5b5905b 112 wait(time);
ushiroji 0:0454d5b5905b 113 Move('1', 0);
ushiroji 0:0454d5b5905b 114 }
ushiroji 0:0454d5b5905b 115 }
ushiroji 0:0454d5b5905b 116
ushiroji 0:0454d5b5905b 117 double AngleGet()
ushiroji 0:0454d5b5905b 118 {
ushiroji 0:0454d5b5905b 119 double angle = 0;
ushiroji 0:0454d5b5905b 120 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
ushiroji 0:0454d5b5905b 121 angle = compass.sample() / 10;
ushiroji 0:0454d5b5905b 122
ushiroji 0:0454d5b5905b 123 double theta;
ushiroji 0:0454d5b5905b 124 double delta;
ushiroji 0:0454d5b5905b 125
ushiroji 0:0454d5b5905b 126 pc.printf("gps.latitude=%f, gps.longitude=%f\r\n", gps.latitude, gps.longitude);
ushiroji 0:0454d5b5905b 127 theta = atan2(next_CP_x - gps.latitude , next_CP_y - gps.longitude) * 180 / M_PI;
ushiroji 0:0454d5b5905b 128 printf("theta=%f\r\n", theta);
ushiroji 0:0454d5b5905b 129 if(theta >= 0) {
ushiroji 0:0454d5b5905b 130 delta = angle - theta;
ushiroji 0:0454d5b5905b 131 }
ushiroji 0:0454d5b5905b 132 else {
ushiroji 0:0454d5b5905b 133 theta = theta + 360;
ushiroji 0:0454d5b5905b 134 delta = angle - theta;
ushiroji 0:0454d5b5905b 135 }
ushiroji 0:0454d5b5905b 136 printf("%f-%f=%f\r\n", angle, theta, delta);
ushiroji 0:0454d5b5905b 137 wait(2);
ushiroji 0:0454d5b5905b 138 return delta;
ushiroji 0:0454d5b5905b 139 }
ushiroji 0:0454d5b5905b 140
ushiroji 0:0454d5b5905b 141 int Calibration()
ushiroji 0:0454d5b5905b 142 {
ushiroji 0:0454d5b5905b 143 pc.printf("calibration start\r\n");
ushiroji 0:0454d5b5905b 144 compass.setCalibrationMode(0x43);
ushiroji 0:0454d5b5905b 145 wait(60);
ushiroji 0:0454d5b5905b 146 compass.setCalibrationMode(0x45);
ushiroji 0:0454d5b5905b 147 pc.printf("calibration end\r\n");
ushiroji 0:0454d5b5905b 148 while(1) {
ushiroji 0:0454d5b5905b 149 if(gps.getgps()) { //現在地取得
ushiroji 0:0454d5b5905b 150 pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
ushiroji 0:0454d5b5905b 151 *pGPS_x = gps.latitude;
ushiroji 0:0454d5b5905b 152 *pGPS_y = gps.longitude;
ushiroji 0:0454d5b5905b 153 break;
ushiroji 0:0454d5b5905b 154 }
ushiroji 0:0454d5b5905b 155 else {
ushiroji 0:0454d5b5905b 156 pc.printf("No data\r\n");//データ取得に失敗した場合
ushiroji 0:0454d5b5905b 157 wait(1);
ushiroji 0:0454d5b5905b 158 }
ushiroji 0:0454d5b5905b 159 }
ushiroji 0:0454d5b5905b 160
ushiroji 0:0454d5b5905b 161 return 0;
ushiroji 0:0454d5b5905b 162 }