Diff: main.cpp

Revision:

8:ca21b4e8a350

Parent:

5:ba883a4bddc3

diff -r ba883a4bddc3 -r ca21b4e8a350 main.cpp
--- a/main.cpp	Thu Aug 06 17:10:30 2015 +0000
+++ b/main.cpp	Sat Aug 08 07:44:32 2015 +0000
@@ -4,10 +4,7 @@
 /**********************************************/
 
 #include "mbed.h"
-#include "XBee.h"
 #include "MBed_Adafruit_GPS.h"
-//#include "AigamozuControlPackets.h"
-#include "agzIDLIST.h"
 #include "aigamozuSetting.h"
 #include "HMC5883L.h"
 #include "VNH5019.h"
@@ -24,7 +21,6 @@
 /////////////////////////////////////////
 VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
 
-
 /////////////////////////////////////////
 //
 //Connection Setting
@@ -38,8 +34,7 @@
 Serial * gps_Serial;
 
 //Serial Connect Setting: XBEE <--> mbed
-XBee xbee(p13,p14);
-ZBRxResponse zbRx = ZBRxResponse();
+Serial xbee(p13,p14);
 
 //set up GPS module
 
@@ -58,6 +53,11 @@
 DigitalOut short_out(p30);
 DigitalInOut nic(p5);
 
+
+
+Timer sep_Timer;
+const int sep_Time = 1000; //seperate time in ms
+
 /////////////////////////////////////////
 //
 //For Kalman data
@@ -244,7 +244,35 @@
     return -1;
 }
 
-
+void calc_target_angle(double angle){
+    if(0 <= angle && angle < 22.5){
+        cansat.set_target_angle(0);
+    }
+    if(22.5 <= angle && angle < 67.5){
+        cansat.set_target_angle(1);
+    }
+    if(67.5 <= angle && angle < 112.5){
+        cansat.set_target_angle(2);
+    }
+    if(112.5 <= angle && angle < 157.5){
+        cansat.set_target_angle(3);
+    }
+    if(157.5 <= angle && angle < 202.5){
+        cansat.set_target_angle(4);
+    }
+    if(202.5 <= angle && angle < 247.5){
+        cansat.set_target_angle(5);
+    }
+    if(247.5 <= angle && angle < 292.5){
+        cansat.set_target_angle(6);
+    }
+    if(292.5 <= angle && angle < 337.5){
+        cansat.set_target_angle(7);
+    }    
+    if(337.5 <= angle && angle < 360){
+        cansat.set_target_angle(7);
+    }  
+}
 /******************************
 スタンバイモード
 ******************************/
@@ -263,13 +291,19 @@
     if(cansat.get_pressure() >= goal_Pressure){
         nic = 1;
         mode = 2;
+        sep_Timer.reset();
     }    
 }
 
+
 /******************************
 走行モード
 ******************************/
 void running(){
+    if(sep_Timer.read_ms() >= sep_Time){
+        nic=0; 
+    }
+    
     double r = 6378.137;
     double y1 = cansat.get_target_y();
     double y2 = cansat.get_robot_y();
@@ -277,12 +311,18 @@
     double x2 = cansat.get_robot_x();
     double dx = x2 - x1;
 
-   cansat.set_target_distance(r*acos(sin(y1)*sin(y2)+cos(y1)*cos(y2)*cos(dx)));
+    //kyorinoset
+    cansat.set_target_distance(r*acos(sin(y1)*sin(y2)+cos(y1)*cos(y2)*cos(dx)));
+    double angle = robot_compass(x2-x1,y2-y1);
+
+    
+    
     
     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_compass_z() < 0) {
         //ひっくり返っている
         cansat.control_Motor(0, cansat.get_speed());
@@ -306,7 +346,8 @@
 停止モード
 ******************************/
 void stopping(){
-            
+       cansat.control_Motor(1,cansat.get_speed());
+       xbee.printf("stop\n");
             
 }
 
@@ -320,8 +361,8 @@
     wait(3);
     //set pc frequency to 57600bps 
     pc.baud(PC_BAUD_RATE); 
-    //set xbee frequency to 57600bps
-    xbee.begin(XBEE_BAUD_RATE);    
+    //set xbee frequency to 9600bps
+    xbee.baud(XBEE_BAUD_RATE);    
         
 
     //GPS setting
@@ -345,12 +386,16 @@
 
     printf("start\n");
     
+    //xbee.printf("start\n");
+    
   //  int mode = -1;
     short_out = 1; //ショートピンの出力:high
     nic.output();
     nic = 0;
     
     while (true) {
+        xbee.printf("test\n");
+        xbee.printf("mode = %d\n",mode);
         
         switch(mode){
             //スタートモード：パラシュートが開くまではこのモードを実行
@@ -371,6 +416,13 @@
             break;
         }
 
+        //落下時のみ気圧センサを読む
+        if(mode == 1){
+            cansat.set_temperature(sensor.getTemperature());
+            cansat.set_pressure(sensor.getPressure());
+            cansat.set_humidity(sensor.getHumidity());
+        }
+        
         myGPS.read();
         //recive gps module
         //check if we recieved a new message from GPS, if so, attempt to parse it,
@@ -387,7 +439,11 @@
             refresh_Timer.reset();
             //print_gps(count);
             Get_GPS(&myGPS);
-            pc.printf("%2.2f degC, %04.2f hPa, %2.2f %%\n", sensor.getTemperature(), sensor.getPressure(), sensor.getHumidity());
+            
+            cansat.set_temperature(sensor.getTemperature());
+            cansat.set_pressure(sensor.getPressure());
+            cansat.set_humidity(sensor.getHumidity());
+            //pc.printf("%2.2f degC, %04.2f hPa, %2.2f %%\n", sensor.getTemperature(), sensor.getPressure(), sensor.getHumidity());
         }
     }