Diff: AI_Friday.cpp

Revision:

20:5b892e37a958

Parent:

19:5091c934ebd0

Child:

21:6e8ab9487985

--- a/AI_Friday.cpp	Wed May 25 13:38:27 2016 +0000
+++ b/AI_Friday.cpp	Wed May 25 16:43:21 2016 +0000
@@ -35,6 +35,10 @@
 void init_TIMER(void);
 void init_PWM(void);
 void init_CN(void);
+void funcBorder(void);
+void smallAngle( float );
+void turnCW(float);
+void turnCCW(float);
 
 int8_t stateA_1=0, stateB_1=0, stateA_2=0, stateB_2=0;
 int8_t state_1 = 0, state_1_old = 0, state_2 = 0, state_2_old = 0;
@@ -58,16 +62,24 @@
 int X_Position_2 = 0, Y_Position_2 = 0, Angle_2 = 0;
 int pwm_duty;
 
+float k_forward = 20, k_turn = 10;
+
 //****  receive and return by bluetooth ************** // bluetooth.getc()
 float xC, yC; // car's position
 float angleC;// car's angle 
 double xP,yP,distance; //position that car need to reach
 float angleR; //  angleR =  car-nextSpot direction  --->  car direction
+int yB1,yB2,xB3,xB4; // broder conditions
 int xGate = 800, yGate = 300;
 double angleGate, distanceGate;
 int aI_State = 0;
 double pi = 3.1415926;
 
+float longC; // car's length
+float longB;// long of the court
+float ballSize; // ball size
+float wideB; // wide of the court
+
 int main() {
     
     init_TIMER();
@@ -196,6 +208,11 @@
 
     angleR = Angle_Target; //  angleR =  car-nextSpot direction  --->  car direction
     
+    yB1 = longC;
+    yB2 = wideB - longC ;
+    xB3 = longC ;
+    xB4 = longB-longC;
+    
     //****   AI_State   ********************
     
     switch(aI_State)
@@ -215,7 +232,7 @@
       
         case 1:   ///   border condition
             // check if car fit border conditions
-            //****funcBorder();
+            funcBorder();
            //****to case2
             break;
         case 2://move to get ball
@@ -244,25 +261,25 @@
               if(distance<50)
               {
                   v1_ref = 200;
-                  v2_ref = -(v1_ref-5);
+                  v2_ref = -(v1_ref-angleR*k_turn);
               }
               else
               {
                   v1_ref = 300;
-                  v2_ref = -(v1_ref-5);
+                  v2_ref = -(v1_ref-angleR*k_turn);
               }
    
            }
-           else if(-15<=angleR<-3) // small angle left
+           else if(-15 <= angleR < -3) // small angle left
            {
               if(distance<50)
               {
-                  v1_ref = -(v2_ref-5);
+                  v1_ref = -(v2_ref+angleR*k_turn);
                   v2_ref = 200;
               }
               else
               {
-                  v1_ref = -(v2_ref-5);
+                  v1_ref = -(v2_ref+angleR*k_turn);
                   v2_ref = 300;
               }
    
@@ -346,24 +363,24 @@
               if(distanceGate<50)
               {
                   v1_ref = 200;
-                  v2_ref = -(v1_ref-5);
+                  v2_ref = -(v1_ref-angleGate*k_turn);
               }
               else
               {
                   v1_ref = 300;
-                  v2_ref = -(v1_ref-5);
+                  v2_ref = -(v1_ref-angleGate*k_turn);
               }  
            }
            else if(-15<=angleGate<-3) // small angle left
            {
               if(distanceGate<50)
               {
-                  v1_ref = -(v2_ref-5);
+                  v1_ref = -(v2_ref+angleGate*k_turn);
                   v2_ref = 200;
               }
               else
               {
-                  v1_ref = -(v2_ref-5);
+                  v1_ref = -(v2_ref+angleGate*k_turn);
                   v2_ref = 300;
               }  
            }
@@ -615,4 +632,157 @@
     stateB_1 = HallB_1.read();
     stateA_2 = HallA_2.read();
     stateB_2 = HallB_2.read();
+}
+
+//****************  funcBorder()  *****************************************
+
+void funcBorder(void) // check if car is too close to the border  //  miss the ball restriction part
+{
+  if(yC > yB1 || yC < yB2 || xC < xB3 || xC > xB4){
+
+    if(yC > yB1) // 靠近上邊界          *****************************
+    {
+        if(xC < xB3) //左上角  
+        {
+           if(angleC <0 && angleC >=-90)         //   I
+            {}
+            //clockwise, to angleC = 45
+            
+           else if(angleC > -180 && angleC <-90)  //   II
+            {}
+            //back 0.5*longC, judge again
+            
+           else if(angleC <=180 && angleC >=90)  //   III
+            {}
+            //counter clockwise, to angleC = 45
+
+           else if(angleC < 90 && angleC >=0)  //  IV
+            {}
+            //forward 0.5*longC, judge again
+
+        }
+        else if(xC > xB4) //右上角  
+        {
+           if(angleC <0 && angleC >=-90)         //   I
+            {}
+            //back 0.5*longC, judge again
+            
+           else if(angleC > -180 && angleC <-90)  //   II
+            {}
+            //counter clockwise, to angleC = 135
+            
+           else if(angleC <=180 && angleC >=90)  //   III
+            {}
+            //forward 0.5*longC, judge again
+
+           else if(angleC < 90 && angleC >=0)  //  IV
+            {
+            //clockwise, to angleC = 135
+            }
+        }
+        else //上邊界 
+        {
+          if(angleC == -90 || angleC == 90)
+          {} // do nothing, turn to next point
+          else
+          {smallAngle(90);} // turn to 90, then turn to next point
+        }
+      
+        
+    }
+    else if(yC < yB2) // 靠近下邊界    *E****************************************************88
+    {
+        if(xC < xB3) //左下角 
+        {
+           if(angleC <0 && angleC >=-90)         //   I
+            {}
+            //forward 0.5*longC, judge again
+            
+           else if(angleC > -180 && angleC <-90)  //   II
+            {}
+            //clockwise, to angleC = -45
+            
+           else if(angleC <=180 && angleC >=90)  //   III
+            {}
+            //back 0.5*longC, judge again
+
+           else if(angleC < 90 && angleC >=0)  //  IV
+            {}
+            //counter clockwise, to angleC = -45
+          
+        }
+        else if(xC > xB4) //右下角
+        {
+           if(angleC <0 && angleC >=-90)         //   I
+            {}
+            //counter clockwise, to angleC = -135
+            
+           else if(angleC > -180 && angleC <-90)  //   II
+            {}
+            //forward 0.5*longC, judge again
+            
+           else if(angleC <=180 && angleC >=90)  //   III
+            {}
+            //clockwise, to angleC = -135
+
+           else if(angleC < 90 && angleC >=0)  //  IV
+            {}
+            //back 0.5*longC, judge again
+          
+        }
+        else //下邊界  
+        {       
+          if(angleC == -90 || angleC == 90)
+          {} // do nothing, turn to next point
+          else
+          {smallAngle(-90);} // turn to 90, then turn to next point
+        }
+        
+       
+    }
+    else if(xC < xB3) //靠近左邊界****************************************************************
+    {
+          if(angleC == 0 || angleC == 180)
+          {} // do nothing, turn to next point
+          else
+          {smallAngle(0);} // turn to 90, then turn to next point
+    }
+
+    else if(xC > xB4) //靠近右邊界     *********************************************88
+    {
+          if(angleC == 0 || angleC == 180)
+          {} // do nothing, turn to next point
+          else
+          {smallAngle(180);} // turn to 90, then turn to next point
+    }
+  }
+  else
+  {aI_State = 2;}
+} // funcBorder() 
+//****************  smallAngle()  *****************************************
+
+void smallAngle( float goodAngle ) // use the smallest turn to right angle  ccw / cw,  based on 0~360 degree system
+{}
+    if(angleR > 0) // CW
+    {
+        // turn clockwise to goodAngle;
+    }
+    else if(angleR < 0) //CCW
+    {
+        // turn counter clockwise to goodAngle;
+    }
+
+
+//***************   turnCW() ***********************************
+
+void turnCW(float goodAngle)
+{
+  // turn clockwise to goodAngle;
+}
+
+//***************   turnCCW() ***********************************
+
+void turnCCW(float goodAngle)
+{
+  // turn counter clockwise to goodAngle;
 }
\ No newline at end of file