Diff: main.cpp

Revision:

0:5fef60d1a47e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Mar 17 08:33:50 2017 +0000
@@ -0,0 +1,164 @@
+#include "mbed.h"
+#include "HeptaGPS.h"
+#include "MPU9250.h"
+#include "ta7291p.h"
+
+#define MORTOR_LOW_SPEED 0.6
+#define MORTOR_HIGH_SPEED 1.0
+
+#define LATITUDE 35.72593
+#define LONGITUDE 140.05737
+
+#define MAGNET_CALIB_X 3.227
+#define MAGNET_CALIB_Y -14.7
+
+#define EQUATORIAL_RADIUS 6738137
+ 
+Serial pc(USBTX, USBRX);//(tx,rx)
+Serial xbee(p9,p10);
+SPI spi(p5, p6, p7);
+mpu9250_spi imu(spi,p8);  
+HeptaGPS gps(p13,p14);//(tx,rx)
+ta7291p mortor1(p25,p24,p26);
+ta7291p mortor2(p22,p21,p23);
+
+int flag;
+void Cmd_rx(){     
+     char cmd;
+     
+     xbee.printf("interrupt!!\r"); 
+     cmd = xbee.getc();
+     
+     if(cmd == 's'){
+         flag = 1;
+     }
+     else if(cmd == 'q'){
+        flag = 2;
+     }
+}//割り込み関数
+    
+int Azimuthcal(float lat1,float log1){
+    float dlog,dlat,dx,dy,azimuth;
+       dlog=(LONGITUDE - log1)*3.1415/180;
+       dlat=(LATITUDE - lat1)*3.1415/180;
+       dx=EQUATORIAL_RADIUS*dlog*cos(lat1*3.1415/180);
+       dy=EQUATORIAL_RADIUS*dlat;
+       azimuth = atan2(dx,dy)*180/3.1415;
+            return azimuth;                                                          
+     }//GPSからの方位角
+     
+int readCompas(float mx,float my){
+    float MagBear,ma,mb;
+    imu.read_all();
+    ma = (mx - MAGNET_CALIB_X)*cos(7*3.1415/180);
+    mb = (my - MAGNET_CALIB_Y)*cos(7*3.1415/180);
+    MagBear = atan2(mb,ma)*180/3.1415;
+    return MagBear;
+    } //地磁気センサーからの磁方位角    
+
+void setup_MPU9250(void){
+        if(imu.init(1,BITS_DLPF_CFG_188HZ)){  //INIT the mpu9250
+        printf("\nCouldn't initialize MPU9250 via SPI!");
+    }    
+    imu.whoami(); //output the I2C address to know if SPI is working, it should be 104
+    wait(1);    
+    imu.set_gyro_scale(BITS_FS_2000DPS);    //Set full scale range for gyros
+    wait(1);  
+    imu.set_acc_scale(BITS_FS_16G);          //Set full scale range for accs
+    wait(1);
+    imu.AK8963_whoami();
+    wait(0.1);  
+    imu.AK8963_calib_Magnetometer();
+    imu.calib_acc();
+      }//9軸センサーの前処理
+
+
+void straight(void){
+      mortor1.rotf(MORTOR_HIGH_SPEED);
+      mortor2.rotf(MORTOR_HIGH_SPEED);
+      }
+
+void turn_left(void){
+      mortor1.rotf(MORTOR_LOW_SPEED);
+      mortor2.rotf(MORTOR_HIGH_SPEED);
+      }
+      
+void turn_right(void){
+      mortor1.rotf(MORTOR_HIGH_SPEED);
+      mortor2.rotf(MORTOR_LOW_SPEED);
+      }
+
+void stop_mortor(void){
+     mortor1.rotstop();
+     mortor2.rotstop();
+     }
+     
+int calc_distance(float lat1_s,float log1_s ){
+    float x1=(LONGITUDE - log1_s)*3.1415/180;
+    float y1=(LATITUDE - lat1_s)*3.1415/180;
+    float dx1=EQUATORIAL_RADIUS*x1*cos(lat1_s*3.1415/180);
+    float dy1=EQUATORIAL_RADIUS*y1;
+    float distance = sqrt(dx1*dx1 + dy1*dy1);
+    
+    if((distance > 0)&&(distance < 1)){
+       return flag = 2;
+        }
+        else{
+           return flag =1;
+        }
+    }//距離計算  
+void setup_GPS(float lat2,float log2){
+    xbee.printf("lat %2.5f,log %2.5f\r",lat2,log2);
+    }
+      
+                        
+int main() {
+    float lat,log;
+    float magx,magy;
+    int Az,Com,attitudeAngle;
+    
+    setup_MPU9250();
+    xbee.attach(Cmd_rx,Serial::RxIrq); 
+    while(1){
+        gps.sensing(&lat,&log);
+        setup_GPS(lat,log);
+        if(flag == 1){
+            break;
+            }
+        }
+   while(1){ 
+             gps.sensing(&lat,&log);
+             xbee.printf("lat=%2.5f,log=%2.5f\r\n",lat,log);
+             Az = Azimuthcal(lat,log);
+             calc_distance(lat,log);   
+             
+             magx = imu.Magnetometer[0];
+             magy = imu.Magnetometer[1];
+             Com = readCompas(magx,magy);
+             
+             attitudeAngle = Az + Com;
+         
+             if(flag==1){          
+                    if((attitudeAngle >= -30 )&&(attitudeAngle <= 30)){
+                        straight();
+                    }
+                    else if(attitudeAngle > 30){
+                        turn_right();
+                    }else{
+                        turn_left();
+                        }
+            }
+            else if(flag == 2){
+                xbee.printf("flag=2\r");
+                stop_mortor();
+            }
+                    else{
+                        xbee.printf("stop\r");
+                        stop_mortor();
+                        }   
+       }         
+                                                             
+       }
+      
+       
+   
\ No newline at end of file