Function.h

00001 #include "mbed.h"
00002 #include "getGPS.h"
00003 #include "us015.h"
00004 #include "HMC6352.h"
00005 #include "TB6612.h"
00006 #include "ATP3011.h"
00007 
00008 US015 hs(D2, D3);
00009 DigitalOut Ultra(D2);
00010 GPS gps(D1, D0);
00011 HMC6352 compass(D4, D5);
00012 ATP3011 talk(D4,D5); // I2C sda scl
00013 TB6612 motor_a(D10,D6,D7);  //モータA制御用(pwma,ain1,ain2)
00014 TB6612 motor_b(D11,D8,D9);  //モータB制御用(pwmb,bin1,bin2)
00015 Serial xbee(A7, A2);
00016 
00017 double GPS_x, GPS_y;  // 現在地の座標
00018 double next_CP_x, next_CP_y;
00019 double CPs_x[100];  // = [];  //CPリスト(x座標)
00020 double CPs_y[100];  // = [];  // CPリスト(y座標)
00021 double theta;
00022 double delta;
00023 
00024 int FrontGet() 
00025 {
00026         Ultra = 1;  //超音波on
00027         hs.TrigerOut();
00028         int distance;
00029         distance = hs.GetDistance(); 
00030         xbee.printf("distance=%d\r\n", distance);  //距離出力
00031         Ultra=0;//超音波off
00032         
00033         if(distance < 500) {
00034             return 1;
00035         }
00036         else {
00037             return 0;
00038         }
00039 }
00040 
00041 
00042 void catchGPS()
00043 {
00044     xbee.printf("GPS Start\r\n");
00045     /* 1秒ごとに現在地を取得してターミナル出力 */
00046     while(1) {
00047         if(gps.getgps()) {  //現在地取得
00048             xbee.printf("%lf %lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
00049             GPS_x = gps.latitude;
00050             GPS_y = gps.longitude;
00051             break;
00052         }
00053         else {
00054             xbee.printf("No data\r\n");//データ取得に失敗した場合
00055             wait(1);
00056         }
00057     }
00058 }
00059 
00060 
00061 void Move(char input_data, float motor_speed) {
00062     switch (input_data) {
00063         case '1':  // 停止
00064             motor_a = 0;
00065             motor_b = 0;
00066             break;
00067         case '2':  // 前進
00068             motor_a = motor_speed;
00069             motor_b = motor_speed;
00070             break;
00071         case '3':  // 後退
00072             motor_a = -motor_speed;
00073             motor_b = -motor_speed;
00074             break;
00075         case '4':  // 時計回りに回転
00076             motor_a = motor_speed;
00077             motor_b = -motor_speed;
00078             break;
00079         case '5':  // 反時計回りに回転
00080             motor_a = -motor_speed;
00081             motor_b = motor_speed;
00082             break;
00083         case '6':  // Aのみ正転
00084             motor_a = motor_speed;
00085             break;
00086         case '7':  // Bのみ正転
00087             motor_b = motor_speed;
00088             break;
00089         case '8':  // Aのみ逆転
00090             motor_a = -motor_speed;
00091             break;
00092         case '9':  // Bのみ逆転
00093             motor_b = -motor_speed;
00094             break;
00095     }
00096 }
00097 
00098 double AngleGet() 
00099 {
00100     double angle = 0;
00101     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00102     angle = compass.sample() / 10;
00103 
00104     double theta;
00105     double delta;
00106 
00107     xbee.printf("gps.latitude=%f, gps.longitude=%f\r\n", gps.latitude, gps.longitude);
00108     theta = atan2(next_CP_y - gps.longitude , next_CP_x - gps.latitude) * 180 / 3.14159265359;
00109     printf("theta=%f\r\n", theta);
00110     if(theta >= 0) {
00111         delta = angle - theta;
00112     }
00113     else {
00114         theta = theta + 360;
00115         delta = angle - theta;
00116     }
00117     printf("delta=%f-%f=%f\r\n", angle, theta, delta); 
00118     wait(2);
00119     return delta;
00120 }
00121 
00122 void Calibration()
00123 {
00124     xbee.printf("calibration start\r\n");
00125     compass.setCalibrationMode(0x43);
00126     Move('4', 0.1);
00127     xbee.printf("mortor mode:4 speed:0.1\n\r");
00128     wait(6);
00129     Move('1', 0);
00130     xbee.printf("mortor mode:1 speed:0\n\r");
00131     compass.setCalibrationMode(0x45);
00132     xbee.printf("calibration end\r\n");
00133     while(1) {
00134         if(gps.getgps()) {  //現在地取得
00135         GPS_x = gps.latitude;
00136         GPS_y = gps.longitude;
00137         break;
00138         }
00139         else {
00140             xbee.printf("No data\r\n");
00141             break;
00142         }
00143     }
00144 
00145     return;
00146 }
00147 
00148  /*地上局から新情報を送るときはflag=がでてきたらスペースか.を入力
00149  3秒後ぐらいにmessage=が出てくるので、そしたら新情報を入力*/
00150 void speak()
00151 {
00152     int timeout_ms=500;
00153     char mess[100];            
00154         if(talk.IsActive(timeout_ms)==true){
00155             xbee.printf("Active\n\rspeak flag=");
00156             wait(5);
00157             if(xbee.readable()){
00158                 xbee.printf("\n\rmessage=");
00159                 int i=0;
00160                     do{
00161                         mess[i++]= xbee.getc();
00162                     }
00163                     while(mess[i-1]!= 0x0d);
00164                 talk.Synthe(mess);
00165             }
00166             else{
00167                 xbee.printf("preset_message speak\r\n");
00168                 talk.Synthe("purissetommese-ji,,konnichiwa.");
00169             }
00170         }
00171         else{
00172             xbee.printf("\r\nNot Active\n");
00173         }
00174 }