CanSat-C 2021 / Mbed 2 deprecated cansatC_test2_xbee2

12/16用テスト

Dependencies:   ATP3012 mbed a HMC US015_2 getGPS

Function.h@0:79033ee3c961, 2021-11-06 (annotated)

Committer:
miyajitakenari
Date:
Sat Nov 06 03:09:38 2021 +0000
Revision:
0:79033ee3c961
Child:
3:ec2b7587be78
a

Who changed what in which revision?

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