File content as of revision 5:794d7d690dc9:

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

int mobile_state;

*****  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:




******************  main structure  ***********************************


switch(mobile_state)
{
    case 0: 
            // IDLE check if stop color appear 
            //check purple color appear or not 
           
        setSpecs();
        break;
    case 1: 
            //  set everything icludes the car's and ball's position
            //  go to case 2 after get all the information
        break;
    case 2: 
        // check if car fit border conditions
        funcBorder();
        to case3
        break;
    case 3://move to get ball
        getBall();
        break;
    case 4: /// move to the gate and release ball
        // move to the point in front of the gate first
        // then head to the gate
        break;
    case 5: 
        break;
        
}






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

funcBorder() // 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) // 靠近下邊界
    {
        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;
}


**********