Revision 19:cb3a4b4c3526, committed 2015-08-13

Comitter:

s1200058

Date:

Thu Aug 13 07:11:57 2015 +0000

Parent:

18:c86872baed44

Child:

20:f92bdcda5a60

Commit message:

08/13_16:10

Changed in this revision

--- a/cansat.lib	Wed Aug 12 23:32:49 2015 +0000
+++ b/cansat.lib	Thu Aug 13 07:11:57 2015 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/s1200058/code/cansat/#ea6a71ec9f9d
+https://developer.mbed.org/users/s1200058/code/cansat/#e40f6ad7dab3

--- a/main.cpp	Wed Aug 12 23:32:49 2015 +0000
+++ b/main.cpp	Thu Aug 13 07:11:57 2015 +0000
@@ -66,6 +66,7 @@
 DigitalInOut nic(p5);
 
  int short_flag = 0;
+ int running_flag = 0;
 
 Timer compass_Timer;
 const int compass_Time = 500;
@@ -89,8 +90,8 @@
 double preHeading = 0.0;
  
 int maxX, minX, maxY, minY;
-const int ofsX = 46;                   //calibration x
-const int ofsY = -950;                   //calibration y
+const int ofsX = 36;                   //calibration x
+const int ofsY = -425;                   //calibration y
 
     int16_t raw[3]; 
 
@@ -114,8 +115,8 @@
 int change = 0;
 
 int mode = -1; //ロボットのモード
-double target_x = 40.085592,target_y = 139.592383;
-double  goal_Pressure = 1003.5, goal_Temperature,goal_Humidity; //地表地点の気圧、気温、湿度
+double target_x = 139.987305,target_y = 40.142655;
+double  goal_Pressure = 1002.20, goal_Temperature,goal_Humidity; //地表地点の気圧、気温、湿度
 
 Timer sep_Timer;
 const int sep_Time = 3000; //seperate time in ms
@@ -172,8 +173,8 @@
     if (myGPS->fix) {
         
         cansat.nowStatus = GPS_AVAIL;
-        cansat.set_robot_x((double)myGPS->latitudeH + (double)(myGPS->latitudeL / 1000000.0 / 60.0));
-        cansat.set_robot_y((double)myGPS->longitudeH +(double)(myGPS->longitudeL / 1000000.0 / 60.0));
+        cansat.set_robot_y((double)myGPS->latitudeH + (double)(myGPS->latitudeL / 10000.0 / 60.0));
+        cansat.set_robot_x((double)myGPS->longitudeH +(double)(myGPS->longitudeL / 10000.0 / 60.0));
     
         
         if(flag < COUNTER_MAX){
@@ -309,7 +310,7 @@
     heading2 = heading1;
     heading1 = heading0;
     heading0 = heading;
-    headingLPF = (heading0 + heading1 + heading2 + heading3)/4; //low pass filter
+    headingLPF = (heading0 + heading1 + heading2 + heading3)/4.0; //low pass filter
 
     headingLPF = headingLPF * 180.0 / M_PI;
     //  pc.printf("heading=%f\r\n",headingLPF);
@@ -337,6 +338,7 @@
     }
     if(short_flag >= 5){
         mode = 1;
+        parachute_Timer.start();
     }
     
 }
@@ -346,7 +348,7 @@
 ******************************/
 void falling_print(){
     
-    xbee.printf("%d times, pressure:%04.2f hPa\n", log_number, cansat.get_pressure());
+    xbee.printf("%d: %04.2f hPa\n", log_number, cansat.get_pressure());
 
     log_number++;
     
@@ -391,15 +393,19 @@
 
 void running_print(){
     
-    xbee.printf("%d times, x:%f, y:%f, speed:%d\n", log_number, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed());
- //   xbee.printf("moter command: %c\n", cansat.motor_command);
- //   xbee.printf("robot_angle:%d, target_angle:%d, compass_angle:%f\n",cansat.get_robot_angle(), cansat.get_target_angle(),cansat.get_compass_angle());
-xbee.printf("robot_x:%f, robot_y:%f, target_x:%f, target_y:%f", cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_target_x(), cansat.get_target_y());
-       double y1 = cansat.get_target_y();
-    double y2 = cansat.get_robot_y();
-    double x1 = cansat.get_target_x();
-    double x2 = cansat.get_robot_x();
-    xbee.printf("taret_angle:%f",robot_compass(x1-x2, y1-y2));
+    if(running_flag <= 20){
+        xbee.printf("%d: %f, %f, %d\n", log_number, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed());
+    }
+    else{
+        xbee.printf("%d: %f, %f, %d\n", log_number, cansat.get_robotKalman_x(), cansat.get_robotKalman_y(), cansat.get_speed());
+    }       
+    xbee.printf("%c, %f\n", cansat.motor_command, cansat.get_compass_angle());
+    xbee.printf("%d, %d\n",cansat.get_robot_angle(), cansat.get_target_angle());
+ //   double y1 = cansat.get_target_y();
+  //  double y2 = cansat.get_robot_y();
+  //  double x1 = cansat.get_target_x();
+  //  double x2 = cansat.get_robot_x();
+  //  xbee.printf("taret_angle:%f",robot_compass(x1-x2, y1-y2));
 
     log_number++;
     
@@ -412,17 +418,32 @@
     double y2 = cansat.get_robot_y();
     double x1 = cansat.get_target_x();
     double x2 = cansat.get_robot_x();
+    double x_k2 = cansat.get_robotKalman_x();
+    double y_k2 = cansat.get_robotKalman_y();
     double dx = x2 - x1;
     double dy = y2 - y1;
-    double y_sub = dy*111135;
-    double x_sub = dx*91191;
+    double dx_k = x_k2 - x1;
+    double dy_k = y_k2 - y1;
+    double y_sub;
+    double x_sub;
+  
+    if(running_flag < 20){
+        y_sub = dy*111135.0;
+        x_sub = dx*91191.0;
+        cansat.set_target_distance(sqrt(y_sub*y_sub + x_sub*x_sub));
+        cansat.set_target_angle(calc_angle(robot_compass(x1-x2, y1-y2)));
+        running_flag++;
+    }
+    else{
+        y_sub = dy_k*111135.0;
+        x_sub = dx_k*91191.0;
+        cansat.set_target_distance(sqrt(y_sub*y_sub + x_sub*x_sub));
+        cansat.set_target_angle(calc_angle(robot_compass(x1-x_k2, y1-y_k2)));
+    } 
     
-    cansat.set_target_distance(sqrt(y_sub*y_sub + x_sub*x_sub));
-    cansat.set_target_angle(calc_angle(robot_compass(x1-x2, y1-y2)));
-    
-    if(cansat.get_target_distance() < 10) cansat.set_speed(32);
-    else if(cansat.get_target_distance() < 20 && cansat.get_target_distance() > 10) cansat.set_speed(64);
-    else cansat.set_speed(128);
+    if(cansat.get_target_distance() < 10) cansat.set_speed(64);
+    else if(cansat.get_target_distance() < 20 && cansat.get_target_distance() > 10) cansat.set_speed(128);
+    else cansat.set_speed(255);
     
     if(fall_flag == 1 && sep_Timer.read_ms() >= sep_Time){
         nic = 0;
@@ -481,7 +502,7 @@
     pc.baud(PC_BAUD_RATE); 
     //set xbee frequency to 57600bps
     //xbee.begin(XBEE_BAUD_RATE);    
-    xbee.baud(9600);    
+    xbee.baud(57600);    
     //Compass setting
     compass.init();
 
@@ -491,6 +512,8 @@
     Timer refresh_Timer;
     const int refresh_Time = 1000; //refresh time in ms
     int count = 0;
+    Timer compass_refresh_Timer;
+    const int compass_refresh_Time = 500;
 
     myGPS.begin(GPS_BAUD_RATE); 
     
@@ -506,24 +529,53 @@
     compass_Timer.start();
     running_Timer.start(); 
     sep_Timer.start();
-    parachute_Timer.start();
+    compass_refresh_Timer.start();
     cansat.set_target(target_x, target_y);
    // wait_ms(10000);
 
-    compass_interrupt.attach(&Compass_intrpt, 0.5);
+    //compass_interrupt.attach(&Compass_intrpt, 0.5);
+
 
     printf("start\n");
+    xbee.printf("target: %f, %f\n",cansat.get_target_x(), cansat.get_target_y());
     //int mode = -1;
     nic.output();
     nic = 0;
     
     short_out = 1;
     wait_ms(1000);
-    if(short_out){
-       xbee.printf("HIGH\n");
-    }
+//    if(short_out){
+//       xbee.printf("HIGH\n");
+//    }
     
     while (true) {
+        
+        myGPS.read();
+        //recive gps module
+        //check if we recieved a new message from GPS, if so, attempt to parse it,
+        if ( myGPS.newNMEAreceived() ) {
+            if ( !myGPS.parse(myGPS.lastNMEA()) ) {
+                continue;   
+            } 
+            else{
+                count++;
+            }    
+        }
+        //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
+        if (refresh_Timer.read_ms() >= refresh_Time) {
+            refresh_Timer.reset();
+            //print_gps(count);
+            Get_GPS(&myGPS);
+            //log++;
+ /*           xbee.printf("%d times, x:%f, y:%f, speed:%d\n", log, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed());
+            xbee.printf("moter command: %c\n", cansat.motor_command);
+            xbee.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_angle(), cansat.get_pressure());
+ */
+        }
+        if(compass_refresh_Timer.read_ms() >= compass_refresh_Time){
+            Compass_intrpt();
+            compass_refresh_Timer.reset();
+        }
 
         switch(mode){
             //スタートモード：パラシュートが開くまではこのモードを実行
@@ -557,28 +609,7 @@
             printf("set robot angle : %d\n", cansat.get_robot_angle());
         }
 */
-        myGPS.read();
-        //recive gps module
-        //check if we recieved a new message from GPS, if so, attempt to parse it,
-        if ( myGPS.newNMEAreceived() ) {
-            if ( !myGPS.parse(myGPS.lastNMEA()) ) {
-                continue;   
-            } 
-            else{
-                count++;
-            }    
-        }
-        //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
-        if (refresh_Timer.read_ms() >= refresh_Time) {
-            refresh_Timer.reset();
-            //print_gps(count);
-            Get_GPS(&myGPS);
-            //log++;
- /*           xbee.printf("%d times, x:%f, y:%f, speed:%d\n", log, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed());
-            xbee.printf("moter command: %c\n", cansat.motor_command);
-            xbee.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_angle(), cansat.get_pressure());
- */
-        }
+            
     }
     
 }
\ No newline at end of file