智也 大野
目的地へたどり着くアルゴリズム
Dependencies: MPU9250_SPI TA7291P mbed
Revision 0:5fef60d1a47e, committed 2017-03-17
- Comitter:
- tomoya123
- Date:
- Fri Mar 17 08:33:50 2017 +0000
- Commit message:
- ???????????????
Changed in this revision
diff -r 000000000000 -r 5fef60d1a47e HeptaGPS.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HeptaGPS.cpp Fri Mar 17 08:33:50 2017 +0000
@@ -0,0 +1,153 @@
+#include "HeptaGPS.h"
+#include "mbed.h"
+
+HeptaGPS::HeptaGPS(PinName tx, PinName rx) : gps(tx,rx)
+{
+}
+
+void HeptaGPS::flushSerialBuffer(void)
+{
+ while (gps.readable())
+ {
+ gps.getc();
+ }
+ return;
+}
+
+void HeptaGPS::sensing_u16(char* lad,char* log, int *dsize)
+{
+ char gph1[8]={0x00},gph2[8]={0x00},gph3[8]={0x00},gph4[8]={0x00},gpt1[8]={0x00},gpt2[8]={0x00},gpt3[8]={0x00},gpt4[8]={0x00};
+ int i=0,j=0;
+ while (gps.readable()){
+ gps.getc();
+ }
+ loop:
+ while(gps.getc()!='$'){}
+ for(j=0;j<5;j++){
+ gps_data[1][j]=gps.getc();
+ }
+ if((gps_data[1][2]==0x52)&(gps_data[1][3]==0x4d)&(gps_data[1][4]==0x43)){
+ for(j=0;j<1;j++){
+ if(j==0){
+ i=0;
+ while((gps_data[j+1][i+5] = gps.getc()) != '\r'){
+ //pc.putc(gps_data[j+1][i+5]);
+ i++;
+ }
+ gps_data[j+1][i+5]='\0';
+ i=0;
+ //pc.printf("\n\r");
+ }
+ else{
+ while(gps.getc()!='$'){}
+ i=0;
+ while((gps_data[j+1][i] = gps.getc()) != '\r'){
+ //pc.putc(gps_data[j+1][i]);
+ i++;
+ }
+ gps_data[j+1][i]='\0';
+ i=0;
+ //pc.printf("\n\r");
+ }
+ }
+ }
+ else
+ {
+ goto loop;
+ }
+ if( sscanf(gps_data[1],"GPRMC,%f,%c,%f,%c,%f,%c,%f",&time,&statas,&hokui,&ns,&tokei,&ew,&vel) >= 1){
+ //hokui
+ d_hokui=int(hokui/100);
+ m_hokui=(hokui-d_hokui*100);
+ //m_hokui=(hokui-d_hokui*100)/60;
+ g_hokui=d_hokui+(hokui-d_hokui*100)/60;
+ sprintf( gph1, "%02X", (char(d_hokui)) & 0xFF);
+ sprintf( gph2, "%02X", (char(m_hokui)) & 0xFF);
+ sprintf( gph3, "%02X", (char((m_hokui-char(m_hokui))*100)) & 0xFF);
+ sprintf( gph4, "%02X", (char(((m_hokui-char(m_hokui))*100-char((m_hokui-char(m_hokui))*100))*100)) & 0xFF);
+
+ //tokei
+ d_tokei=int(tokei/100);
+ m_tokei=(tokei-d_tokei*100);
+ //m_tokei=(tokei-d_tokei*100)/60;
+ g_tokei=d_tokei+(tokei-d_tokei*100)/60;
+ sprintf( gpt1, "%02X", (char(d_tokei)) & 0xFF);
+ sprintf( gpt2, "%02X", (char(m_tokei)) & 0xFF);
+ sprintf( gpt3, "%02X", (char((m_tokei-char(m_tokei))*100)) & 0xFF);
+ sprintf( gpt4, "%02X", (char(((m_tokei-char(m_tokei))*100-char((m_tokei-char(m_tokei))*100))*100)) & 0xFF);
+ lad[0] = gph1[0];
+ lad[1] = gph1[1];
+ lad[2] = gph2[0];
+ lad[3] = gph2[1];
+ lad[4] = gph3[0];
+ lad[5] = gph3[1];
+ lad[6] = gph4[0];
+ lad[7] = gph4[1];
+ log[0] = gpt1[0];
+ log[1] = gpt1[1];
+ log[2] = gpt2[0];
+ log[3] = gpt2[1];
+ log[4] = gpt3[0];
+ log[5] = gpt3[1];
+ log[6] = gpt4[0];
+ log[7] = gpt4[1];
+ }
+ *dsize = 8;
+}
+
+void HeptaGPS::sensing(float* lat, float* log )
+{
+ int i=0,j=0;
+ while (gps.readable())
+ {
+ gps.getc();
+ }
+ loop:
+ while(gps.getc()!='$'){}
+ for(j=0;j<5;j++)
+ {
+ gps_data[1][j]=gps.getc();
+ }
+ if((gps_data[1][2]==0x52)&(gps_data[1][3]==0x4d)&(gps_data[1][4]==0x43))
+ {
+ for(j=0;j<1;j++){
+ if(j==0){
+ i=0;
+ while((gps_data[j+1][i+5] = gps.getc()) != '\r'){
+ //pc.putc(gps_data[j+1][i+5]);
+ i++;
+ }
+ gps_data[j+1][i+5]='\0';
+ i=0;
+ //pc.printf("\n\r");
+ }
+ else{
+ while(gps.getc()!='$'){}
+ i=0;
+ while((gps_data[j+1][i] = gps.getc()) != '\r'){
+ //pc.putc(gps_data[j+1][i]);
+ i++;
+ }
+ gps_data[j+1][i]='\0';
+ i=0;
+ //pc.printf("\n\r");
+ }
+ }
+ }
+ else{
+ goto loop;
+ }
+ if( sscanf(gps_data[1],"GPRMC,%f,%c,%f,%c,%f,%c,%f",&time,&statas,&hokui,&ns,&tokei,&ew,&vel) >= 1){
+ //hokui
+ d_hokui=int(hokui/100);
+ m_hokui=(hokui-d_hokui*100);
+ //m_hokui=(hokui-d_hokui*100)/60;
+ *lat=d_hokui+(hokui-d_hokui*100)/60;
+ //tokei
+ d_tokei=int(tokei/100);
+ m_tokei=(tokei-d_tokei*100);
+ //m_tokei=(tokei-d_tokei*100)/60;
+ *log = d_tokei+(tokei-d_tokei*100)/60;
+ //pc.printf("Lat:%4.6f,Log:%4.6f\n\r",g_hokui,g_tokei);
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r 5fef60d1a47e HeptaGPS.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HeptaGPS.h Fri Mar 17 08:33:50 2017 +0000
@@ -0,0 +1,27 @@
+#ifndef MBED_HEPTAGPS_H
+#define MBED_HEPTAGPS_H
+#include "mbed.h"
+
+//GPS Sensor GT723F
+
+class HeptaGPS{
+public:
+ Serial gps;
+ HeptaGPS(
+ PinName tx,
+ PinName rx
+ );
+ void flushSerialBuffer(void);
+ void sensing(float* lat, float* log );
+ void sensing_u16(char* lad,char* log, int *dsize);
+private:
+ int i,rlock,stn;
+ char gps_data[7][1000];
+ char ns,ew,statas;
+ float time,hokui,tokei,vel;
+ float g_hokui,g_tokei;
+ float d_hokui,m_hokui,d_tokei,m_tokei;
+ int h_time,m_time,s_time;
+ //FILE *fp;
+};
+#endif
\ No newline at end of file
diff -r 000000000000 -r 5fef60d1a47e MPU9250.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU9250.lib Fri Mar 17 08:33:50 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/kylongmu/code/MPU9250_SPI/#084e8ba240c1
diff -r 000000000000 -r 5fef60d1a47e TA7291P.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TA7291P.lib Fri Mar 17 08:33:50 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/takoyaki/code/TA7291P/#2abc17614090
diff -r 000000000000 -r 5fef60d1a47e main.cpp
--- /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
diff -r 000000000000 -r 5fef60d1a47e mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Mar 17 08:33:50 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/25aea2a3f4e3 \ No newline at end of file