CanSat-C 2021 / Mbed 2 deprecated warikomi_flag1

flagを1つにした。frontgetを割り込みにした

Dependencies:   ATP3012 mbed a HMC US015_2 getGPS

Function.h@2:5a54fa1f8e25, 2021-12-17 (annotated)

Committer:
miyajitakenari
Date:
Fri Dec 17 08:21:28 2021 +0000
Revision:
2:5a54fa1f8e25
Parent:
0:18be66de24f7 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
miyajitakenari 0:18be66de24f7 1 #include "mbed.h"
miyajitakenari 0:18be66de24f7 2 #include "getGPS.h"
miyajitakenari 0:18be66de24f7 3 #include "us015.h"
miyajitakenari 0:18be66de24f7 4 #include "HMC6352.h"
miyajitakenari 0:18be66de24f7 5 #include "TB6612.h"
miyajitakenari 0:18be66de24f7 6 #include "ATP3011.h"
miyajitakenari 0:18be66de24f7 7
miyajitakenari 0:18be66de24f7 8 US015 hs(D2, D3);
miyajitakenari 0:18be66de24f7 9 DigitalOut Ultra(D2);
miyajitakenari 0:18be66de24f7 10 GPS gps(D1, D0);
miyajitakenari 0:18be66de24f7 11 HMC6352 compass(D4, D5);
miyajitakenari 0:18be66de24f7 12 ATP3011 talk(D4,D5); // I2C sda scl
miyajitakenari 0:18be66de24f7 13 TB6612 motor_a(D10,D6,D7); //モータA制御用(pwma,ain1,ain2)
miyajitakenari 0:18be66de24f7 14 TB6612 motor_b(D11,D8,D9); //モータB制御用(pwmb,bin1,bin2)
miyajitakenari 0:18be66de24f7 15 Serial xbee(A7, A2);
miyajitakenari 0:18be66de24f7 16
miyajitakenari 0:18be66de24f7 17 double GPS_x, GPS_y; // 現在地の座標
miyajitakenari 0:18be66de24f7 18 double next_CP_x, next_CP_y;
miyajitakenari 0:18be66de24f7 19 double CPs_x[100]; // = []; //CPリスト(x座標)
miyajitakenari 0:18be66de24f7 20 double CPs_y[100]; // = []; // CPリスト(y座標)
miyajitakenari 0:18be66de24f7 21 double theta;
miyajitakenari 0:18be66de24f7 22 double delta;
miyajitakenari 0:18be66de24f7 23
miyajitakenari 0:18be66de24f7 24 int FrontGet()
miyajitakenari 0:18be66de24f7 25 {
miyajitakenari 0:18be66de24f7 26 Ultra = 1; //超音波on
miyajitakenari 0:18be66de24f7 27 hs.TrigerOut();
miyajitakenari 0:18be66de24f7 28 wait(1);
miyajitakenari 0:18be66de24f7 29 int distance;
miyajitakenari 0:18be66de24f7 30 distance = hs.GetDistance();
miyajitakenari 0:18be66de24f7 31 xbee.printf("distance=%d\r\n", distance); //距離出力
miyajitakenari 0:18be66de24f7 32 Ultra=0;//超音波off
miyajitakenari 0:18be66de24f7 33
miyajitakenari 0:18be66de24f7 34 if(distance < 1500) {
miyajitakenari 0:18be66de24f7 35 return 1;
miyajitakenari 0:18be66de24f7 36 }
miyajitakenari 0:18be66de24f7 37 else {
miyajitakenari 0:18be66de24f7 38 return 0;
miyajitakenari 0:18be66de24f7 39 }
miyajitakenari 0:18be66de24f7 40 }
miyajitakenari 0:18be66de24f7 41
miyajitakenari 0:18be66de24f7 42
miyajitakenari 0:18be66de24f7 43 void catchGPS()
miyajitakenari 0:18be66de24f7 44 {
miyajitakenari 0:18be66de24f7 45 xbee.printf("GPS Start\r\n");
miyajitakenari 0:18be66de24f7 46 /* 1秒ごとに現在地を取得してターミナル出力 */
miyajitakenari 0:18be66de24f7 47 while(1) {
miyajitakenari 0:18be66de24f7 48 if(gps.getgps()) { //現在地取得
miyajitakenari 0:18be66de24f7 49 xbee.printf("%lf %lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
miyajitakenari 0:18be66de24f7 50 GPS_x = gps.latitude;
miyajitakenari 0:18be66de24f7 51 GPS_y = gps.longitude;
miyajitakenari 0:18be66de24f7 52 break;
miyajitakenari 0:18be66de24f7 53 }
miyajitakenari 0:18be66de24f7 54 else {
miyajitakenari 0:18be66de24f7 55 xbee.printf("No data\r\n");//データ取得に失敗した場合
miyajitakenari 0:18be66de24f7 56 wait(1);
miyajitakenari 0:18be66de24f7 57 }
miyajitakenari 0:18be66de24f7 58 }
miyajitakenari 0:18be66de24f7 59 return;
miyajitakenari 0:18be66de24f7 60 }
miyajitakenari 0:18be66de24f7 61
miyajitakenari 0:18be66de24f7 62
miyajitakenari 0:18be66de24f7 63 void Move(char input_data, float motor_speed) {
miyajitakenari 0:18be66de24f7 64 switch (input_data) {
miyajitakenari 0:18be66de24f7 65 case '1': // 停止
miyajitakenari 0:18be66de24f7 66 motor_a = 0;
miyajitakenari 0:18be66de24f7 67 motor_b = 0;
miyajitakenari 0:18be66de24f7 68 break;
miyajitakenari 0:18be66de24f7 69 case '2': // 前進
miyajitakenari 0:18be66de24f7 70 motor_a = motor_speed;
miyajitakenari 0:18be66de24f7 71 motor_b = motor_speed;
miyajitakenari 0:18be66de24f7 72 break;
miyajitakenari 0:18be66de24f7 73 case '3': // 後退
miyajitakenari 0:18be66de24f7 74 motor_a = -motor_speed;
miyajitakenari 0:18be66de24f7 75 motor_b = -motor_speed;
miyajitakenari 0:18be66de24f7 76 break;
miyajitakenari 0:18be66de24f7 77 case '4': // 時計回りに回転
miyajitakenari 0:18be66de24f7 78 motor_a = motor_speed;
miyajitakenari 0:18be66de24f7 79 motor_b = -motor_speed;
miyajitakenari 0:18be66de24f7 80 break;
miyajitakenari 0:18be66de24f7 81 case '5': // 反時計回りに回転
miyajitakenari 0:18be66de24f7 82 motor_a = -motor_speed;
miyajitakenari 0:18be66de24f7 83 motor_b = motor_speed;
miyajitakenari 0:18be66de24f7 84 break;
miyajitakenari 0:18be66de24f7 85 case '6': // Aのみ正転
miyajitakenari 0:18be66de24f7 86 motor_a = motor_speed;
miyajitakenari 0:18be66de24f7 87 break;
miyajitakenari 0:18be66de24f7 88 case '7': // Bのみ正転
miyajitakenari 0:18be66de24f7 89 motor_b = motor_speed;
miyajitakenari 0:18be66de24f7 90 break;
miyajitakenari 0:18be66de24f7 91 case '8': // Aのみ逆転
miyajitakenari 0:18be66de24f7 92 motor_a = -motor_speed;
miyajitakenari 0:18be66de24f7 93 break;
miyajitakenari 0:18be66de24f7 94 case '9': // Bのみ逆転
miyajitakenari 0:18be66de24f7 95 motor_b = -motor_speed;
miyajitakenari 0:18be66de24f7 96 break;
miyajitakenari 0:18be66de24f7 97 }
miyajitakenari 0:18be66de24f7 98 }
miyajitakenari 0:18be66de24f7 99
miyajitakenari 0:18be66de24f7 100 double AngleGet()
miyajitakenari 0:18be66de24f7 101 {
miyajitakenari 0:18be66de24f7 102 double angle = 0;
miyajitakenari 0:18be66de24f7 103 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
miyajitakenari 0:18be66de24f7 104 angle = compass.sample() / 10;
miyajitakenari 0:18be66de24f7 105
miyajitakenari 0:18be66de24f7 106 double theta;
miyajitakenari 0:18be66de24f7 107 double delta;
miyajitakenari 0:18be66de24f7 108
miyajitakenari 0:18be66de24f7 109 xbee.printf("gps.latitude=%f, gps.longitude=%f\r\n", gps.latitude, gps.longitude);
miyajitakenari 0:18be66de24f7 110 theta = atan2(next_CP_y - gps.longitude , next_CP_x - gps.latitude) * 180 / 3.14159265359;
miyajitakenari 0:18be66de24f7 111 printf("theta=%f\r\n", theta);
miyajitakenari 0:18be66de24f7 112 if(theta >= 0) {
miyajitakenari 0:18be66de24f7 113 delta = angle - theta;
miyajitakenari 0:18be66de24f7 114 }
miyajitakenari 0:18be66de24f7 115 else {
miyajitakenari 0:18be66de24f7 116 theta = theta + 360;
miyajitakenari 0:18be66de24f7 117 delta = angle - theta;
miyajitakenari 0:18be66de24f7 118 }
miyajitakenari 0:18be66de24f7 119 if(delta<0){
miyajitakenari 0:18be66de24f7 120 delta+=360;
miyajitakenari 0:18be66de24f7 121 }
miyajitakenari 0:18be66de24f7 122 printf("%f-%f=%f\r\n", angle, theta, delta);
miyajitakenari 0:18be66de24f7 123 wait(2);
miyajitakenari 0:18be66de24f7 124 return delta;
miyajitakenari 0:18be66de24f7 125 }
miyajitakenari 0:18be66de24f7 126
miyajitakenari 0:18be66de24f7 127 void Calibration()
miyajitakenari 0:18be66de24f7 128 {
miyajitakenari 0:18be66de24f7 129 xbee.printf("calibration start\r\n");
miyajitakenari 0:18be66de24f7 130 compass.setCalibrationMode(0x43);
miyajitakenari 0:18be66de24f7 131 Move('4', 0.15);
miyajitakenari 0:18be66de24f7 132 xbee.printf("mortor mode:4 speed:0.15\n\r");
miyajitakenari 0:18be66de24f7 133 wait(60);
miyajitakenari 0:18be66de24f7 134 Move('1', 0);
miyajitakenari 0:18be66de24f7 135 xbee.printf("mortor mode:1 speed:0\n\r");
miyajitakenari 0:18be66de24f7 136 compass.setCalibrationMode(0x45);
miyajitakenari 0:18be66de24f7 137 xbee.printf("calibration end\r\n");
miyajitakenari 0:18be66de24f7 138 while(1) {
miyajitakenari 0:18be66de24f7 139 if(gps.getgps()) { //現在地取得
miyajitakenari 0:18be66de24f7 140 xbee.printf("lati=%lf\nlong=%lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
miyajitakenari 0:18be66de24f7 141 GPS_x = gps.latitude;
miyajitakenari 0:18be66de24f7 142 GPS_y = gps.longitude;
miyajitakenari 0:18be66de24f7 143 break;
miyajitakenari 0:18be66de24f7 144 }
miyajitakenari 0:18be66de24f7 145 else {
miyajitakenari 0:18be66de24f7 146 xbee.printf("No data\r\n");//データ取得に失敗した場合
miyajitakenari 0:18be66de24f7 147 wait(1);
miyajitakenari 0:18be66de24f7 148 }
miyajitakenari 0:18be66de24f7 149 }
miyajitakenari 0:18be66de24f7 150
miyajitakenari 0:18be66de24f7 151 return;
miyajitakenari 0:18be66de24f7 152 }
miyajitakenari 0:18be66de24f7 153
miyajitakenari 0:18be66de24f7 154 /*地上局から新情報を送るときはflag=がでてきたらスペースか.を入力
miyajitakenari 0:18be66de24f7 155 3秒後ぐらいにmessage=が出てくるので、そしたら新情報を入力*/
miyajitakenari 0:18be66de24f7 156 void speak()
miyajitakenari 0:18be66de24f7 157 {
miyajitakenari 0:18be66de24f7 158 int timeout_ms=500;
miyajitakenari 0:18be66de24f7 159 char mess[100];
miyajitakenari 0:18be66de24f7 160 float as;
miyajitakenari 0:18be66de24f7 161 if(talk.IsActive(timeout_ms)==true){
miyajitakenari 0:18be66de24f7 162 xbee.printf("Active\n\rflag=");
miyajitakenari 0:18be66de24f7 163 wait(3);
miyajitakenari 0:18be66de24f7 164 if(xbee.readable()){
miyajitakenari 0:18be66de24f7 165 xbee.printf("\n\rmessage=");
miyajitakenari 0:18be66de24f7 166 int i=0;
miyajitakenari 0:18be66de24f7 167 do{
miyajitakenari 0:18be66de24f7 168 mess[i++]= xbee.getc();
miyajitakenari 0:18be66de24f7 169 }
miyajitakenari 0:18be66de24f7 170 while(mess[i-1]!= 0x0d && i<99);
miyajitakenari 0:18be66de24f7 171 talk.Synthe(mess);
miyajitakenari 0:18be66de24f7 172 xbee.printf("advance speed=");
miyajitakenari 0:18be66de24f7 173 xbee.scanf("%f",&as);
miyajitakenari 0:18be66de24f7 174 }
miyajitakenari 0:18be66de24f7 175 else{
miyajitakenari 0:18be66de24f7 176 xbee.printf("preset_message speak\r\n");
miyajitakenari 0:18be66de24f7 177 talk.Synthe("hinannyuudoudesu,,tuitekitekudasai.");
miyajitakenari 0:18be66de24f7 178 as=0.39;
miyajitakenari 0:18be66de24f7 179 }
miyajitakenari 0:18be66de24f7 180 }
miyajitakenari 0:18be66de24f7 181 else{
miyajitakenari 0:18be66de24f7 182 xbee.printf("\r\nNot Active\n");
miyajitakenari 0:18be66de24f7 183 as=0.39;
miyajitakenari 0:18be66de24f7 184 }
miyajitakenari 0:18be66de24f7 185 Move('2', as);
miyajitakenari 0:18be66de24f7 186 xbee.printf("mortor mode:2 speed:%f",as);
miyajitakenari 0:18be66de24f7 187 }