File content as of revision 4:ae47210fa346:

****  receive and return by bluetooth **************
float xC, yC // car's position

float angleC // car's angle 

float xP,yP //position that car need to reach

float angleR //  angleR =  car-nextSpot direction  --->  car direction

****  specs of everything   ***************

float longC // car's length

float ballSize // ball size

float longB // long of the court

float wideB // wide of the court

*****  Code Begin  **********

int yB1 yB2 xB3 xB4; // broder conditions

yB1 = longC;

yB2 = wideB - longC ;

xB3 = longC ;

xB4 = longB-longC;
*******************************************************************************

遠離邊界  

car: away from the border --> turn to the ball direction --> to get the ball --> get ball
     ---> turn to the point before the gate ---> to the point before the gate --> turn to the gate direction 
     ---> to the gate --> release the ball



matlab:




********************************************************************************

****************  funcBorder()  *****************************************

funcBorder() // check if car is too close to the border

  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) // 靠近下邊界
    {
        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) //靠近右邊界     
    {
          if(angleC = 0 || angleC = 180)
          {} // do nothing, turn to next point
          else
          {smallAngle(180);} // turn to 90, then turn to next point
    }
  }
    
****************  smallAngle()  *****************************************

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() ***********************************

turnCW(float goodAngle)
{
  // turn clockwise to goodAngle;
}

***************   turnCCW() ***********************************

turnCCW(float goodAngle)
{
  // turn counter clockwise to goodAngle;
}


**********