CanSat2018_4にGPSとtweliteをつけ足して関数化したもの
Dependencies: mbed
Diff: main.cpp
- Revision:
- 0:704adb172e40
- Child:
- 1:342818b81499
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Aug 10 18:18:00 2018 +0000 @@ -0,0 +1,308 @@ +/*MPU9250_PROGRAM ver2.1 PROGRAMED BY RYOTARO FUNAI 2018/08/10*/ +#include <mbed.h> +#include <math.h> +#define PI 3.14159265359 + +DigitalOut motor1(p21); +DigitalOut motor2(p22); +DigitalOut motor3(p23); +DigitalOut motor4(p24); +I2C i2c(p9, p10); +Serial pc(USBTX, USBRX); //TWELITE使う予定なら13,14ピン +Serial gps(p28,p27); // tx, rx +Serial twelite(p13,p14); //twelite.printfでtwilite経由で通信できる +DigitalOut led1(p12); +AnalogIn Lx_in(p15); + +//GPS用変数群 +int i,rlock,mode; +char gps_data[256]; +char ns,ew; +float x=135.585800, y=34.648525; //実験用初期位地 +float w_time,hokui,tokei; +float g_hokui,g_tokei,old_hokui, old_tokei; //oldとg_を比べることで距離と角度を出す +float d_hokui,m_hokui,d_tokei,m_tokei; +unsigned char c; + +#define MPU9250_ADDRESS 0x68<<1 //I2CでのMPU9250のスレーブアドレス +#define AK8963_ADDRESS 0x0c<<1 //磁気センサのスレーブアドレス +#define Whoami 0x75 //who_am_iレジスタのアドレス、0x71が返ってくる +#define PWR 0x6b //スリープモードをonにするためのアドレス +#define MAG_OPN 0x37 //mpu9250から磁気センサにアクセスできるようにする +#define ACC_CONFIG 0x1c //加速度センサ設定用のアドレス +#define ACC_2G 0x00 //加速度センサのレンジ(2G) +#define ACC_4G 0x08 //加速度センサのレンジ(4G) +#define ACC_8G 0x10 //加速度センサのレンジ(8G) +#define ACC_16G 0x18 //加速度センサのレンジ(16Gまで計測可能) +#define MAG_CONFIG 0x0a //磁気センサ設定用のアドレス +#define MAG_8HZ 0x12 //磁気センサの出力周期(8Hz) +#define MAG_100HZ 0x16 //磁気センサの出力周期(100Hz) +#define accRange 16.0 //加速度センサの測定レンジ +#define ST2 0x02 //磁気センサのステータスが入っているアドレス +#define Ain 35 +#define SDA 21 +#define SCL 22 +#define led 2 //チェック用のLEDピン + +//void LCD_Reset(); //m5stackLCDを更新 +float Lx_Read(); //cdsセルから値を取得 +void Ac_Read(int16_t*, int16_t*, int16_t*); //9軸から加速度の値を取得 +void Mag_Read(int16_t*, int16_t*, int16_t*); +//addrにスレーブアドレス、regにアクセスするアドレスを入力する +void i2cRead(uint8_t addr,uint8_t reg, uint8_t bytes,uint8_t* data); +void i2cWrite(uint8_t addr,uint8_t reg, uint8_t data); +uint8_t IDcheck(); +void Breky(); +void Turn(); +void Return(); +void Right(); +void Left(); +void release(); +void Faling(); +void nikurom(); +void getGPS(); +void distance(); +void geoDirection(); +void compass(); + +uint8_t accgyrodata[14]; +uint8_t magneticdata[7]; +uint8_t ST2_Bit; //磁気センサのステータスを入れておく + +int main() { + gps.baud(9600); + twelite.baud(115200); + void Release(); //放出判定 + gps.attach(getGPS,Serial::RxIrq); //GPS割り込み + void Faling();//落下判定 + void nikurom(); //ニクロム線を切る + void compass();//九軸で進む方向決める + +} + + +/****************関数群****************/ +//cdsセルからアナログ値を持ってくる +float Lx_Read(){ + float lx; + lx = Lx_in.read(); + return lx; +} + +//Who_am_Iアドレスで接続確認ができる。0x71もしくは10進数で113が返ってくればok +uint8_t IDcheck(){ + uint8_t address; + i2cRead(MPU9250_ADDRESS, Whoami, 1, &address); + return address; +} + +//mpu9250から加速度センサのみ引っ張ってくる +void Ac_Read(int16_t* ax, int16_t* ay, int16_t* az){ + i2cWrite(MPU9250_ADDRESS, PWR, 0x00); //スリープモードの解除 + i2cWrite(MPU9250_ADDRESS, ACC_CONFIG, ACC_16G);//加速度センサの測定レンジの設定 + i2cRead(MPU9250_ADDRESS, 0x3b, 14, accgyrodata); + *ax = (accgyrodata[0] << 8) | accgyrodata[1];//accGyroTempData[0]を左に8シフトし(<<),accGyroTempData[1]を足し合わせる(|) |は論理和 + *ay = (accgyrodata[2] << 8) | accgyrodata[3];//accGyroTempData[2]を左に8シフトし(<<),accGyroTempData[3]を足し合わせる(|) + *az = (accgyrodata[4] << 8) | accgyrodata[5];//accGyroTempData[4]を左に8シフトし(<<),accGyroTempData[5]を足し合わせる(|) +} + +//mpu9250から磁気センサのみ引っ張ってくる +void Mag_Read(int16_t* mx, int16_t* my, int16_t* mz){ + i2cWrite(MPU9250_ADDRESS, PWR, 0x00); //スリープモードの解除 + i2cWrite(MPU9250_ADDRESS, MAG_OPN, 0x02); //磁気センサの起動 + i2cWrite(AK8963_ADDRESS, MAG_CONFIG, MAG_100HZ);//磁気センサの測定レンジの設定 + i2cRead(AK8963_ADDRESS, ST2, 1, &ST2_Bit);//読み出し準備ができたか確認 + if(ST2_Bit & 0x01){ //ちゃんと読めたかをST2レジスタの値を読んで確認 + i2cRead(AK8963_ADDRESS, 0x03, 7, magneticdata); + } + else pc.printf("ERROR!!\n"); + *mx = (magneticdata[0] << 8) | magneticdata[1]; + *my = (magneticdata[2] << 8) | magneticdata[3]; + *mz = (magneticdata[4] << 8) | magneticdata[5]; +} + +//mpu9250からデータを取得(bytesに受け取るデータのバイト数、dataに実際のデータを挿入していく) +void i2cRead(uint8_t addr,uint8_t reg, uint8_t bytes,uint8_t* data){ + char cmd[1]; + char written_data[14]; + cmd[0] = reg; + i2c.write(addr, cmd, 1, 1); + i2c.read(addr, written_data, bytes, 0); + for(int ii = 0; ii < bytes; ii++) { + data[ii] = written_data[ii]; + } +} + +//mpu9250にデータを送信(dataに送信するデータを入力する) +void i2cWrite(uint8_t addr,uint8_t reg, uint8_t data){ + char cmd[2]; + cmd[0] = reg; //レジスタ指定 + cmd[1] = data; //送信するデータ + i2c.write(addr, cmd, 2); //レジスタ指定はどうする? +} + +//各種モータの関数 +void Breky(){ + motor1 = 0; + motor2 = 0; + motor3 = 0; + motor4 = 0; +} + +void Turn(){ + motor1 = 1; + motor2 = 0; + motor3 = 1; + motor4 = 0; +} + +void Return(){ + motor1 = 0; + motor2 = 1; + motor3 = 0; + motor4 = 1; +} + +void Left(){ + motor1 = 1; + motor2 = 0; + motor3 = 0; + motor4 = 1; +} + +void Right(){ + motor1 = 0; + motor2 = 1; + motor3 = 1; + motor4 = 0; +} + +void release(){ + while(1){ + //cdsセルの値の確認 + //暗いときは0.8~1.0程度の値 + float blight; + blight = Lx_Read(); + pc.printf("%4.1f\n\r",blight); + if(blight<=0.6) break;// 明るさが0.6以下なら次のフェーズに進む + } +} + +void Faling(){ + while(1){ + int16_t ax, ay, az; + float accX, accY, accZ, acc; + //加速度の値を取得し、落下判定 + Ac_Read(&ax,&ay,&az); + accX = ax * accRange / 32768.0;//[G]に変換 + accY = ay * accRange / 32768.0;//[G]に変換 + accZ = az * accRange / 32768.0;//[G]に変換 + acc = sqrt((accX * accX) + (accY * accY) + (accZ * accZ)); + // 各軸のGを表示 + pc.printf("accX :%f\n\raccY :%f\n\raccZ :%f\n\racc :%f\n\r",accX, accY, accZ, acc); + //静止時は1.0程度、落下時はそれを超える為、電源を入れるのは1.1以下にした + if(acc <= 1.10){ + pc.printf("PWRON!!"); + Turn(); + break; + //led1 = 0; + }else{ + Breky(); + //led1 = 0; + } + } +} + +void nikurom(){ + wait(60); +} + +void getGPS() { + c = gps.getc(); + if( c=='$' || i == 256){ + mode = 0; + i = 0; + for(int j=0; j<256; j++){ + gps_data[j]=NULL; + } + } + + if(mode==0){ + if((gps_data[i]=c) != '\r'){ + i++; + }else{ + gps_data[i]='\0'; + + if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock) >= 1){ + if(rlock==1){ + // pc.printf("Status:Lock(%d)\n\r",rlock); + //logitude + d_tokei= int(tokei/100); + m_tokei= (tokei-d_tokei*100)/60; + g_tokei= d_tokei+m_tokei; + //Latitude + d_hokui=int(hokui/100); + m_hokui=(hokui-d_hokui*100)/60; + g_hokui=d_hokui+m_hokui; + //現在地のgpsデータ + pc.printf("Lat:%.6f, Lon:%.6f\n\r",g_hokui, g_tokei); + twelite.printf("%.6f,%.6f\n\r",g_hokui, g_tokei); + //前回の計測からの変化量 + //pc.printf("ch_Lat:%.6f, ch_Lon:%.6f\n\r",old_hokui-g_hokui, old_tokei-g_tokei); + //pc.printf("dis : %fm\n\r", 1000*6378.137*acos(sin(y*PI/180)*sin(g_hokui*PI/180)+cos(y*PI/180)*cos(g_hokui*PI/180)*cos(x*PI/180-g_tokei*PI/180))); //別の距離の出し方 + distance(); + geoDirection(); + old_hokui=g_hokui; + old_tokei=g_tokei; + }else{//gpsデータ未受信時 + //pc.printf("\n\rStatus:unLock(%d)\n\r",rlock); + ////pc.printf("%s",gps_data); + } + } + } + } +} + + //ヒューベニの公式で距離出す +void distance(){ + float rlat1 = y*PI/180, rlon1 = x*PI/180; + float rlat2 = g_hokui*PI/180, rlon2 = g_tokei*PI/180; + float a=6378137.000, a2=pow(a,2), e=sqrt((a2-pow(6356752.314,2))/a2); + float lat_diff = rlat1-rlat2; + float lon_diff = rlon1-rlon2; + float lat_ave = (rlat1+rlat2)/2; + float w = sqrt(1-pow(e,2)*pow(sin(lat_ave),2)); + float meridian = a*(1-pow(e,2))/pow(w,3); + float n=a/w; + float distance2 = sqrt(pow(lat_diff*meridian,2)+pow(lon_diff*n*cos(lat_ave),2)); + //pc.printf("dis2 : %fm\n\r",distance2); +} + +//角度出す +void geoDirection() { + // 緯度 g_hokui(lat1),経度 g_tokei(lng1) の点を出発として、緯度 y(lat2),経度 x(lng2) への方位 + // 北を0度で右回りの角度0~360度 + float Y = cos(x*PI/180)*sin(y*PI/180-g_hokui*PI/180); + float X = cos(g_tokei*PI/180)*sin(x*PI/180)-sin(g_tokei*PI/180)*cos(x*PI/180)*cos(y*PI/180-g_hokui*PI/180); + float dirE0 = 180*atan2(Y, X)/PI; + if(dirE0 < 0){ + dirE0 = dirE0 + 360; + } + float dirN0 = (int)(dirE0 + 90) % 360; + pc.printf("%f\n\r",dirN0); +} + +//方角だす +void compass(){ + int16_t mx, my, mz; + float magX, magY, magZ, mag; + int theta; + //磁気の値を取得し、方位判定 + Mag_Read(&mx, &my, &mz); + magX = (mx + 340.0f) / 32768.0f * 4800.0f;//[uT]に変換 + magY = (my - 234.0f) / 32768.0f * 4800.0f;//[uT]に変換 + magZ = mz / 32768.0f * 4800.0f;//[uT]に変換 + theta = (int)(180.0 * atan2(magY, magX) / 3.14) + 180; + // 角度の表示 + pc.printf("%d\n\r", theta); +} \ No newline at end of file