Diff: main.cpp

Revision:

10:ce253d8a5f2c

Parent:

9:2741e17438d6

Child:

11:19091694455e

--- a/main.cpp	Sun Aug 09 15:05:57 2015 +0000
+++ b/main.cpp	Mon Aug 10 10:01:55 2015 +0000
@@ -17,6 +17,10 @@
 
 #define SIGMA_MIN 0.0001
 
+#define STOP 0 //compass initial
+#define CAL 1 //compass calibration
+#define RUN 2 //compass run
+
 /////////////////////////////////////////
 //
 //Pin Setting
@@ -41,6 +45,9 @@
 XBee xbee(p13,p14);
 ZBRxResponse zbRx = ZBRxResponse();
 
+// compass
+HMC5883L compass(p9, p10);
+
 //set up GPS module
 
 //set up AigamozuControlPackets library
@@ -60,6 +67,27 @@
 
 /////////////////////////////////////////
 //
+//For Compass data
+//
+/////////////////////////////////////////
+Ticker compass_interrupt;
+double heading0 = 0.0;    
+double heading1 = 0.0;
+double heading2 = 0.0;
+double heading3 = 0.0;
+double headingLPF = 0.0;
+double initHeading;
+double tgtHeading;
+double preHeading = 0.0;
+ 
+int maxX, minX, maxY, minY;
+const int ofsX = -122;                   //calibration x
+const int ofsY = -137;                   //calibration y
+
+    int16_t raw[3]; 
+
+/////////////////////////////////////////
+//
 //For Kalman data
 //
 /////////////////////////////////////////
@@ -248,7 +276,38 @@
     return -1;
 }
 
+int calc_angle(double c){
+    if(c > 337.5 ||(c >=0 && c < 22.5) return 0;
+    else if(c >= 22.5 && c < 67.5) return 1;
+    else if(c >= 67.5 && c < 112.5) return 2;
+    else if(c >= 112.5 && c < 157.5) return 3;
+    else if(c >= 157.5 && c < 202.5) return 4;
+    else if(c >= 202.5 && c < 247.5) return 5;
+    else if(c >= 247.5 && c < 292.5) return 6;
+    else if(c >= 292.5 && c < 337.5) return 7;
+    else return 8;
+}  
 
+void Compass_intrpt(){    
+   
+    compass.getXYZ(raw);
+    double heading = atan2(static_cast<double>(raw[2]-ofsY), static_cast<double>(raw[0]-ofsX)); //y=raw[2]
+    if(heading < 0)heading += 2*M_PI;
+    if(heading > 2*M_PI)heading -= 2*M_PI;
+    heading3 = heading2;
+    heading2 = heading1;
+    heading1 = heading0;
+    heading0 = heading;
+    headingLPF = (heading0 + heading1 + heading2 + heading3)/4; //low pass filter
+
+    headingLPF = headingLPF * 180.0 / M_PI;
+    //  pc.printf("heading=%f\r\n",headingLPF);
+
+    cansat.set_compass(raw[0], raw[2], raw[1], headingLPF);
+    cansat.set_robot_angle(calc_angle(headingLPF));
+}
+
+    
 /******************************
 スタンバイモード
 ******************************/
@@ -345,6 +404,8 @@
     //set xbee frequency to 57600bps
     xbee.begin(XBEE_BAUD_RATE);    
         
+    //Compass setting
+    compass.init();
 
     //GPS setting
     gps_Serial = new Serial(p28,p27);
@@ -393,6 +454,17 @@
                 stopping();
             break;
         }
+        
+        compass_interrupt.attach(&Compass_intrpt, 0.5);
+        while(1){
+            
+            printf("compass x : %i, compass y : %i, compass z : %i\n", raw[0], raw[1], raw[2]);
+            printf("set compass x : %i, set compass y : %i, set compass z : %i\n", cansat.get_compass_x(), cansat.get_compass_y(), cansat.get_compass_z());
+            printf("compass angle : %f\n", headingLPF);
+            printf("set compass angle : %f\n", cansat.get_compass_angle());
+            printf("robot angle : %d\n", calc_angle(headingLPF));
+            printf("set robot angle : %d\n", cansat.get_robot_angle());
+        }
 
         myGPS.read();
         //recive gps module
@@ -411,8 +483,8 @@
             //print_gps(count);
             Get_GPS(&myGPS);
             log++;
-            pc.printf("%d times, x:%f, y:%f, speed:%d\n", log, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed());
-            pc.printf("robot_angle:%d, target_angle:%d, robot_compass:%f, %04.2f hPa\n",cansat.get_robot_angle(), cansat.get_target_angle(),  cansat.get_compass_z(), cansat.get_pressure());
+           // pc.printf("%d times, x:%f, y:%f, speed:%d\n", log, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed());
+           // pc.printf("robot_angle:%d, target_angle:%d, robot_compass:%f, %04.2f hPa\n",cansat.get_robot_angle(), cansat.get_target_angle(),  cansat.get_compass_z(), cansat.get_pressure());
         }
     }