CanSat-C 2021 / Mbed 2 deprecated CanSat-C_test

統合試験用です。

Dependencies:   ATP3012 mbed TB6612FNG HMC US015 getGPS

Function.h@20:02afaa2186f3, 2021-11-04 (annotated)

Committer:
ushiroji
Date:
Thu Nov 04 11:35:18 2021 +0000
Revision:
20:02afaa2186f3
Parent:
17:1b6a84ab4433 
add Calibration to Function.h

Who changed what in which revision?

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