Stra1_SpeedPlanner.h

00001 
00002 
00003 
00004 
00005 double PrintTemp;
00006 double PrintTemp2;
00007 int BackTargetCount = 0;
00008         
00009 
00010 double ForwardCounts = 0, BackwardCounts = 0;
00011 double ForwardMovingCounts = 0;
00012 void Stra1_SpeedPlanner()
00013 {
00014     switch(Stra1_SpeedPlannerState)
00015     {
00016         case SpeedPlannerIdle:        
00017         DesiredTurningSpeed = 0;
00018         DesiredMovingSpeed = 0;     
00019         //bluetooth.printf("000000000000000");
00020         Stra1_SpeedPlannerState = SpeedPlannerTurning;   
00021         break;
00022         case SpeedPlannerTurning:
00023         Stra1_SpeedPlanner_Turning();
00024         //bluetooth.printf("11111111111111");
00025         break;
00026         case SpeedPlannerMoving:
00027         Stra1_SpeedPlanner_Movinging();
00028         //bluetooth.printf("2222222222222222");
00029         break;       
00030         case SpeedPlannerFinishing:
00031         Stra1_SpeedPlanner_Finishing();
00032         //bluetooth.printf("333333333333333");
00033         break;
00034         case SpeedPlannerForwardForWhile:
00035         DesiredMovingSpeed = 50;
00036         ForwardCounts++;        
00037         if(ForwardCounts >= 100) 
00038         {
00039             DesiredMovingSpeed = 0;
00040             Stra1_SpeedPlannerState = SpeedPlannerFinishing;   
00041             ForwardCounts = 0;
00042         }
00043         break;
00044         case SpeedPlannerBackwardForWhile:
00045         DesiredMovingSpeed = -50;
00046         BackwardCounts++;        
00047         if(BallSpeed >= 3) BackTargetCount = 50; 
00048         else BackTargetCount = 0; 
00049         if(BackwardCounts >= BackTargetCount)//BallSpeed * 10) //if(BackwardCounts >= 100) 
00050         {
00051             DesiredMovingSpeed = 0;
00052             Stra1_SpeedPlannerState = SpeedPlannerFinishing;   
00053             BackwardCounts = 0;
00054         }
00055         break;
00056         default:
00057         break;
00058     }
00059     TransToDesiredWheelsSpeed(DesiredMovingSpeed, DesiredTurningSpeed);
00060     
00061     double BallDirection = atan2( Ball_Coordinate[1] - NowCoordinate[1], Ball_Coordinate[0] - NowCoordinate[0]);
00062     PrintTemp2 = BallDirection - NowDirectionInRad;  
00063     double TargetDistance = sqrt( (TargetCoordinate[1] - NowCoordinate[1])*(TargetCoordinate[1] - NowCoordinate[1]) + (TargetCoordinate[0] - NowCoordinate[0])*(TargetCoordinate[0] - NowCoordinate[0]));
00064     PrintTemp = TargetDistance;
00065 }
00066 
00067 double AngleError;
00068 
00069 
00070 void Stra1_SpeedPlanner_Turning()
00071 {
00072     /*
00073     if(Stra1_State == Stra1_Shooting ) 
00074     {
00075         TargetCoordinate[0] = Goal_1_Coordinate[0];
00076         TargetCoordinate[1] = Goal_1_Coordinate[1];
00077     }
00078     */
00079     double TargetAngleInRad = atan2( TargetCoordinate[1] - NowCoordinate[1], TargetCoordinate[0] - NowCoordinate[0]);
00080     AngleError = TargetAngleInRad - NowDirectionInRad;  
00081     
00082     //if(/*DesiredTurningSpeed <= 1 && */AngleError < 0)   
00083     //{
00084     //    DesiredTurningSpeed += 0.02;
00085     //    //pc.printf("+");
00086    // }
00087     //else if(/*DesiredTurningSpeed >= -1 &&*/ AngleError > 0) 
00088     //{
00089      //   DesiredTurningSpeed -= 0.02;//0.0005
00090     //    //pc.printf("+");
00091     //}
00092     DesiredTurningSpeed = AngleError * -1.2;
00093     if(DesiredTurningSpeed > 2) DesiredTurningSpeed = 2;
00094     if(DesiredTurningSpeed < -2) DesiredTurningSpeed -2;
00095      
00096     if(abs(AngleError) <= 0.6)
00097     {
00098         //pc.printf("%f", DesiredTurningSpeed);
00099         DesiredTurningSpeed = 0;
00100         //DesiredMovingSpeed  = 0;
00101         Stra1_SpeedPlannerState = SpeedPlannerMoving;
00102         //pc.printf("Reach");
00103         ForwardMovingCounts = 0;
00104     }
00105     //bluetooth.printf("\n%f", AngleError);
00106 }
00107 void Stra1_SpeedPlanner_Movinging()
00108 {
00109     ForwardMovingCounts++;
00110     /*
00111     if(Stra1_State == Stra1_Shooting ) 
00112     {
00113         TargetCoordinate[0] = Goal_1_Coordinate[0];
00114         TargetCoordinate[1] = Goal_1_Coordinate[1];
00115     }*/
00116     
00117     double TargetAngle_InRad = atan2( TargetCoordinate[1] - NowCoordinate[1], TargetCoordinate[0] - NowCoordinate[0]);
00118     double AngleError = TargetAngle_InRad - NowDirectionInRad;  
00119     /*
00120     if(DesiredTurningSpeed < 1 && AngleError < 0)   
00121     {
00122         DesiredTurningSpeed += 0.03;
00123         //pc.printf("+");
00124     }
00125     else if(DesiredTurningSpeed > -1 && AngleError > 0) 
00126     {
00127         DesiredTurningSpeed -= 0.03;
00128         //pc.printf("+");
00129     }
00130     */
00131     //DesiredTurningSpeed = AngleError * -2;
00132     /////////////////////////////////
00133     /*
00134     if(DesiredMovingSpeed < 200)  
00135     {
00136         DesiredMovingSpeed += 2;
00137     }
00138     */
00139     
00140     
00141     double Target_Distance = sqrt((TargetCoordinate[1] - NowCoordinate[1])*(TargetCoordinate[1] - NowCoordinate[1]) + (TargetCoordinate[0] - NowCoordinate[0])*(TargetCoordinate[0] - NowCoordinate[0]));
00142     DesiredMovingSpeed = Target_Distance * 0.35 + 25 + TargetSpeed * 1;
00143     DesiredTurningSpeed = AngleError * -2 * DesiredMovingSpeed* 0.0155;// -1.1;
00144     if(abs(Target_Distance) < 40 && Stra1_State != Stra1_GettingBall)
00145     {
00146         DesiredMovingSpeed = 0;
00147         DesiredTurningSpeed = 0;
00148         Stra1_SpeedPlannerState = SpeedPlannerFinishing;
00149     }
00150     if(ForwardCounts >= 500)
00151     {
00152         ForwardMovingCounts = 0;
00153         Stra1_SpeedPlannerState = SpeedPlannerIdle;
00154         Stra1_State = Stra1_Idle;
00155     }
00156 }
00157 
00158 void Stra1_SpeedPlanner_Finishing()
00159 {
00160     
00161     DesiredTurningSpeed = 0;
00162     DesiredMovingSpeed = 0; 
00163     //Stra1_SpeedPlannerState = SpeedPlannerTurning;
00164 }