Diff: main.cpp

Revision:

12:5724d4a57a4c

Parent:

11:19091694455e

Child:

13:4f3fd6c4ddc2

--- a/main.cpp	Mon Aug 10 18:22:40 2015 +0000
+++ b/main.cpp	Tue Aug 11 01:18:12 2015 +0000
@@ -26,8 +26,8 @@
 //Pin Setting
 //
 /////////////////////////////////////////
-VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
-
+VNH5019 agz_motorShield(p23,p22,p25,p21,p24,p26);
+//VNH5019 agz_motorShield(p21,p22,p23,p24,p25,p26);
 
 /////////////////////////////////////////
 //
@@ -42,7 +42,7 @@
 Serial * gps_Serial;
 
 //Serial Connect Setting: XBEE <--> mbed
-XBee xbee(p13,p14);
+Serial xbee(p13,p14);
 ZBRxResponse zbRx = ZBRxResponse();
 
 // compass
@@ -65,6 +65,8 @@
 DigitalOut short_out(p30);
 DigitalInOut nic(p5);
 
+Timer compass_Timer;
+const int compass_Time = 500;
 /////////////////////////////////////////
 //
 //For Compass data
@@ -81,8 +83,8 @@
 double preHeading = 0.0;
  
 int maxX, minX, maxY, minY;
-const int ofsX = -122;                   //calibration x
-const int ofsY = -137;                   //calibration y
+const int ofsX = 95;                   //calibration x
+const int ofsY = -186;                   //calibration y
 
     int16_t raw[3]; 
 
@@ -361,23 +363,25 @@
     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;
+    if(compass_Timer.read_ms() >= compass_Time){
+        compass_Timer.reset();
+        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;
+            }
         }
     }
-    
     if(cansat.get_target_distance() <= 1){
         mode = 100;
     }
@@ -403,8 +407,8 @@
     //set pc frequency to 57600bps 
     pc.baud(PC_BAUD_RATE); 
     //set xbee frequency to 57600bps
-    xbee.begin(XBEE_BAUD_RATE);    
-        
+    //xbee.begin(XBEE_BAUD_RATE);    
+    xbee.baud(9600);    
     //Compass setting
     compass.init();
 
@@ -428,20 +432,20 @@
     refresh_Timer.start();
     
     cansat.set_target(target_x, target_y);
-    wait_ms(10000);
+   // wait_ms(10000);
 
     compass_interrupt.attach(&Compass_intrpt, 0.5);
 
     printf("start\n");
     
-  //  int mode = -1;
+    //int mode = -1;
     short_out = 1; //ショートピンの出力:high
     nic.output();
     nic = 0;
     int log = 0;
     
     while (true) {
-        
+          
         switch(mode){
             //スタートモード：パラシュートが開くまではこのモードを実行
             case -1:
@@ -488,9 +492,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("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());
+            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());
         }
     }