Revision 9:3f585b1f304d, committed 2017-02-28

Comitter:

Nike3221

Date:

Tue Feb 28 17:43:35 2017 +0000

Parent:

8:c8512336fa70

Commit message:

mag3110
;

Changed in this revision

--- a/main.cpp	Mon Jan 16 14:23:38 2017 +0000
+++ b/main.cpp	Tue Feb 28 17:43:35 2017 +0000
@@ -7,6 +7,7 @@
 #include "mbed.h"
 #include "MotionSensor.h"
 #include "MAG3110.h"
+#include "math.h"
 
 Serial pc(USBTX,USBRX);
 int16_t x,y,z;
@@ -14,14 +15,22 @@
 MAG3110 mag(p28,p27);
 DigitalIn local_on(p20);
 DigitalIn local_del(p19);
+DigitalIn north(p21);
+DigitalIn east(p22);
+DigitalIn south(p23);
+DigitalIn west(p24);
+DigitalIn cds(p29);
 DigitalOut bit1(LED4);
 DigitalOut bit2(LED3);
 DigitalOut bit3(LED2);
 DigitalOut bit4(LED1);
 
 int offset_x,offset_y;
-int x_max,y_max,x_min,y_min;
+float x_max,y_max,x_min,y_min;
 int count_offset;
+float direction;
+float x_dat,y_dat;
+char angle_c;
 
 
 void init(){
@@ -50,44 +59,93 @@
     if(y_min>y_rd && y_rd != 0){
         y_min=y_rd;}
         
-    offset_x = ((abs(x_min)+abs(x_max))/2);
-    offset_y = ((abs(y_min)+abs(y_max))/2);
+    offset_x = (x_min+x_max)/2;
+    offset_y = (y_min+y_max)/2;
     
-    //offset_x = (x_min + x_max)/2;
-    //offset_y = (y_min + y_max)/2;
-    
-    count_offset++;
-    if(count_offset > 300){//300個分のサンプ取得
-           x = x + offset_x;
-           y = y - offset_y;}
+    if(count_offset < 301){
+           count_offset++;}
+    if(count_offset > 300)//300個分のサンプ取得
+     {
+           if(0 < offset_x)
+            { x_dat = x + offset_x; }
+           else if(0 > offset_x)
+            { x_dat = x - offset_x; }
+           
+           if(0 < offset_y)
+            { y_dat = y - offset_y; }
+           else if(0 > offset_y)
+            { y_dat = y + offset_y; }
+            /*
+            if(0 < offset_x)
+             { x_dat = x + offset_x*1.3; }
+            else if(0 > offset_x)
+              { x_dat = x - offset_x*1.3; }
+           
+            if(0 < offset_y)
+              { y_dat = y + offset_y*1.3; }
+            else if(0 > offset_y)
+              { y_dat = y - offset_y*1.3; }*/
+            
+     }
         
     return 0;    
 }
 
+void cal(){
+           
+           direction = atan2(y_dat,x_dat);
+           if(direction < 0)
+             {direction = direction + 2*3.1415926535;}
+             
+           direction = direction*57.2958;//ラジアンに変換
+           //direction = (atan(y_dat/x_dat))*57.2958;
+           //direction = (atan(y_cal/x_cal))*57.2958;
+           
+           if(0 < direction && 89 > direction){
+               angle_c = 'N';}
+               
+           else if(90 < direction && 179 > direction){
+               angle_c = 'E';}
+               
+           else if(180 < direction && 269 > direction || -180 < direction && -91 > direction){
+               angle_c = 'S';}
+               
+           else if(270 < direction && 359 > direction || -90 < direction && -1 > direction){
+               angle_c = 'W';}
+        
+           else{ angle_c = '?';}     
+    
+}
+
 void save(){
 
     bit1=1;//led4を1にしてデータを保存
     FILE *fp = fopen("/local/commpas_new.txt","a");
-               fprintf(fp,"%d,%d,%d\n",x,y,z);
+               fprintf(fp,"%c,%d,%d,%d,%f,%f,%f\n",angle_c,x,y,z,x_dat,y_dat,direction);
+               //fprintf(fp,"%d,%d,%d,%d,%d\n",x,y,z,offset_x,offset_y);
                fclose(fp);
-    bit1=0;
+    bit1=0;    
+}
+
+void dele(){
     
-    if(local_del == 1){//pin19がhighの時、保存したデータを消去
-               bit2=1;//led3を1
-               if(remove("/local/commpas_new.txt") == 0){
-                   pc.printf("delete finished");}}
-    else{bit2=0;}
-    
+    bit2=1;//led3を1
+               
+    if(remove("/local/commpas_new.txt") == 1){
+              pc.printf("delete errer");}
+    bit2=0;
 }
 
 int main(){
     
     mag.enable();//コンパス有効化
-    mag.sampleRate(0x80);
+    mag.sampleRate(0x40);
     init(); //初期化
     
     while(1){
-    
+           
+           if(cds == 0){
+           while(1){
            //void disable(void);
            //uint32_t whoAmI(void);
            //uint32_t dataReady(void);
@@ -101,8 +159,14 @@
            //void getAxis(MotionSensorDataUnits &data);
            //void readRegs(int addr, uint8_t * data, int len);
            offset(x,y);//x,y　オフセット計算
-           if(local_on == 1){save();}//データ保存
-           pc.printf("x: %d  y: %d  z: %d x_max: %d  x_min: %d\n",x,y,z,x_max,x_min);
+           //cal(x_dat,y_dat);
+           cal();
+           if(local_on == 1){save();}//pin20がhighの時、データ保存
+           if(local_del == 1){dele();}//pin19がhighの時、保存したデータを消去
+           pc.printf("<%c> x: %d , y: %d , x_dat: %f , y_dat: %f , direction: %f \n",angle_c,x,y,x_dat,y_dat,direction);    
+           //pc.printf("x_dat: %d  y_dat: %d  x: %d  offset: %d x_max: %d  x_min: %d\n",x_dat,y_dat,x,offset,x_max,x_min);
+            }
+           }
     
      }
 }
\ No newline at end of file