strategy1.h

00001 
00002 
00003 
00004 void Stra1_Init();
00005 void Stra1_Nrtloop();
00006 void Stra1_ReviseCoordinates();
00007 void Stra1_MakePCangleNear();
00008 void Stra1_SpeedPlanner();
00009 void Stra1_SpeedPlanner_Turning();
00010 void Stra1_SpeedPlanner_Movinging();
00011 void Stra1_SpeedPlanner_Finishing();
00012 void UpdateDataFromBT();
00013 bool IfGotBall();
00014 
00015 
00016 
00017 //void Stra1_Rtloop();
00018 /////////////////////////////////////////////////////
00019 #define SpeedPlannerIdle             0
00020 #define SpeedPlannerTurning          1
00021 #define SpeedPlannerMoving           2
00022 #define SpeedPlannerFinishing        3
00023 #define SpeedPlannerForwardForWhile  4
00024 #define SpeedPlannerBackwardForWhile     5
00025 
00026 int Stra1_SpeedPlannerState;
00027 
00028 ///////////////////////////////////////////////////
00029 /////////////////////////////////////////////////////
00030 #define Stra1_Idle        0
00031 #define Stra1_GettingBall 1
00032 #define Stra1_Shooting    2
00033 #define Stra1_Done        3
00034 
00035 
00036 int Stra1_State;
00037 ///////////////////////////////////////////////////
00038 double  NowCoordinateFromPC[2], NowFaceAngleFromPC, EnemyCoordinateFromPC[2];
00039 double  ReviseCoordinatesOriginalXYgain = 0; //0~1
00040 double  ReviseCoordinatesOriginalAngleGain = 0; //0~1
00041 
00042 double DesiredTurningSpeed, DesiredMovingSpeed;
00043 
00044 bool BallGotTemp = false;
00045 bool StartTemp = false;
00046 bool Pause;
00047 double BallSpeed;
00048 double TargetSpeed;
00049 int  CatchClosingCounts = 0;
00050 int  NoBallCounts = 0;
00051 
00052 #include "Stra1_Shooting.h"
00053 
00054 void Stra1_Init()
00055 {
00056     Stra1_SpeedPlannerState = 0;
00057     //TargetCoordinate[0] = 150;
00058     //TargetCoordinate[1] = 110;
00059     Stra1_State = 0;
00060     Pause = true;
00061     Last_Ball_Coordinate[0] = 0;
00062     Last_Ball_Coordinate[1] = 0;
00063 }
00064 
00065 void Stra1_Nrtloop()
00066 {
00067     /*
00068     pc.printf("\n%f",NowCoordinateFromPC[0]);
00069     pc.printf("  %f",NowCoordinateFromPC[1]);
00070     pc.printf("  %f",NowFaceAngleFromPC);
00071     pc.printf("  %f",Ball_1_Coordinate[0]);
00072     pc.printf("  %f",Ball_1_Coordinate[1]);
00073     */
00074     /*
00075     pc.printf("\n  %f",Car1XforStra1);
00076     pc.printf("  %f",Car1YforStra1);
00077     pc.printf("  %f",Car1DirectionforStra1);
00078     pc.printf("  %f",BlueBallXforStra1);
00079     pc.printf("  %f \n",BlueBallYforStra1);
00080     */
00081     
00082     //MakePCangleNear();
00083     //UpdateDataFromBT();
00084     
00085 }
00086 
00087 void Stra1_Rtloop()
00088 {
00089     
00090     UpdateDataFromBT();
00091     Stra1_ReviseCoordinates();    
00092     
00093     //if(BallGotTemp == true) Stra1_State = Stra1_Shooting;
00094     bool NotBlocked;
00095     
00096     
00097     switch(Stra1_State)
00098     {
00099         case Stra1_Idle:
00100         Stra1_SpeedPlannerState = SpeedPlannerIdle;
00101         //
00102         double MiddleX = 165;
00103         double MiddleY = 116;
00104         double ToBallAngle = atan2( Ball_Coordinate[1] - NowCoordinate[1], Ball_Coordinate[0] - NowCoordinate[0]);
00105         double ToBallDistance = sqrt((Ball_Coordinate[1] - NowCoordinate[1])*(Ball_Coordinate[1] - NowCoordinate[1]) + (Ball_Coordinate[0] - NowCoordinate[0])*(Ball_Coordinate[0] - NowCoordinate[0]));
00106         double MiddleMotorAngle = atan2( MiddleY - NowCoordinate[1], MiddleX - NowCoordinate[0]);
00107         double ToMiddleMotorDistance = sqrt((MiddleY - NowCoordinate[1])*(MiddleY - NowCoordinate[1]) + (MiddleX - NowCoordinate[0])*(MiddleX - NowCoordinate[0]));
00108         double AngleBetween = MiddleMotorAngle - ToBallAngle;
00109         if(abs(AngleBetween) <= 0.2 && ToMiddleMotorDistance < ToBallDistance)  NotBlocked = false;
00110         else NotBlocked = true;
00111         //
00112         if(TargetCoordinateValid ==  true && Pause == false/*&& StartTemp == false*/ && NotBlocked)// (Ball_Coordinate[0] < MiddleLength*1.1))
00113         {
00114             Stra1_State = Stra1_GettingBall;
00115             //pc.printf("\nGet");
00116             StartTemp = true;
00117             //Catch_Release();
00118         }
00119         break;
00120         case Stra1_GettingBall:
00121         TargetCoordinate[0] = Ball_Coordinate[0];//Goal_1_Coordinate[0] + (Ball_1_Coordinate[0] - Goal_1_Coordinate[0])*1.2;
00122         TargetCoordinate[1] = Ball_Coordinate[1];//Goal_1_Coordinate[1] + (Ball_1_Coordinate[1] - Goal_1_Coordinate[1])*1.2;
00123         TargetSpeed = BallSpeed;
00124         if(IfGotBall() == true) 
00125         {
00126             CatchClosingCounts++;
00127             Catch_Hold();
00128         }        
00129         if(IfGotBall() == true && CatchClosingCounts >= 40)//if(Stra1_SpeedPlannerState == SpeedPlannerFinishing)
00130         {
00131             CatchClosingCounts = 0;
00132             //Stra1_State = Stra1_Shooting;
00133             //TargetCoordinate[0] = Goal_1_Coordinate[0];
00134             //TargetCoordinate[1] = Goal_1_Coordinate[1];
00135             Stra1_SpeedPlannerState = SpeedPlannerIdle;
00136             //pc.printf("\nShoot");
00137             Stra1_State = Stra1_Shooting;
00138             //Catch_Hold();
00139             Stra1ShootingState = ShootingState_Init;  
00140                 
00141         }
00142         
00143         break;
00144         case Stra1_Shooting:
00145         TargetSpeed = 0;
00146         Stra1Shooting();
00147         if(IfGotBall() == false) NoBallCounts++;
00148         else NoBallCounts = 0;
00149         if(NoBallCounts >= 100)
00150         {
00151             NoBallCounts = 0;
00152             Catch_Release();
00153             //Stra1_State = Stra1_GettingBall;
00154             Stra1_State = Stra1_Idle;
00155         }
00156         
00157         break;
00158         case Stra1_Done:
00159         Stra1_SpeedPlannerState = SpeedPlannerIdle;
00160         //pc.printf("\nDone");
00161         break;
00162     }
00163     Stra1_SpeedPlanner();
00164     if(Pause == true)   
00165     {
00166         Stra1_State = Stra1_Idle;
00167         Catch_Release();
00168     }
00169     if(WatchDogCheck(WatchDogCounts))
00170     {
00171         Stra1_State = Stra1_Idle;
00172         Catch_Release();
00173         WatchDogCounts = 0;
00174     }
00175         
00176 }
00177 
00178 void Stra1_ReviseCoordinates()
00179 {
00180     NowCoordinate[0] = NowCoordinate[0]*ReviseCoordinatesOriginalXYgain + NowCoordinateFromPC[0]*(1 - ReviseCoordinatesOriginalXYgain); 
00181     NowCoordinate[1] = NowCoordinate[1]*ReviseCoordinatesOriginalXYgain + NowCoordinateFromPC[1]*(1 - ReviseCoordinatesOriginalXYgain);
00182     NowDirectionInRad = NowDirectionInRad*ReviseCoordinatesOriginalAngleGain + NowFaceAngleFromPC*(1 - ReviseCoordinatesOriginalAngleGain);
00183 }
00184 
00185 void Stra1_MakePCangleNear()
00186 {
00187     NowFaceAngleFromPC;
00188     
00189 }
00190 
00191 #include "Stra1_SpeedPlanner.h"
00192 double UpdateBallSpeedCounts = 0;
00193 
00194 void UpdateDataFromBT()
00195 {
00196     //if(DataValidorNot == true)
00197     {
00198         int WhileCounts = 0;
00199         //TargetCoordinateValid = false;
00200         //DataValidorNot = false;
00201         if(GREEN_BALL_MODE == true)
00202         {
00203             if(FirstReceive) 
00204             {
00205                 NowFaceAngleFromPC = Car2DirectionforStra1;
00206                 FirstReceive = false;
00207             }
00208             NowCoordinateFromPC[0] = Car2XforStra1;
00209             NowCoordinateFromPC[1] = Car2YforStra1;
00210             Ball_Coordinate[0] = GreenBallXforStra1;//BlueBallXforStra1;
00211             Ball_Coordinate[1] = GreenBallYforStra1;
00212             EnemyCoordinateFromPC[0] = Car1XforStra1;
00213             EnemyCoordinateFromPC[1] = Car1YforStra1;
00214             
00215             Enemy_Ball_Coordinate[0] = BlueBallXforStra1;
00216             Enemy_Ball_Coordinate[1] = BlueBallYforStra1;
00217             //
00218             //if(Car2DirectionforStra1 >= 4.2 + NowFaceAngleFromPC) Car2DirectionforStra1 -= PI;
00219             //if(Car2DirectionforStra1 <= -4.2 + NowFaceAngleFromPC) Car2DirectionforStra1 += PI;
00220             /*
00221             while(Car1DirectionforStra1 > 1.8*PI + NowFaceAngleFromPC && WhileCounts <= 20)   
00222             {
00223                 Car2DirectionforStra1 -= 2*PI; //LED_3 = 1;
00224                 WhileCounts++;
00225             }
00226             while(Car1DirectionforStra1 < -1.8*PI + NowFaceAngleFromPC && WhileCounts <= 20)   
00227             {
00228                 Car2DirectionforStra1 += 2*PI; //LED_3 = 1;
00229                 WhileCounts++;
00230             }
00231             */
00232             NowFaceAngleFromPC = Car2DirectionforStra1;// +  angleErrorFromPC ; 
00233         }
00234         else
00235         {
00236             if(FirstReceive) 
00237             {
00238                 NowFaceAngleFromPC = Car1DirectionforStra1;
00239                 FirstReceive = false;
00240             }
00241             Ball_Coordinate[0] = BlueBallXforStra1;
00242             Ball_Coordinate[1] = BlueBallYforStra1;
00243             NowCoordinateFromPC[0] = Car1XforStra1;
00244             NowCoordinateFromPC[1] = Car1YforStra1;     
00245             EnemyCoordinateFromPC[0] = Car2XforStra1;
00246             EnemyCoordinateFromPC[1] = Car2YforStra1;
00247             
00248             Enemy_Ball_Coordinate[0] = GreenBallXforStra1;
00249             Enemy_Ball_Coordinate[1] = GreenBallYforStra1;
00250             /*
00251             while(Car1DirectionforStra1 > 1.8*PI + NowFaceAngleFromPC && WhileCounts <= 20)   
00252             {
00253                 Car1DirectionforStra1 -= 2*PI; //LED_3 = 1;
00254                 WhileCounts++;
00255             }
00256             while(Car1DirectionforStra1 < -1.8*PI + NowFaceAngleFromPC && WhileCounts <= 20)   
00257             {
00258                 Car1DirectionforStra1 += 2*PI; //LED_3 = 1;
00259                 WhileCounts++;
00260             }
00261             */
00262             NowFaceAngleFromPC = Car1DirectionforStra1;// +  angleErrorFromPC ; 
00263         }
00264         //LED_3 = 0;
00265         UpdateBallSpeedCounts++;
00266         if(UpdateBallSpeedCounts >= 50)
00267         {
00268         BallSpeed = sqrt((Ball_Coordinate[0] - Last_Ball_Coordinate[0])*(Ball_Coordinate[0] - Last_Ball_Coordinate[0]) + (Ball_Coordinate[1] - Last_Ball_Coordinate[1])*(Ball_Coordinate[1] - Last_Ball_Coordinate[1]));
00269         Last_Ball_Coordinate[0] = Ball_Coordinate[0];
00270         Last_Ball_Coordinate[1] = Ball_Coordinate[1];
00271         UpdateBallSpeedCounts = 0;
00272         
00273         //count for car speed
00274         double carspeed = sqrt((NowCoordinate[0] - LastCoordinate[0])*(NowCoordinate[0] - LastCoordinate[0]) + (NowCoordinate[1] - LastCoordinate[1])*(NowCoordinate[1] - LastCoordinate[1]));
00275         if(carspeed <= 180) 
00276         {
00277             WatchDogCounts++; 
00278             LED_3 = 1;
00279         } 
00280         else 
00281         {
00282             WatchDogCounts = 0;
00283             LED_3 = 0;
00284         }
00285         LastCoordinate[0] = NowCoordinate[0];
00286         LastCoordinate[1] = NowCoordinate[1];
00287         }
00288         //TargetCoordinate[0] = Goal_1_Coordinate[0] + (Ball_1_Coordinate[0] - Goal_1_Coordinate[0])*1.2;
00289         //TargetCoordinate[1] = Goal_1_Coordinate[1] + (Ball_1_Coordinate[1] - Goal_1_Coordinate[1])*1.2;
00290         //TargetCoordinateValid = true;
00291         //Stra1_SpeedPlannerState = SpeedPlannerTurning; 
00292     }
00293 }
00294 bool IfGotBall()
00295 {
00296     double BallDistance = sqrt((Ball_Coordinate[0] - NowCoordinate[0])*(Ball_Coordinate[0] - NowCoordinate[0]) + (Ball_Coordinate[1] - NowCoordinate[1])*(Ball_Coordinate[1] - NowCoordinate[1]));
00297     double BallDirection = atan2( Ball_Coordinate[1] - NowCoordinate[1], Ball_Coordinate[0] - NowCoordinate[0]);
00298     if((BallDistance < 31) && (abs(BallDirection - NowDirectionInRad) <= 0.3))
00299     {
00300         return true;        
00301     }
00302     else
00303     {
00304         return false;
00305     }    
00306     BallGotTemp = true;
00307 }
00308 bool IfInLeftRestrictedArea()
00309 {
00310     double LeftPointX = 13;
00311     double LeftPointY = 119;
00312     double RightPointX = 305;
00313     double RughtPointY = 115;
00314     double Distance1 = sqrt((LeftPointX - NowCoordinate[0])*(LeftPointX - NowCoordinate[0]) + (LeftPointY - NowCoordinate[1])*(LeftPointY - NowCoordinate[1]));
00315     if(Distance1 <= 50) return true;
00316     else return false;
00317 }
00318 bool IfInRightRestrictedArea()
00319 {
00320     double RightPointX = 305;
00321     double RughtPointY = 115;
00322     double Distance2 = sqrt((RightPointX - NowCoordinate[0])*(RightPointX - NowCoordinate[0]) + (RughtPointY - NowCoordinate[1])*(RughtPointY - NowCoordinate[1]));
00323     if(Distance2 <= 50) return true;
00324     else return false;
00325 }
00326 void Stra1_GetTargetFromAstar(double* Target)
00327 {
00328     
00329 }
00330 void Stra1_GoingForAstarTarget()
00331 {
00332     double _Target[2];
00333     Stra1_GetTargetFromAstar(_Target);
00334     TargetCoordinate[0] = _Target[0];
00335     TargetCoordinate[1] = _Target[1];
00336     if(Stra1_SpeedPlannerState == SpeedPlannerFinishing)
00337     {        
00338         NowGoingTarget++;
00339         Stra1_SpeedPlannerState = SpeedPlannerTurning;
00340     }
00341 }