智也 大野 / Mbed 2 deprecated ANCHOR_Navigation3

目的地へたどり着くアルゴリズム

Dependencies:   MPU9250_SPI TA7291P mbed

main.cpp@0:5fef60d1a47e, 2017-03-17 (annotated)

Committer:
tomoya123
Date:
Fri Mar 17 08:33:50 2017 +0000
Revision:
0:5fef60d1a47e
???????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tomoya123 0:5fef60d1a47e 1 #include "mbed.h"
tomoya123 0:5fef60d1a47e 2 #include "HeptaGPS.h"
tomoya123 0:5fef60d1a47e 3 #include "MPU9250.h"
tomoya123 0:5fef60d1a47e 4 #include "ta7291p.h"
tomoya123 0:5fef60d1a47e 5
tomoya123 0:5fef60d1a47e 6 #define MORTOR_LOW_SPEED 0.6
tomoya123 0:5fef60d1a47e 7 #define MORTOR_HIGH_SPEED 1.0
tomoya123 0:5fef60d1a47e 8
tomoya123 0:5fef60d1a47e 9 #define LATITUDE 35.72593
tomoya123 0:5fef60d1a47e 10 #define LONGITUDE 140.05737
tomoya123 0:5fef60d1a47e 11
tomoya123 0:5fef60d1a47e 12 #define MAGNET_CALIB_X 3.227
tomoya123 0:5fef60d1a47e 13 #define MAGNET_CALIB_Y -14.7
tomoya123 0:5fef60d1a47e 14
tomoya123 0:5fef60d1a47e 15 #define EQUATORIAL_RADIUS 6738137
tomoya123 0:5fef60d1a47e 16
tomoya123 0:5fef60d1a47e 17 Serial pc(USBTX, USBRX);//(tx,rx)
tomoya123 0:5fef60d1a47e 18 Serial xbee(p9,p10);
tomoya123 0:5fef60d1a47e 19 SPI spi(p5, p6, p7);
tomoya123 0:5fef60d1a47e 20 mpu9250_spi imu(spi,p8);
tomoya123 0:5fef60d1a47e 21 HeptaGPS gps(p13,p14);//(tx,rx)
tomoya123 0:5fef60d1a47e 22 ta7291p mortor1(p25,p24,p26);
tomoya123 0:5fef60d1a47e 23 ta7291p mortor2(p22,p21,p23);
tomoya123 0:5fef60d1a47e 24
tomoya123 0:5fef60d1a47e 25 int flag;
tomoya123 0:5fef60d1a47e 26 void Cmd_rx(){
tomoya123 0:5fef60d1a47e 27 char cmd;
tomoya123 0:5fef60d1a47e 28
tomoya123 0:5fef60d1a47e 29 xbee.printf("interrupt!!\r");
tomoya123 0:5fef60d1a47e 30 cmd = xbee.getc();
tomoya123 0:5fef60d1a47e 31
tomoya123 0:5fef60d1a47e 32 if(cmd == 's'){
tomoya123 0:5fef60d1a47e 33 flag = 1;
tomoya123 0:5fef60d1a47e 34 }
tomoya123 0:5fef60d1a47e 35 else if(cmd == 'q'){
tomoya123 0:5fef60d1a47e 36 flag = 2;
tomoya123 0:5fef60d1a47e 37 }
tomoya123 0:5fef60d1a47e 38 }//割り込み関数
tomoya123 0:5fef60d1a47e 39
tomoya123 0:5fef60d1a47e 40 int Azimuthcal(float lat1,float log1){
tomoya123 0:5fef60d1a47e 41 float dlog,dlat,dx,dy,azimuth;
tomoya123 0:5fef60d1a47e 42 dlog=(LONGITUDE - log1)*3.1415/180;
tomoya123 0:5fef60d1a47e 43 dlat=(LATITUDE - lat1)*3.1415/180;
tomoya123 0:5fef60d1a47e 44 dx=EQUATORIAL_RADIUS*dlog*cos(lat1*3.1415/180);
tomoya123 0:5fef60d1a47e 45 dy=EQUATORIAL_RADIUS*dlat;
tomoya123 0:5fef60d1a47e 46 azimuth = atan2(dx,dy)*180/3.1415;
tomoya123 0:5fef60d1a47e 47 return azimuth;
tomoya123 0:5fef60d1a47e 48 }//GPSからの方位角
tomoya123 0:5fef60d1a47e 49
tomoya123 0:5fef60d1a47e 50 int readCompas(float mx,float my){
tomoya123 0:5fef60d1a47e 51 float MagBear,ma,mb;
tomoya123 0:5fef60d1a47e 52 imu.read_all();
tomoya123 0:5fef60d1a47e 53 ma = (mx - MAGNET_CALIB_X)*cos(7*3.1415/180);
tomoya123 0:5fef60d1a47e 54 mb = (my - MAGNET_CALIB_Y)*cos(7*3.1415/180);
tomoya123 0:5fef60d1a47e 55 MagBear = atan2(mb,ma)*180/3.1415;
tomoya123 0:5fef60d1a47e 56 return MagBear;
tomoya123 0:5fef60d1a47e 57 } //地磁気センサーからの磁方位角
tomoya123 0:5fef60d1a47e 58
tomoya123 0:5fef60d1a47e 59 void setup_MPU9250(void){
tomoya123 0:5fef60d1a47e 60 if(imu.init(1,BITS_DLPF_CFG_188HZ)){ //INIT the mpu9250
tomoya123 0:5fef60d1a47e 61 printf("\nCouldn't initialize MPU9250 via SPI!");
tomoya123 0:5fef60d1a47e 62 }
tomoya123 0:5fef60d1a47e 63 imu.whoami(); //output the I2C address to know if SPI is working, it should be 104
tomoya123 0:5fef60d1a47e 64 wait(1);
tomoya123 0:5fef60d1a47e 65 imu.set_gyro_scale(BITS_FS_2000DPS); //Set full scale range for gyros
tomoya123 0:5fef60d1a47e 66 wait(1);
tomoya123 0:5fef60d1a47e 67 imu.set_acc_scale(BITS_FS_16G); //Set full scale range for accs
tomoya123 0:5fef60d1a47e 68 wait(1);
tomoya123 0:5fef60d1a47e 69 imu.AK8963_whoami();
tomoya123 0:5fef60d1a47e 70 wait(0.1);
tomoya123 0:5fef60d1a47e 71 imu.AK8963_calib_Magnetometer();
tomoya123 0:5fef60d1a47e 72 imu.calib_acc();
tomoya123 0:5fef60d1a47e 73 }//9軸センサーの前処理
tomoya123 0:5fef60d1a47e 74
tomoya123 0:5fef60d1a47e 75
tomoya123 0:5fef60d1a47e 76 void straight(void){
tomoya123 0:5fef60d1a47e 77 mortor1.rotf(MORTOR_HIGH_SPEED);
tomoya123 0:5fef60d1a47e 78 mortor2.rotf(MORTOR_HIGH_SPEED);
tomoya123 0:5fef60d1a47e 79 }
tomoya123 0:5fef60d1a47e 80
tomoya123 0:5fef60d1a47e 81 void turn_left(void){
tomoya123 0:5fef60d1a47e 82 mortor1.rotf(MORTOR_LOW_SPEED);
tomoya123 0:5fef60d1a47e 83 mortor2.rotf(MORTOR_HIGH_SPEED);
tomoya123 0:5fef60d1a47e 84 }
tomoya123 0:5fef60d1a47e 85
tomoya123 0:5fef60d1a47e 86 void turn_right(void){
tomoya123 0:5fef60d1a47e 87 mortor1.rotf(MORTOR_HIGH_SPEED);
tomoya123 0:5fef60d1a47e 88 mortor2.rotf(MORTOR_LOW_SPEED);
tomoya123 0:5fef60d1a47e 89 }
tomoya123 0:5fef60d1a47e 90
tomoya123 0:5fef60d1a47e 91 void stop_mortor(void){
tomoya123 0:5fef60d1a47e 92 mortor1.rotstop();
tomoya123 0:5fef60d1a47e 93 mortor2.rotstop();
tomoya123 0:5fef60d1a47e 94 }
tomoya123 0:5fef60d1a47e 95
tomoya123 0:5fef60d1a47e 96 int calc_distance(float lat1_s,float log1_s ){
tomoya123 0:5fef60d1a47e 97 float x1=(LONGITUDE - log1_s)*3.1415/180;
tomoya123 0:5fef60d1a47e 98 float y1=(LATITUDE - lat1_s)*3.1415/180;
tomoya123 0:5fef60d1a47e 99 float dx1=EQUATORIAL_RADIUS*x1*cos(lat1_s*3.1415/180);
tomoya123 0:5fef60d1a47e 100 float dy1=EQUATORIAL_RADIUS*y1;
tomoya123 0:5fef60d1a47e 101 float distance = sqrt(dx1*dx1 + dy1*dy1);
tomoya123 0:5fef60d1a47e 102
tomoya123 0:5fef60d1a47e 103 if((distance > 0)&&(distance < 1)){
tomoya123 0:5fef60d1a47e 104 return flag = 2;
tomoya123 0:5fef60d1a47e 105 }
tomoya123 0:5fef60d1a47e 106 else{
tomoya123 0:5fef60d1a47e 107 return flag =1;
tomoya123 0:5fef60d1a47e 108 }
tomoya123 0:5fef60d1a47e 109 }//距離計算
tomoya123 0:5fef60d1a47e 110 void setup_GPS(float lat2,float log2){
tomoya123 0:5fef60d1a47e 111 xbee.printf("lat %2.5f,log %2.5f\r",lat2,log2);
tomoya123 0:5fef60d1a47e 112 }
tomoya123 0:5fef60d1a47e 113
tomoya123 0:5fef60d1a47e 114
tomoya123 0:5fef60d1a47e 115 int main() {
tomoya123 0:5fef60d1a47e 116 float lat,log;
tomoya123 0:5fef60d1a47e 117 float magx,magy;
tomoya123 0:5fef60d1a47e 118 int Az,Com,attitudeAngle;
tomoya123 0:5fef60d1a47e 119
tomoya123 0:5fef60d1a47e 120 setup_MPU9250();
tomoya123 0:5fef60d1a47e 121 xbee.attach(Cmd_rx,Serial::RxIrq);
tomoya123 0:5fef60d1a47e 122 while(1){
tomoya123 0:5fef60d1a47e 123 gps.sensing(&lat,&log);
tomoya123 0:5fef60d1a47e 124 setup_GPS(lat,log);
tomoya123 0:5fef60d1a47e 125 if(flag == 1){
tomoya123 0:5fef60d1a47e 126 break;
tomoya123 0:5fef60d1a47e 127 }
tomoya123 0:5fef60d1a47e 128 }
tomoya123 0:5fef60d1a47e 129 while(1){
tomoya123 0:5fef60d1a47e 130 gps.sensing(&lat,&log);
tomoya123 0:5fef60d1a47e 131 xbee.printf("lat=%2.5f,log=%2.5f\r\n",lat,log);
tomoya123 0:5fef60d1a47e 132 Az = Azimuthcal(lat,log);
tomoya123 0:5fef60d1a47e 133 calc_distance(lat,log);
tomoya123 0:5fef60d1a47e 134
tomoya123 0:5fef60d1a47e 135 magx = imu.Magnetometer[0];
tomoya123 0:5fef60d1a47e 136 magy = imu.Magnetometer[1];
tomoya123 0:5fef60d1a47e 137 Com = readCompas(magx,magy);
tomoya123 0:5fef60d1a47e 138
tomoya123 0:5fef60d1a47e 139 attitudeAngle = Az + Com;
tomoya123 0:5fef60d1a47e 140
tomoya123 0:5fef60d1a47e 141 if(flag==1){
tomoya123 0:5fef60d1a47e 142 if((attitudeAngle >= -30 )&&(attitudeAngle <= 30)){
tomoya123 0:5fef60d1a47e 143 straight();
tomoya123 0:5fef60d1a47e 144 }
tomoya123 0:5fef60d1a47e 145 else if(attitudeAngle > 30){
tomoya123 0:5fef60d1a47e 146 turn_right();
tomoya123 0:5fef60d1a47e 147 }else{
tomoya123 0:5fef60d1a47e 148 turn_left();
tomoya123 0:5fef60d1a47e 149 }
tomoya123 0:5fef60d1a47e 150 }
tomoya123 0:5fef60d1a47e 151 else if(flag == 2){
tomoya123 0:5fef60d1a47e 152 xbee.printf("flag=2\r");
tomoya123 0:5fef60d1a47e 153 stop_mortor();
tomoya123 0:5fef60d1a47e 154 }
tomoya123 0:5fef60d1a47e 155 else{
tomoya123 0:5fef60d1a47e 156 xbee.printf("stop\r");
tomoya123 0:5fef60d1a47e 157 stop_mortor();
tomoya123 0:5fef60d1a47e 158 }
tomoya123 0:5fef60d1a47e 159 }
tomoya123 0:5fef60d1a47e 160
tomoya123 0:5fef60d1a47e 161 }
tomoya123 0:5fef60d1a47e 162
tomoya123 0:5fef60d1a47e 163
tomoya123 0:5fef60d1a47e 164