Diff: main.cpp

Revision:

13:4f3fd6c4ddc2

Parent:

12:5724d4a57a4c

Child:

14:4dfeeca65308

diff -r 5724d4a57a4c -r 4f3fd6c4ddc2 main.cpp
--- a/main.cpp	Tue Aug 11 01:18:12 2015 +0000
+++ b/main.cpp	Tue Aug 11 23:03:59 2015 +0000
@@ -83,8 +83,8 @@
 double preHeading = 0.0;
  
 int maxX, minX, maxY, minY;
-const int ofsX = 95;                   //calibration x
-const int ofsY = -186;                   //calibration y
+const int ofsX = 46;                   //calibration x
+const int ofsY = -950;                   //calibration y
 
     int16_t raw[3]; 
 
@@ -356,8 +356,8 @@
     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)));
+    cansat.set_target_angle(calc_angle(robot_compass(x1-x2, y1-y2)));
     
     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);
@@ -379,6 +379,9 @@
                 case 'r':
                     cansat.control_Motor(3, cansat.get_speed());
                     break;
+                case 'b':
+                    cansat.control_Motor(4, cansat.get_speed());
+                    break;
             }
         }
     }
@@ -392,7 +395,7 @@
 ******************************/
 void stopping(){
             
-            cansat.control_Motor(0, cansat.get_speed());
+            cansat.control_Motor(1, cansat.get_speed());
             
 }
 
@@ -430,6 +433,7 @@
        
     //interrupt start
     refresh_Timer.start();
+    compass_Timer.start();
     
     cansat.set_target(target_x, target_y);
    // wait_ms(10000);