五力全開 / Mbed 2 deprecated Robotics_test1

ll

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Revision 7:1818632c0b60, committed 2016-06-22

Comitter:
Nthu_Five
Date:
Wed Jun 22 07:21:16 2016 +0000
Parent:
6:434bb72ffc26
Commit message:
The final competition

Changed in this revision

Stra1_Shooting.h Show annotated file Show diff for this revision Revisions of this file
Stra1_SpeedPlanner.h Show annotated file Show diff for this revision Revisions of this file
WatchDog.h Show annotated file Show diff for this revision Revisions of this file
loop.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
strategy1.h Show annotated file Show diff for this revision Revisions of this file 
--- a/Stra1_Shooting.h	Thu Jun 16 20:18:42 2016 +0000
+++ b/Stra1_Shooting.h	Wed Jun 22 07:21:16 2016 +0000
@@ -140,33 +140,33 @@
         TargetNumber = 3;
         TargetX[0] = MiddleLength;
         TargetY[0] = MiddleWidth * 0.5;
-        TargetX[1] = Goal_1_Coordinate[0]*0.7;///2*MiddleLength;
-        TargetY[1] = Goal_1_Coordinate[1];///MiddleWidth;
-        TargetX[2] = Goal_1_Coordinate[0]*0.85;///2*MiddleLength;
-        TargetY[2] = Goal_1_Coordinate[1];///MiddleWidth;
+        TargetX[1] = 229;//Goal_1_Coordinate[0]*0.7;///2*MiddleLength;
+        TargetY[1] = 90;//Goal_1_Coordinate[1];///MiddleWidth;
+        TargetX[2] = 267;//Goal_1_Coordinate[0]*0.85;///2*MiddleLength;
+        TargetY[2] = 105;//Goal_1_Coordinate[1];///MiddleWidth;
         break;
         case 2:
         TargetNumber = 2;
-        TargetX[0] = Goal_1_Coordinate[0]*0.7;
-        TargetY[0] = Goal_1_Coordinate[1];
-        TargetX[1] = Goal_1_Coordinate[0]*0.85;///2*MiddleLength;
-        TargetY[1] = Goal_1_Coordinate[1];///MiddleWidth;
+        TargetX[0] = 229;//Goal_1_Coordinate[0]*0.7;
+        TargetY[0] = 90;//Goal_1_Coordinate[1];
+        TargetX[1] = 267;//Goal_1_Coordinate[0]*0.85;///2*MiddleLength;
+        TargetY[1] = 105;//Goal_1_Coordinate[1];///MiddleWidth;
         break;
         case 3:
         TargetNumber = 3;
         TargetX[0] = MiddleLength;
         TargetY[0] = MiddleWidth * 1.5;
-        TargetX[1] = Goal_1_Coordinate[0]*0.7;///2*MiddleLength;
-        TargetY[1] = Goal_1_Coordinate[1];///MiddleWidth;
-        TargetX[2] = Goal_1_Coordinate[0]*0.85;///2*MiddleLength;
-        TargetY[2] = Goal_1_Coordinate[1];///MiddleWidth;
+        TargetX[1] = 229;//Goal_1_Coordinate[0]*0.7;///2*MiddleLength;
+        TargetY[1] = 150;//Goal_1_Coordinate[1];///MiddleWidth;
+        TargetX[2] = 267;//Goal_1_Coordinate[0]*0.85;///2*MiddleLength;
+        TargetY[2] = 135;//Goal_1_Coordinate[1];///MiddleWidth;
         break;
         case 4:
         TargetNumber = 2;
-        TargetX[0] = Goal_1_Coordinate[0]*0.7;
-        TargetY[0] = Goal_1_Coordinate[1];
-        TargetX[1] = Goal_1_Coordinate[0]*0.85;///2*MiddleLength;
-        TargetY[1] = Goal_1_Coordinate[1];///MiddleWidth;
+        TargetX[0] = 229;//Goal_1_Coordinate[0]*0.7;
+        TargetY[0] = 150;//Goal_1_Coordinate[1];
+        TargetX[1] = 267;//Goal_1_Coordinate[0]*0.85;///2*MiddleLength;
+        TargetY[1] = 135;//Goal_1_Coordinate[1];///MiddleWidth;
         break;
         default:
         break;

--- a/Stra1_SpeedPlanner.h	Thu Jun 16 20:18:42 2016 +0000
+++ b/Stra1_SpeedPlanner.h	Wed Jun 22 07:21:16 2016 +0000
@@ -8,7 +8,7 @@
         
 
 double ForwardCounts = 0, BackwardCounts = 0;
-
+double ForwardMovingCounts = 0;
 void Stra1_SpeedPlanner()
 {
     switch(Stra1_SpeedPlannerState)
@@ -89,22 +89,24 @@
      //   DesiredTurningSpeed -= 0.02;//0.0005
     //    //pc.printf("+");
     //}
-    DesiredTurningSpeed = AngleError * -1.8;//-1.2;
+    DesiredTurningSpeed = AngleError * -1.2;
     if(DesiredTurningSpeed > 2) DesiredTurningSpeed = 2;
     if(DesiredTurningSpeed < -2) DesiredTurningSpeed -2;
      
-    if(abs(AngleError) <= 0.5)
+    if(abs(AngleError) <= 0.6)
     {
         //pc.printf("%f", DesiredTurningSpeed);
         DesiredTurningSpeed = 0;
         //DesiredMovingSpeed  = 0;
         Stra1_SpeedPlannerState = SpeedPlannerMoving;
         //pc.printf("Reach");
+        ForwardMovingCounts = 0;
     }
     //bluetooth.printf("\n%f", AngleError);
 }
 void Stra1_SpeedPlanner_Movinging()
 {
+    ForwardMovingCounts++;
     /*
     if(Stra1_State == Stra1_Shooting ) 
     {
@@ -135,16 +137,22 @@
     }
     */
     
+    
     double Target_Distance = sqrt((TargetCoordinate[1] - NowCoordinate[1])*(TargetCoordinate[1] - NowCoordinate[1]) + (TargetCoordinate[0] - NowCoordinate[0])*(TargetCoordinate[0] - NowCoordinate[0]));
-    DesiredMovingSpeed = Target_Distance * 0.4 + 25 + TargetSpeed * 1;
-    DesiredTurningSpeed = AngleError * -2 * DesiredMovingSpeed* 0.01;// -1.1;
+    DesiredMovingSpeed = Target_Distance * 0.35 + 25 + TargetSpeed * 1;
+    DesiredTurningSpeed = AngleError * -2 * DesiredMovingSpeed* 0.0155;// -1.1;
     if(abs(Target_Distance) < 40 && Stra1_State != Stra1_GettingBall)
     {
         DesiredMovingSpeed = 0;
         DesiredTurningSpeed = 0;
         Stra1_SpeedPlannerState = SpeedPlannerFinishing;
     }
-    
+    if(ForwardCounts >= 500)
+    {
+        ForwardMovingCounts = 0;
+        Stra1_SpeedPlannerState = SpeedPlannerIdle;
+        Stra1_State = Stra1_Idle;
+    }
 }
 
 void Stra1_SpeedPlanner_Finishing()

--- a/WatchDog.h	Thu Jun 16 20:18:42 2016 +0000
+++ b/WatchDog.h	Wed Jun 22 07:21:16 2016 +0000
@@ -1,6 +1,6 @@
 bool WatchDogCheck(int Count)
 {
-    if(Count >= 15)
+    if(0)//Count >= 8)
     {
         
         return true;

--- a/loop.h	Thu Jun 16 20:18:42 2016 +0000
+++ b/loop.h	Wed Jun 22 07:21:16 2016 +0000
@@ -1,6 +1,6 @@
 bool temp = false;
 
-
+int UnPause_Counts = 0;
 
 void loop(void)
 {
@@ -61,7 +61,14 @@
         GreenBallYforStra1 = GreenBallY;
         TargetCoordinateValid = true;
         if(BluetoothCommand == 0x01) Pause = true;
-        else if(BluetoothCommand == 0x00) Pause = false;
+        else if(BluetoothCommand == 0x00) UnPause_Counts++;//Pause = false;
+        
+        if(UnPause_Counts >= 3) 
+        {
+            Pause = false;
+            UnPause_Counts = 0;
+        }
+        
         //pc.printf("\n  %f",TargetCoordinate[0]);
         //pc.printf("  %f",TargetCoordinate[1]);
         //pc.printf("  %d",Stra1_State);
@@ -71,6 +78,7 @@
         //pc.printf("  %d",GREEN_BALL_MODE);
        //pc.printf("\n  %f",BallSpeed);
        
+      // pc.printf("\n  %d",WatchDogCounts);
         //pc.printf("\n  %f",Car2XforStra1);
         //pc.printf("  %f",Car2YforStra1);
         //pc.printf("  %f",Car1DirectionforStra1);

--- a/main.cpp	Thu Jun 16 20:18:42 2016 +0000
+++ b/main.cpp	Wed Jun 22 07:21:16 2016 +0000
@@ -7,7 +7,7 @@
 
 Serial pc(SERIAL_TX, SERIAL_RX);
 DigitalIn Switch_0(D4);
-DigitalIn Switch_1(D1);
+DigitalIn Switch_1(D3);
 DigitalOut LED_Red(D5);
 DigitalOut LED_2(A4);
 DigitalOut LED_3(A5);

--- a/strategy1.h	Thu Jun 16 20:18:42 2016 +0000
+++ b/strategy1.h	Wed Jun 22 07:21:16 2016 +0000
@@ -99,14 +99,14 @@
         case Stra1_Idle:
         Stra1_SpeedPlannerState = SpeedPlannerIdle;
         //
-        double MiddleX = FieldLength/2;
-        double MiddleY = FieldWidth/2;
+        double MiddleX = 165;
+        double MiddleY = 116;
         double ToBallAngle = atan2( Ball_Coordinate[1] - NowCoordinate[1], Ball_Coordinate[0] - NowCoordinate[0]);
         double ToBallDistance = sqrt((Ball_Coordinate[1] - NowCoordinate[1])*(Ball_Coordinate[1] - NowCoordinate[1]) + (Ball_Coordinate[0] - NowCoordinate[0])*(Ball_Coordinate[0] - NowCoordinate[0]));
         double MiddleMotorAngle = atan2( MiddleY - NowCoordinate[1], MiddleX - NowCoordinate[0]);
         double ToMiddleMotorDistance = sqrt((MiddleY - NowCoordinate[1])*(MiddleY - NowCoordinate[1]) + (MiddleX - NowCoordinate[0])*(MiddleX - NowCoordinate[0]));
         double AngleBetween = MiddleMotorAngle - ToBallAngle;
-        if(abs(AngleBetween) <= 0.25 && ToMiddleMotorDistance < ToBallDistance)  NotBlocked = false;
+        if(abs(AngleBetween) <= 0.2 && ToMiddleMotorDistance < ToBallDistance)  NotBlocked = false;
         else NotBlocked = true;
         //
         if(TargetCoordinateValid ==  true && Pause == false/*&& StartTemp == false*/ && NotBlocked)// (Ball_Coordinate[0] < MiddleLength*1.1))
@@ -195,6 +195,7 @@
 {
     //if(DataValidorNot == true)
     {
+        int WhileCounts = 0;
         //TargetCoordinateValid = false;
         //DataValidorNot = false;
         if(GREEN_BALL_MODE == true)
@@ -216,8 +217,18 @@
             //
             //if(Car2DirectionforStra1 >= 4.2 + NowFaceAngleFromPC) Car2DirectionforStra1 -= PI;
             //if(Car2DirectionforStra1 <= -4.2 + NowFaceAngleFromPC) Car2DirectionforStra1 += PI;
-            while(Car1DirectionforStra1 > 1.8*PI + NowFaceAngleFromPC)   Car2DirectionforStra1 -= 2*PI; LED_3 = 1;
-            while(Car1DirectionforStra1 < -1.8*PI + NowFaceAngleFromPC)   Car2DirectionforStra1 += 2*PI; LED_3 = 1;
+            /*
+            while(Car1DirectionforStra1 > 1.8*PI + NowFaceAngleFromPC && WhileCounts <= 20)   
+            {
+                Car2DirectionforStra1 -= 2*PI; //LED_3 = 1;
+                WhileCounts++;
+            }
+            while(Car1DirectionforStra1 < -1.8*PI + NowFaceAngleFromPC && WhileCounts <= 20)   
+            {
+                Car2DirectionforStra1 += 2*PI; //LED_3 = 1;
+                WhileCounts++;
+            }
+            */
             NowFaceAngleFromPC = Car2DirectionforStra1;// +  angleErrorFromPC ; 
         }
         else
@@ -236,12 +247,21 @@
             
             Enemy_Ball_Coordinate[0] = GreenBallXforStra1;
             Enemy_Ball_Coordinate[1] = GreenBallYforStra1;
-            
-            while(Car1DirectionforStra1 > 1.8*PI + NowFaceAngleFromPC)   Car1DirectionforStra1 -= 2*PI; LED_3 = 1;
-            while(Car1DirectionforStra1 < -1.8*PI + NowFaceAngleFromPC)   Car1DirectionforStra1 += 2*PI; LED_3 = 1;
+            /*
+            while(Car1DirectionforStra1 > 1.8*PI + NowFaceAngleFromPC && WhileCounts <= 20)   
+            {
+                Car1DirectionforStra1 -= 2*PI; //LED_3 = 1;
+                WhileCounts++;
+            }
+            while(Car1DirectionforStra1 < -1.8*PI + NowFaceAngleFromPC && WhileCounts <= 20)   
+            {
+                Car1DirectionforStra1 += 2*PI; //LED_3 = 1;
+                WhileCounts++;
+            }
+            */
             NowFaceAngleFromPC = Car1DirectionforStra1;// +  angleErrorFromPC ; 
         }
-        LED_3 = 0;
+        //LED_3 = 0;
         UpdateBallSpeedCounts++;
         if(UpdateBallSpeedCounts >= 50)
         {
@@ -252,8 +272,16 @@
         
         //count for car speed
         double carspeed = sqrt((NowCoordinate[0] - LastCoordinate[0])*(NowCoordinate[0] - LastCoordinate[0]) + (NowCoordinate[1] - LastCoordinate[1])*(NowCoordinate[1] - LastCoordinate[1]));
-        if(carspeed <= 8) WatchDogCounts++;
-        else WatchDogCounts = 0;
+        if(carspeed <= 180) 
+        {
+            WatchDogCounts++; 
+            LED_3 = 1;
+        } 
+        else 
+        {
+            WatchDogCounts = 0;
+            LED_3 = 0;
+        }
         LastCoordinate[0] = NowCoordinate[0];
         LastCoordinate[1] = NowCoordinate[1];
         }