Function.h

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