Diff: main.cpp

Revision:

8:c8512336fa70

Parent:

7:a8a59a2b6cf6

Child:

9:3f585b1f304d

--- a/main.cpp	Thu Jan 12 05:21:13 2017 +0000
+++ b/main.cpp	Mon Jan 16 14:23:38 2017 +0000
@@ -11,30 +11,98 @@
 Serial pc(USBTX,USBRX);
 int16_t x,y,z;
 LocalFileSystem local("local");
+MAG3110 mag(p28,p27);
+DigitalIn local_on(p20);
+DigitalIn local_del(p19);
+DigitalOut bit1(LED4);
+DigitalOut bit2(LED3);
+DigitalOut bit3(LED2);
+DigitalOut bit4(LED1);
 
-MAG3110 mag(p28,p27);
+int offset_x,offset_y;
+int x_max,y_max,x_min,y_min;
+int count_offset;
 
 
+void init(){
+    
+    x_max = -32500;//x,yの最大値を初期化
+    y_max = -32500;
+    x_min = 32500;
+    y_min = 32500;
+    
+}
+
+int offset(int x_rd,int y_rd){
+    
+    if(x_max == 0 || y_max == 0 || x_min == 0 || y_min == 0){
+        init();}//最大値、最小値に0が入った時初期化
+    
+    if(x_max<x_rd && x_rd != 0){
+        x_max=x_rd;}
+        
+    if(y_max<y_rd && y_rd != 0){
+        y_max=y_rd;}
+    
+    if(x_min>x_rd && x_rd != 0){
+        x_min=x_rd;}
+        
+    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;
+    
+    count_offset++;
+    if(count_offset > 300){//300個分のサンプ取得
+           x = x + offset_x;
+           y = y - offset_y;}
+        
+    return 0;    
+}
+
+void save(){
+
+    bit1=1;//led4を1にしてデータを保存
+    FILE *fp = fopen("/local/commpas_new.txt","a");
+               fprintf(fp,"%d,%d,%d\n",x,y,z);
+               fclose(fp);
+    bit1=0;
+    
+    if(local_del == 1){//pin19がhighの時、保存したデータを消去
+               bit2=1;//led3を1
+               if(remove("/local/commpas_new.txt") == 0){
+                   pc.printf("delete finished");}}
+    else{bit2=0;}
+    
+}
 
 int main(){
-    while(1){
-    mag.enable();
-    //void disable(void);
+    
+    mag.enable();//コンパス有効化
     mag.sampleRate(0x80);
-    //uint32_t whoAmI(void);
-    //uint32_t dataReady(void);
-    mag.getX(&x);
-    mag.getY(&y);
-    mag.getZ(&z);
-    //void getX(float * x);
-    //void getY(float * y);
-    //void getZ(float * z);
-    //void getAxis(MotionSensorDataCounts &data);
-    //void getAxis(MotionSensorDataUnits &data);
-    //void readRegs(int addr, uint8_t * data, int len);
-    pc.printf("x: %d  y: %d  z: %d\n",x,y,z);
+    init(); //初期化
+    
+    while(1){
     
-    FILE *fp = fopen("/local/commpas_new.txt","a");
-               fprintf(fp,"%d,%d,%d\n",x,y,z);
-               fclose(fp);}
+           //void disable(void);
+           //uint32_t whoAmI(void);
+           //uint32_t dataReady(void);
+           mag.getX(&x);
+           mag.getY(&y);
+           mag.getZ(&z);
+           //void getX(float * x);
+           //void getY(float * y);
+           //void getZ(float * z);
+           //void getAxis(MotionSensorDataCounts &data);
+           //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);
+    
+     }
 }
\ No newline at end of file