CanSat-C 2021 / Mbed 2 deprecated test_notchange

気球試験1,2回目 3回目はtest2-xbee

Dependencies:   ATP3012 mbed a HMC US015_2 getGPS

Function.h@0:e386cbba36d5, 2021-12-19 (annotated)

Committer:
miyajitakenari
Date:
Sun Dec 19 09:31:36 2021 +0000
Revision:
0:e386cbba36d5
1,2kaime

Who changed what in which revision?

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