CanSat-C 2021 / Mbed 2 deprecated Run-GPS

座標判定用のプログラムです。 (モータードライバー＋GPS)

Dependencies:   ATP3012 mbed a HMC US015_2 getGPS

Function.h@1:db6b55732559, 2021-12-03 (annotated)

Committer:
ushiroji
Date:
Fri Dec 03 08:27:23 2021 +0000
Revision:
1:db6b55732559
Parent:
0:99f4fe3e21c6 
first version

Who changed what in which revision?

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