CanSat-C 2021 / Mbed 2 deprecated test

テスト用　gps手入力

Dependencies:   ATP3012 mbed a HMC US015_2 getGPS

Function.h@0:f7517c11d468, 2021-11-11 (annotated)

Committer:
miyajitakenari
Date:
Thu Nov 11 09:41:22 2021 +0000
Revision:
0:f7517c11d468
Child:
1:41dfafb10c80
test you

Who changed what in which revision?

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