Diff: main.cpp

Revision:

11:19091694455e

Parent:

10:ce253d8a5f2c

Child:

12:5724d4a57a4c

--- a/main.cpp	Mon Aug 10 10:01:55 2015 +0000
+++ b/main.cpp	Mon Aug 10 18:22:40 2015 +0000
@@ -105,7 +105,8 @@
 void Kalman(double Latitude,double Longitude);
 int change = 0;
 
-int mode = -1; //ロボットのモード
+int mode = 2; //ロボットのモード
+double target_x = 37.52260177176629,target_y = 139.93881346945034;
 double  goal_Pressure, goal_Temperature,goal_Humidity; //地表地点の気圧、気温、湿度
 
 Timer sep_Timer;
@@ -192,9 +193,9 @@
 } 
 
 //ロボットの動き方
-char robot_Action(double robot_angle, double target_angle) {
+char robot_Action(int robot_angle, int target_angle) {
     
-    double n, t, r;
+    int n, t, r;
     t = target_angle;
     r = robot_angle;
     n = r - t;
@@ -277,7 +278,7 @@
 }
 
 int calc_angle(double c){
-    if(c > 337.5 ||(c >=0 && c < 22.5) return 0;
+    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;
@@ -425,6 +426,11 @@
        
     //interrupt start
     refresh_Timer.start();
+    
+    cansat.set_target(target_x, target_y);
+    wait_ms(10000);
+
+    compass_interrupt.attach(&Compass_intrpt, 0.5);
 
     printf("start\n");
     
@@ -455,8 +461,7 @@
             break;
         }
         
-        compass_interrupt.attach(&Compass_intrpt, 0.5);
-        while(1){
+   /*     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());
@@ -465,7 +470,7 @@
             printf("robot angle : %d\n", calc_angle(headingLPF));
             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,
@@ -483,8 +488,9 @@
             //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("moter command: %c\n", cansat.motor_command);
+            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());
         }
     }