Diff: AI_Friday.cpp

Revision:

18:2db6c97a4145

Parent:

17:d96afd9d2692

Child:

19:5091c934ebd0

--- a/AI_Friday.cpp	Wed May 25 13:04:35 2016 +0000
+++ b/AI_Friday.cpp	Wed May 25 13:29:52 2016 +0000
@@ -50,15 +50,14 @@
 int angle = 90;
 float servo_duty = 0.079;//0.079 +(0.084/180)*angle, -90<angle<90
 
-int Command_Flag = 0, Receive_Flag = 0, Receive_Counter = 0, 
-int Receive_Data[33];
+int Command_Flag = 0, Receive_Flag = 0, Receive_Counter = 0; 
+int Receive_Data[33] = {0};
 
-int Distance_Target = 0, Angle_Target = 0;
+double Distance_Target = 0, Angle_Target = 0;
 int X_Position_1 = 0, Y_Position_1 = 0, Angle_1 = 0;
 int X_Position_2 = 0, Y_Position_2 = 0, Angle_2 = 0;
 int pwm_duty;
 
-
 //****  receive and return by bluetooth ************** // bluetooth.getc()
 float xC, yC; // car's position
 float angleC;// car's angle 
@@ -192,8 +191,8 @@
     angleC = Angle_1; // car's angle 
     
     distance = Distance_Target;
-    xP = Distance_Target*cos(Angle_Target);
-    yP = Distance_Target*sin(Angle_Target);//position that car need to reach
+    xP = Distance_Target*cos(Angle_Target*pi/180);
+    yP = Distance_Target*sin(Angle_Target*pi/180);//position that car need to reach
 
     angleR = Angle_Target; //  angleR =  car-nextSpot direction  --->  car direction
     
@@ -321,7 +320,7 @@
         case 4: /// move to the gate and release ball
         // move to the point in front of the gate first
         // then head to the gate
-            distanceGate = sqrt((xGate-xP)^2+(yGate-yP)^2);
+            distanceGate = sqrt(((xGate-xP)*(xGate-xP))+((yGate-yP)*(yGate-yP)));
             angleGate = atan2((yGate-yP),(xGate-xP))*180/pi;
             if(-3<=angleGate<=3) // direct to the gate
            {
@@ -427,7 +426,7 @@
                  pwm2.write(0.3f + 0.5f);
                  wait(1);
                  
-                 aI_State = 0; // return to idle
+                 aI_State = 0;
                   
               }