Diff: Function.h

Revision:

3:ec2b7587be78

Parent:

0:79033ee3c961

Child:

4:975b0d9bd51b

--- a/Function.h	Sat Nov 06 03:20:14 2021 +0000
+++ b/Function.h	Wed Nov 10 08:36:55 2021 +0000
@@ -101,8 +101,6 @@
             motor_b = -motor_speed;
             break;
     }
-    pc.printf("input_data=%d\r\n", input_data);
-    xbee.printf("input_data=%d\r\n", input_data);
 }
 
 double AngleGet() 
@@ -116,7 +114,7 @@
 
     pc.printf("gps.latitude=%f, gps.longitude=%f\r\n", gps.latitude, gps.longitude);
     xbee.printf("gps.latitude=%f, gps.longitude=%f\r\n", gps.latitude, gps.longitude);
-    theta = atan2(next_CP_x - gps.latitude , next_CP_y - gps.longitude) * 180 / 3.14159265359;
+    theta = atan2(next_CP_y - gps.longitude , next_CP_x - gps.latitude) * 180 / 3.14159265359;
     printf("theta=%f\r\n", theta);
     if(theta >= 0) {
         delta = angle - theta;
@@ -135,13 +133,13 @@
     pc.printf("calibration start\r\n");
     xbee.printf("calibration start\r\n");
     compass.setCalibrationMode(0x43);
-    Timer t;
-    t.start();
-    while(t.read()<=60){
-        Move('4', 0.5);
-        }
-    t.stop();
-    t.reset();
+    Move('4', 0.1);
+    pc.printf("mortor mode:4 speed:1");
+    xbee.printf("mortor mode:4 speed:1");
+    wait(60);
+    Move('1', 0);
+    pc.printf("mortor mode:1 speed:0");
+    xbee.printf("mortor mode:1 speed:0");
     compass.setCalibrationMode(0x45);
     pc.printf("calibration end\r\n");
     xbee.printf("calibration end\r\n");