ushiroji takuma / Mbed 2 deprecated RadioControl

ラジコン用プログラムです。

Dependencies:   mbed ATP3012 a HMC US015_2 getGPS

Function.h@0:149a860cbcb9, 2021-12-10 (annotated)

Committer:
ushiroji
Date:
Fri Dec 10 03:58:41 2021 +0000
Revision:
0:149a860cbcb9
Child:
1:6ff5fbdf32bb
first version

Who changed what in which revision?

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