Stra1_Shooting.h

00001 void Stra1_ArrangeTargets();
00002 void Stra1_CheckPosition();
00003 void Stra1_GoingForTargets();
00004 
00005 int TargetNumber, NowGoingTarget;
00006 double TargetX[20], TargetY[20];
00007 int Stra1ShootingState;// = 1;
00008 double   MiddleLength = FieldLength/2;
00009 double   MiddleWidth = FieldWidth/2;
00010 
00011 int    CheckPositionFeedback;    ///////   1  |  2
00012 ////////////////////////////////////////--------------
00013 ////////////////////////////////////////   3  |  4
00014 
00015 #define  ShootingState_Init                 1
00016 #define  ShootingState_GoingBackForBall     2
00017 #define  ShootingState_CheckPosition        3
00018 #define  ShootingState_GoingForTargets      4
00019 #define  ShootingState_Shoot                5
00020 #define  ShootingState_CheckIfBallIn        6
00021 #define  ShootingState_CheckIfEnemyBallIn   7
00022 #define  ShootingState_Defence              8
00023 #define  ShootingState_Back                 9
00024 #define  ShootingState_Push                 10
00025 
00026 /*
00027 void Srta1Shoot_Init()
00028 {
00029     
00030 }
00031 */
00032 void Stra1Shooting(void)
00033 {
00034     switch(Stra1ShootingState)
00035     {
00036         case ShootingState_Init:
00037         Stra1ShootingState = ShootingState_Back;//           
00038         
00039         Stra1_SpeedPlannerState = SpeedPlannerBackwardForWhile;
00040         break;
00041         case ShootingState_Back:
00042         if(Stra1_SpeedPlannerState == SpeedPlannerFinishing)
00043         {
00044             Stra1_SpeedPlannerState = SpeedPlannerIdle;
00045             Stra1ShootingState = ShootingState_CheckPosition;
00046         } 
00047         break;
00048         case ShootingState_CheckPosition:
00049         Stra1_CheckPosition();
00050         case ShootingState_GoingForTargets:
00051         Stra1_GoingForTargets();
00052         break;
00053         case ShootingState_Shoot:
00054         //LED_3 = 1;  
00055         Catch_Release();//Shoot to Servo                       
00056         Stra1_SpeedPlannerState = SpeedPlannerForwardForWhile;
00057         Stra1ShootingState = ShootingState_Push;        
00058         break;
00059         case ShootingState_Push:
00060         if(Stra1_SpeedPlannerState == SpeedPlannerFinishing)
00061         {
00062             Stra1_SpeedPlannerState = SpeedPlannerIdle;
00063             Stra1ShootingState = ShootingState_CheckIfBallIn;
00064             //LED_3 = 0;  
00065         } 
00066         break;
00067         case ShootingState_CheckIfBallIn:
00068         if(Ball_Coordinate[0] > 305)
00069         {
00070         Stra1ShootingState = ShootingState_CheckIfEnemyBallIn;
00071         //LED_2 = 1; 
00072         }
00073         else   Stra1_State = Stra1_Idle;        
00074         break;                
00075         case ShootingState_CheckIfEnemyBallIn:
00076         Stra1_SpeedPlannerState = SpeedPlannerTurning;
00077         TargetCoordinate[0] = 200;
00078         TargetCoordinate[1] = 100;
00079         //check
00080         if(Stra1_SpeedPlannerState == SpeedPlannerFinishing)    
00081         //check
00082         if(1) //enemy ball in
00083         {
00084             //Stra1ShootingState = ShootingState_Init;
00085             //Stra1_State = Stra1_Done;
00086         }
00087         else  Stra1ShootingState = ShootingState_Defence;        
00088         break;
00089         case ShootingState_Defence:
00090         //go back to defence        
00091         break;
00092         default:
00093         break;
00094     }
00095     
00096     if(1)//IfGotBall())
00097     {     
00098         //pc.printf("In");       
00099         //TargetCoordinate[0] = Goal_1_Coordinate[0];
00100         //TargetCoordinate[1] = Goal_1_Coordinate[1];
00101         //Stra1_SpeedPlannerState = SpeedPlannerMoving;
00102         /*
00103         if(Stra1_SpeedPlannerState == SpeedPlannerFinishing) 
00104         {
00105             Stra1_State = Stra1_Done;
00106             Stra1_SpeedPlannerState = SpeedPlannerIdle;
00107             //pc.printf("\nDone");
00108         }
00109         */
00110      }
00111      else
00112      {
00113           //Stra1_State = Stra1_GettingBall;
00114             
00115      }
00116                 
00117 }
00118 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00119 void Stra1_CheckPosition()
00120 {
00121     if(NowCoordinate[0] > MiddleLength)
00122     {
00123        if(NowCoordinate[1] > MiddleWidth) CheckPositionFeedback = 4;
00124        else  CheckPositionFeedback = 2;
00125     }
00126     else
00127     {
00128        if(NowCoordinate[1] > MiddleWidth) CheckPositionFeedback = 3;
00129        else  CheckPositionFeedback = 1;
00130     }
00131     Stra1_ArrangeTargets();
00132 }
00133 
00134 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00135 void Stra1_ArrangeTargets()
00136 {
00137     switch(CheckPositionFeedback)
00138     {
00139         case 1:
00140         TargetNumber = 3;
00141         TargetX[0] = MiddleLength;
00142         TargetY[0] = MiddleWidth * 0.5;
00143         TargetX[1] = 229;//Goal_1_Coordinate[0]*0.7;///2*MiddleLength;
00144         TargetY[1] = 90;//Goal_1_Coordinate[1];///MiddleWidth;
00145         TargetX[2] = 267;//Goal_1_Coordinate[0]*0.85;///2*MiddleLength;
00146         TargetY[2] = 105;//Goal_1_Coordinate[1];///MiddleWidth;
00147         break;
00148         case 2:
00149         TargetNumber = 2;
00150         TargetX[0] = 229;//Goal_1_Coordinate[0]*0.7;
00151         TargetY[0] = 90;//Goal_1_Coordinate[1];
00152         TargetX[1] = 267;//Goal_1_Coordinate[0]*0.85;///2*MiddleLength;
00153         TargetY[1] = 105;//Goal_1_Coordinate[1];///MiddleWidth;
00154         break;
00155         case 3:
00156         TargetNumber = 3;
00157         TargetX[0] = MiddleLength;
00158         TargetY[0] = MiddleWidth * 1.5;
00159         TargetX[1] = 229;//Goal_1_Coordinate[0]*0.7;///2*MiddleLength;
00160         TargetY[1] = 150;//Goal_1_Coordinate[1];///MiddleWidth;
00161         TargetX[2] = 267;//Goal_1_Coordinate[0]*0.85;///2*MiddleLength;
00162         TargetY[2] = 135;//Goal_1_Coordinate[1];///MiddleWidth;
00163         break;
00164         case 4:
00165         TargetNumber = 2;
00166         TargetX[0] = 229;//Goal_1_Coordinate[0]*0.7;
00167         TargetY[0] = 150;//Goal_1_Coordinate[1];
00168         TargetX[1] = 267;//Goal_1_Coordinate[0]*0.85;///2*MiddleLength;
00169         TargetY[1] = 135;//Goal_1_Coordinate[1];///MiddleWidth;
00170         break;
00171         default:
00172         break;
00173     }
00174     NowGoingTarget = 0;
00175     Stra1_SpeedPlannerState = SpeedPlannerTurning;
00176     Stra1ShootingState = ShootingState_GoingForTargets;
00177 }
00178 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00179 void Stra1_GoingForTargets()
00180 {
00181     //////set target
00182     TargetCoordinate[0] = TargetX[NowGoingTarget];
00183     TargetCoordinate[1] = TargetY[NowGoingTarget];
00184     
00185     //check if arrive one target
00186     //if(sqrt((NowCoordinate[0] - TargetCoordinate[0])*(NowCoordinate[0] - TargetCoordinate[0]) + (NowCoordinate[1] - TargetCoordinate[1])*(NowCoordinate[1] - TargetCoordinate[1])) <= 30)
00187     if(Stra1_SpeedPlannerState == SpeedPlannerFinishing)
00188     {        
00189         NowGoingTarget++;
00190         double TargetAngleInRad = atan2( TargetY[NowGoingTarget] - NowCoordinate[1], TargetX[NowGoingTarget] - NowCoordinate[0]);
00191         double AngleError = TargetAngleInRad - NowDirectionInRad;  
00192         if(abs(AngleError) <= 0.15) Stra1_SpeedPlannerState = SpeedPlannerMoving;
00193         else Stra1_SpeedPlannerState = SpeedPlannerTurning;
00194     }
00195     
00196     
00197     
00198     //check if no target left
00199     if(NowGoingTarget >= TargetNumber) Stra1ShootingState = ShootingState_Shoot;
00200 }