Diff: main.cpp

Revision:

18:c86872baed44

Parent:

16:a3a0e5654835

Child:

19:cb3a4b4c3526

--- a/main.cpp	Wed Aug 12 12:49:40 2015 +0000
+++ b/main.cpp	Wed Aug 12 23:32:49 2015 +0000
@@ -65,8 +65,14 @@
 DigitalOut short_out(p30);
 DigitalInOut nic(p5);
 
+ int short_flag = 0;
+
 Timer compass_Timer;
 const int compass_Time = 500;
+Timer running_Timer;
+const int running_Time = 500;
+Timer parachute_Timer;
+const int parachute_Time = 30000;
 /////////////////////////////////////////
 //
 //For Compass data
@@ -107,12 +113,12 @@
 void Kalman(double Latitude,double Longitude);
 int change = 0;
 
-int mode = 2; //ロボットのモード
+int mode = -1; //ロボットのモード
 double target_x = 40.085592,target_y = 139.592383;
-double  goal_Pressure, goal_Temperature,goal_Humidity; //地表地点の気圧、気温、湿度
+double  goal_Pressure = 1003.5, goal_Temperature,goal_Humidity; //地表地点の気圧、気温、湿度
 
 Timer sep_Timer;
-const int sep_Time = 30000; //seperate time in ms
+const int sep_Time = 3000; //seperate time in ms
 int fall_flag = 0;
 
 int log_number = 0;
@@ -166,8 +172,8 @@
     if (myGPS->fix) {
         
         cansat.nowStatus = GPS_AVAIL;
-        cansat.set_robot_x((double)myGPS->latitudeH + (double)(myGPS->latitudeL / 1000000.0));
-        cansat.set_robot_y((double)myGPS->longitudeH +(double)(myGPS->longitudeL / 1000000.0));
+        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));
     
         
         if(flag < COUNTER_MAX){
@@ -309,7 +315,7 @@
     //  pc.printf("heading=%f\r\n",headingLPF);
 
     cansat.set_compass(raw[0], raw[2], raw[1], headingLPF);
-    cansat.set_robot_angle(calc_angle(headingLPF));
+   cansat.set_robot_angle(calc_angle(cansat.get_compass_angle()));
 }
 
     
@@ -319,9 +325,18 @@
 void standby(){
     
     cansat.control_Motor(1, cansat.get_speed());
-    if(short_in == 0){
+    if(!short_in){
+        xbee.printf("change mode: falling\n");
+        short_flag++;
+    }
+    else{
+        if(running_Timer.read_ms() >= running_Time){
+            running_Timer.reset();
+            xbee.printf("stand by\n");
+        }
+    }
+    if(short_flag >= 5){
         mode = 1;
-        xbee.printf("change mode: falling\n");
     }
     
 }
@@ -331,8 +346,7 @@
 ******************************/
 void falling_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("Pressure:%04.2f hPa\n", cansat.get_pressure());
+    xbee.printf("%d times, pressure:%04.2f hPa\n", log_number, cansat.get_pressure());
 
     log_number++;
     
@@ -344,21 +358,30 @@
     cansat.set_pressure(sensor.getPressure());
     cansat.set_humidity(sensor.getHumidity());
     
+    falling_print();
+    
     if(cansat.get_pressure() >= goal_Pressure){
         if(fall_flag == 0){
             nic = 1;
             fall_flag = 1;
             sep_Timer.reset();
-        }
-        if(fall_flag == 1 && sep_Timer.read_ms() >= sep_Time){
             mode = 2;
-            nic = 0;
+            xbee.printf("my pressure is high!\n");
+            xbee.printf("remove the parachute\n");
             xbee.printf("change mode: running\n");
         }
+    }
+   if(fall_flag == 0 && parachute_Timer.read_ms() >= parachute_Time){
+        mode = 2;
+        nic = 1;
+        xbee.printf("Time out!\n");
+        xbee.printf("remove the parachute\n");
+        wait_ms(3000);
+        nic = 0;
+        xbee.printf("change mode: running\n");
 
     }    
 
-    falling_print();
 
 }
 
@@ -369,8 +392,14 @@
 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, robot_compass:%f, prressure:%04.2f hPa\n",cansat.get_robot_angle(), cansat.get_target_angle(),  cansat.get_compass_angle(), cansat.get_pressure());
+ //   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));
 
     log_number++;
     
@@ -378,20 +407,26 @@
 
 void running(){
     
-    double r = 6378.137;
+  //  double r = 6378.137;
     double y1 = cansat.get_target_y();
     double y2 = cansat.get_robot_y();
     double x1 = cansat.get_target_x();
     double x2 = cansat.get_robot_x();
     double dx = x2 - x1;
+    double dy = y2 - y1;
+    double y_sub = dy*111135;
+    double x_sub = dx*91191;
     
-    cansat.set_target_distance(r*acos(sin(y1)*sin(y2)+cos(y1)*cos(y2)*cos(dx)));
+    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(fall_flag == 1 && sep_Timer.read_ms() >= sep_Time){
+        nic = 0;
+    }
     if(compass_Timer.read_ms() >= compass_Time){
         compass_Timer.reset();
         if(cansat.get_compass_z() < 0) {
@@ -415,13 +450,14 @@
             }
         }
     }
+    
+    running_print();
+    
     if(cansat.get_target_distance() <= 1){
         mode = 100;
         xbee.printf("change mode: stopping\n");
         xbee.printf("it is goal point.\n");
     }
-    
-    running_print();
 }
 
 /******************************
@@ -454,8 +490,6 @@
     Adafruit_GPS myGPS(gps_Serial); 
     Timer refresh_Timer;
     const int refresh_Time = 1000; //refresh time in ms
-    Timer running_Timer;
-    const int running_Time = 500;
     int count = 0;
 
     myGPS.begin(GPS_BAUD_RATE); 
@@ -471,18 +505,24 @@
     refresh_Timer.start();
     compass_Timer.start();
     running_Timer.start(); 
+    sep_Timer.start();
+    parachute_Timer.start();
     cansat.set_target(target_x, target_y);
    // wait_ms(10000);
 
     compass_interrupt.attach(&Compass_intrpt, 0.5);
 
     printf("start\n");
-    
     //int mode = -1;
-    short_out = 1; //ショートピンの出力:high
     nic.output();
     nic = 0;
     
+    short_out = 1;
+    wait_ms(1000);
+    if(short_out){
+       xbee.printf("HIGH\n");
+    }
+    
     while (true) {
 
         switch(mode){