Diff: AI_Friday.cpp

Revision:

34:b0ae940a2fb5

Parent:

33:587e6dc249b8

--- a/AI_Friday.cpp	Thu May 26 16:35:54 2016 +0000
+++ b/AI_Friday.cpp	Fri May 27 00:41:22 2016 +0000
@@ -58,7 +58,7 @@
 int X_Position_1 = 0, Y_Position_1 = 0, Angle_1 = 0;
 int X_Position_2 = 0, Y_Position_2 = 0, Angle_2 = 0;
 
-float k_forward = 0.3, k_turn = 1;
+float k_forward = 0.05, k_turn = 1;
 
 //****  receive and return by bluetooth ************** // bluetooth.getc()
 float xC, yC; // car's position
@@ -170,8 +170,8 @@
     ///code for PI control///    
     v1_err = v1_ref - v1;
     v1_ierr = 0.01f*v1_err + v1_ierr;
-    PIout_1 = Kp*v1_err + Ki*v1_ierr; 
-    
+    //PIout_1 = Kp*v1_err + Ki*v1_ierr; 
+    PIout_1 = v1_ref;
     if(PIout_1 >= 0.5f)PIout_1 = 0.5f;
     else if(PIout_1 <= -0.5f)PIout_1 = -0.5f;
     pwm1.write(PIout_1 + 0.5f);
@@ -184,7 +184,8 @@
     ///code for PI control///
     v2_err = v2_ref - v2;
     v2_ierr = 0.01f*v2_err + v2_ierr;
-    PIout_2 = Kp*v2_err + Ki*v2_ierr; 
+    //PIout_2 = Kp*v2_err + Ki*v2_ierr; 
+    PIout_2 = v2_ref;
     
     if(PIout_2 >= 0.5f)PIout_2 = 0.5f;
     else if(PIout_2 <= -0.5f)PIout_2 = -0.5f;
@@ -295,18 +296,19 @@
            //****to case2
             break;
         case 2://move to get ball
-            if((distance == 50) && (-5<=angleR<=5))
+            
+            if((40<=distance <= 50) && (-5<=angleR<=5))
             {
                 aI_State = 3;
             }
             else
             {
-                v1_ref = k_forward*(distance - 50) + k_turn*angleR;
-                v2_ref = -k_forward*(distance - 50) - k_turn*angleR;
+                v1_ref = k_forward*(distance - 40) + k_turn*angleR;
+                v2_ref = -k_forward*(distance - 40) + k_turn*angleR;
             }
             break;
         case 3: //****getBall();
-             if((distance == 50) && (-5<=angleR<=5))
+             if((40<=distance <= 50) && (-5<=angleR<=5))
              {
                  angle = -10;
                  servo_duty = 0.079 + (0.084/180)*angle;
@@ -440,7 +442,7 @@
 
 void CN_interrupt(void)
 {
-    /*//Motor 1
+    //Motor 1
     stateA_1 = HallA_1.read();
     stateB_1 = HallB_1.read();
     
@@ -535,7 +537,7 @@
         else if(state_2-state_2_old == 3)
         v2Count=v2Count-1;     
     }
-        state_2_old = state_2;*/
+        state_2_old = state_2;
     
     //////////////////////////////////