Diff: main.cpp

Revision:

5:ba883a4bddc3

Parent:

4:0fc7221e2f79

Child:

6:23bb3b018f44

Child:

8:ca21b4e8a350

--- a/main.cpp	Thu Jul 23 09:18:20 2015 +0000
+++ b/main.cpp	Thu Aug 06 17:10:30 2015 +0000
@@ -13,6 +13,7 @@
 #include "VNH5019.h"
 #include "cansat.h"
 #include "math.h"
+#include "BME280.h"
 
 #define SIGMA_MIN 0.0001
 
@@ -46,6 +47,16 @@
 //AigamozuControlPackets agz(agz_motorShield);
 CanSat cansat(agz_motorShield);
 
+//set up for tempratures...
+#if defined(TARGET_LPC1768)
+BME280 sensor(p9, p10);
+#else
+BME280 sensor(I2C_SDA, I2C_SCL);
+#endif
+
+DigitalIn short_in(p29);
+DigitalOut short_out(p30);
+DigitalInOut nic(p5);
 
 /////////////////////////////////////////
 //
@@ -66,6 +77,8 @@
 void Kalman(double Latitude,double Longitude);
 int change = 0;
 
+int mode = -1; //ロボットのモード
+double  goal_Pressure, goal_Temperature,goal_Humidity; //地表地点の気圧、気温、湿度
 
 
 /////////////////////////////////////////
@@ -184,18 +197,73 @@
     return 'b';
 }
 
+//対象とロボットの角度
+double robot_compass(double robot_x, double robot_y) {
+    double angle = 0;
+    
+    if(robot_x==0&&robot_y>0)
+        return 0;
+    else if(robot_x>0&&robot_y==0) //東
+        return 90;
+    else if(robot_x==0&&robot_y<0) //南
+        return 180;
+    else if(robot_x<0&&robot_y==0) //西
+        return 270;
+    else if(robot_x>=0&&robot_y>=0) { //北東
+        if(robot_x<=robot_y)
+            angle = atan2(robot_x, robot_y);
+        else
+            angle = (M_PI/2) - atan2(robot_y, robot_x);
+        return angle * 180.0 / M_PI;
+    } else if(robot_x>=0&&robot_y<0) { //南東
+        if(robot_x>abs(robot_y)){
+            angle = (M_PI/2) - atan2(abs(robot_y), robot_x);
+        }
+        else{
+            angle = atan2(abs(robot_y), robot_x);
+        }
+        return angle * 180.0 / M_PI + 90;
+    } else if(robot_x<0&&robot_y<0) { //南西
+        if(abs(robot_x)<abs(robot_y)){
+            angle = atan2(abs(robot_x), abs(robot_y));
+        }
+        else{
+            angle = (M_PI/2) - atan2(abs(robot_y), abs(robot_x));
+        }
+        return angle * 180.0 / M_PI + 180;
+    } else if(robot_x<0&&robot_y>=0) { //北西
+        if(abs(robot_x)>robot_y){
+            angle = (M_PI/2) - atan2(robot_y, abs(robot_x));
+        }
+        else{
+            angle = atan2(abs(robot_x), robot_y);
+        }
+        return angle * 180.0 / M_PI + 270;
+    }
+    
+    return -1;
+}
+
+
 /******************************
 スタンバイモード
 ******************************/
 void standby(){
-    ;
+    
+    if(short_in == 0){
+        mode = 1;
+    }
+    
 }
 
 /******************************
 落下モード
 ******************************/
 void falling(){
-                
+    if(cansat.get_pressure() >= goal_Pressure){
+        nic = 1;
+        mode = 2;
+    }    
 }
 
 /******************************
@@ -203,14 +271,35 @@
 ******************************/
 void running(){
     double r = 6378.137;
-    double y1 = get_ta
-    double y2 = 
-    double x1 =
-    double x2 =
-    set_target_distance(r*acos(sin);
-    if(get_compass_z < 0) {
+    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;
+
+   cansat.set_target_distance(r*acos(sin(y1)*sin(y2)+cos(y1)*cos(y2)*cos(dx)));
+    
+    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());
+    } else {
+        switch(robot_Action(cansat.get_robot_angle(), cansat.get_target_angle())) {
+            case 'f': //前進
+                cansat.control_Motor(0, cansat.get_speed());
+                break;
+            case 'l':
+                cansat.control_Motor(2, cansat.get_speed());
+                break;
+            case 'r':
+                cansat.control_Motor(3, cansat.get_speed());
+                break;
+        }
+    }
+    
 }
 
 /******************************
@@ -256,7 +345,10 @@
 
     printf("start\n");
     
-    int mode = -1;
+  //  int mode = -1;
+    short_out = 1; //ショートピンの出力:high
+    nic.output();
+    nic = 0;
     
     while (true) {
         
@@ -295,7 +387,7 @@
             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());
         }
     }