main.cpp

00001 #include "mbed.h"
00002 #include "BM1422AGMV.h"
00003 #include "KX224.h"
00004 //#include "wire.h"
00005 
00006 DigitalOut myled(LED1);
00007 
00008 //https://www.google.com/search?q=bm1422agmv+arduino&rlz=1C1SQJL_enTH863TH863&oq=BM1422&aqs=chrome.5.69i57j0i19j69i59l2j0i19l2j69i60l2.3962j0j7&sourceid=chrome&ie=UTF-8#imgrc=llWSwJWhYP3wyM
00009 
00010 I2C i2c(I2C_SDA, I2C_SCL);         // setup i2c (SDA,SCL)  
00011 
00012 BM1422AGMV bm1422(I2C_SDA, I2C_SCL);
00013 KX224 accelerometer(I2C_SDA, I2C_SCL);
00014 
00015 //BM1422AGMV bm1422(i2c,0x0E);
00016 Serial pc(USBTX, USBRX);
00017 
00018 int _px  = 6.3;//-44.5; //中点x
00019 int _py  = -24.8;//-38.5; //中点y
00020 
00021 /** 角度を求める **/
00022 double getDirection(double x, double y){
00023   double dir  = 0;
00024   dir = atan((y - _py)/(x - _px));
00025   return dir;
00026 }
00027 
00028 int main() {
00029     //bm1422.initialize();
00030     pc.baud(115200);                              // baud rate: 9600
00031     pc.printf("test\n");
00032   float mag[3];
00033   float acc[3];
00034   int ret;
00035 
00036   double rd;
00037   int x = mag[0];
00038   int y = mag[1];
00039   int z = mag[2];
00040   
00041     //Initialize KX224-1053
00042     ret = accelerometer.initialize();
00043     if (ret != 0) {
00044         printf("\r\nKX224-1053  :   Failed initialization \n");
00045     }
00046         
00047     while(1) {
00048       accelerometer.get_val(&acc[0]);
00049              bm1422.get_val(mag);
00050       rd = getDirection(mag[0],mag[1]);
00051       x = mag[0];
00052       y = mag[1];
00053       z = mag[2];
00054       
00055       printf("%7.2f       %7.2f       %7.2f       %7.2f       %7.2f       %7.2f\r\n", mag[0], mag[1], mag[2],acc[0], acc[1], acc[2]);
00056 
00057   //磁北ではないときステッピングモーターを回転させる
00058   //ステッピングモーターの回転数に応じてrdに余裕を持たせています
00059   if(rd <= 0.3 && rd >= -0.3 && x > _px){
00060     //磁北の状態//Serial.println("North!");//https://deviceplus.jp/arduino/entry041/
00061   pc.printf("%f     %d      %d      %d      North!  \n",rd,x,y,z);
00062   myled = 1;
00063 
00064   }else{
00065   pc.printf("%f     %d      %d      %d       -      \n",rd,x,y,z);
00066   myled = 0;
00067   }
00068 /*
00069   else if(rd > 0){
00070     moveStepper(2,-1);
00071   }
00072   else{
00073     moveStepper(2);
00074   }  
00075 */  
00076   
00077         //myled = 1;
00078         //wait(.1);
00079         //myled = 0;
00080         //wait(.1);
00081     }
00082 }