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 theta;
00019 double delta;
00020 
00021 int FrontGet() 
00022 {
00023         Ultra = 1;  //超音波on
00024         hs.TrigerOut();
00025         wait(1);
00026         int distance;
00027         distance = hs.GetDistance(); 
00028         xbee.printf("distance=%d\r\n", distance);  //距離出力
00029         Ultra=0;//超音波off
00030         
00031         if(distance < 1500) {
00032             return 1;
00033         }
00034         else {
00035             return 0;
00036         }
00037 }
00038 
00039 
00040 void catchGPS()
00041 {
00042     xbee.printf("GPS Start\r\n");
00043     /* 1秒ごとに現在地を取得してターミナル出力 */
00044     while(1) {
00045         if(gps.getgps()) {  //現在地取得
00046             xbee.printf("%lf %lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
00047             GPS_x = gps.latitude;
00048             GPS_y = gps.longitude;
00049             break;
00050         }
00051         else {
00052             xbee.printf("No data\r\n");//データ取得に失敗した場合
00053             wait(1);
00054         }
00055     }
00056     return;
00057 }
00058 
00059 
00060 void Move(char input_data, float motor_speed) {
00061     switch (input_data) {
00062         case '1':  // 停止
00063             motor_a = 0;
00064             motor_b = 0;
00065             break;
00066         case '2':  // 前進
00067             motor_a = motor_speed;
00068             motor_b = motor_speed;
00069             break;
00070         case '3':  // 後退
00071             motor_a = -motor_speed;
00072             motor_b = -motor_speed;
00073             break;
00074         case '4':  // 時計回りに回転
00075             motor_a = motor_speed;
00076             motor_b = -motor_speed;
00077             break;
00078         case '5':  // 反時計回りに回転
00079             motor_a = -motor_speed;
00080             motor_b = motor_speed;
00081             break;
00082         case '6':  // Aのみ正転
00083             motor_a = motor_speed;
00084             break;
00085         case '7':  // Bのみ正転
00086             motor_b = motor_speed;
00087             break;
00088         case '8':  // Aのみ逆転
00089             motor_a = -motor_speed;
00090             break;
00091         case '9':  // Bのみ逆転
00092             motor_b = -motor_speed;
00093             break;
00094     }
00095 }
00096 
00097 double AngleGet(double next_x, double next_y) 
00098 {
00099     double angle = 0;
00100     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00101     angle = compass.sample() / 10;
00102     
00103     double theta;
00104     double delta;
00105 
00106     xbee.printf("gps.latitude=%f, gps.longitude=%f\r\n", gps.latitude, gps.longitude);
00107     theta = atan2(next_y - gps.longitude , next_x - gps.latitude) * 180 / 3.14159265359;
00108     if(theta >= 0) {
00109         delta = angle - theta;
00110     }
00111     else {
00112         
00113         theta = theta + 360;
00114         delta = angle - theta;
00115     }    
00116     if(delta<0){
00117         delta+=360;
00118         }
00119     xbee.printf("%f-%f=%f\r\n", angle, theta, delta); 
00120     return delta;
00121 }
00122 
00123 void Calibration()
00124 {
00125     xbee.printf("calibration start\r\n");
00126     compass.setCalibrationMode(0x43);
00127     Move('4', 0.19);
00128     wait(60);
00129     Move('1', 0);
00130     compass.setCalibrationMode(0x45);
00131     xbee.printf("calibration end\r\n");
00132     while(1) {
00133         if(gps.getgps()) {  //現在地取得
00134         xbee.printf("lati=%lf\nlong=%lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
00135         GPS_x = gps.latitude;
00136         GPS_y = gps.longitude;
00137         break;
00138         }
00139         else {
00140             xbee.printf("No data\r\n");//データ取得に失敗した場合
00141             wait(1);
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     float as;          
00155         if(talk.IsActive(timeout_ms)==true){
00156             /*xbee.printf("Active\n\rflag=");
00157             wait(3);
00158             if(xbee.readable()){
00159                 xbee.printf("\n\rmessage=");
00160                 int i=0;
00161                 do{
00162                     mess[i++]= xbee.getc();
00163                     }
00164                     while(mess[i-1]!= 0x0d && i<99);
00165                 talk.Synthe(mess);
00166                 xbee.printf("advance speed=");
00167                 xbee.scanf("%f",&as);
00168             }
00169             else{*/
00170                 xbee.printf("preset_message speak\r\n");
00171                 talk.Synthe("hinannyuudoudesu,,tuitekitekudasai.");
00172                 as=0.39;
00173             //}
00174         }
00175         else{
00176             xbee.printf("\r\nNot Active\n");
00177             as=0.39;
00178         }
00179         Move('2', as);
00180         xbee.printf("mortor mode:2 speed:%f",as);
00181 }