loop.h

00001 bool temp = false;
00002 
00003 int UnPause_Counts = 0;
00004 
00005 void loop(void)
00006 {
00007     
00008     //bluetooth.printf("\n");
00009     //bluetooth.printf("   %d", LeftWheelCounter);
00010     //bluetooth.printf("   %d", RightWheelCounter);
00011     //bluetooth.printf("   %d", LastCycleLeftWheelCounter);
00012     //bluetooth.printf("   %d", LastRightWheelCounter);
00013     //bluetooth.printf("   %f", LeftWheelSpeed);
00014     //bluetooth.printf("   %f", RightWheelSpeed);
00015     //pc.printf("\n   %f", LeftWheelPower);
00016     //pc.printf("   %f", RightWheelPower);
00017     //pc.printf("   %f", DesiredLeftWheelSpeed);
00018     //pc.printf("   %f", DesiredRightWheelSpeed);
00019     //pc.printf("\n   %f", DesiredTurningSpeed);
00020     //pc.printf("   %f", AngleError);
00021     
00022     //pc.printf("\n  %d",Stra1_State);
00023     //pc.printf("\n  %d",Stra1_SpeedPlannerState);
00024     
00025     //bluetooth.printf("   %f", TargetAngleInRad);
00026     //bluetooth.printf("   %f", NowCoordinate[0]);
00027     //bluetooth.printf("   %f", NowCoordinate[1]);    
00028     //bluetooth.printf("   %f", NowDirectionInRad);
00029     
00030     //bluetooth.printf("\n%f", NowDirectionInRad);
00031     //bluetooth.printf("   %d", Timer1Counter);
00032     
00033     if(DataValidorNot == true )
00034     {
00035         DataValidorNot = false;
00036         //pc.printf("\n");
00037         /*
00038         pc.printf("\n%f",Car1X);
00039         pc.printf("  %f",Car1Y);
00040         pc.printf("  %f",Car2X);
00041         pc.printf("  %f",Car2Y);        
00042         pc.printf("  %f",BlueBallX);
00043         pc.printf("  %f",BlueBallY);
00044         pc.printf("  %f",GreenBallX);
00045         pc.printf("  %f",GreenBallY);
00046         pc.printf("  %f",Car1DirectionInRad);
00047         pc.printf("  %f",Car2DirectionInRad);
00048         pc.printf("  %c",BluetoothCommand);
00049         
00050         */
00051         
00052         Car1XforStra1 = Car1X;
00053         Car1YforStra1 = Car1Y;
00054         Car2XforStra1 = Car2X;
00055         Car2YforStra1 = Car2Y;
00056         Car1DirectionforStra1 = Car1DirectionInRad;
00057         Car2DirectionforStra1 = Car2DirectionInRad;
00058         BlueBallXforStra1 = BlueBallX;
00059         BlueBallYforStra1 = BlueBallY;
00060         GreenBallXforStra1 = GreenBallX;
00061         GreenBallYforStra1 = GreenBallY;
00062         TargetCoordinateValid = true;
00063         if(BluetoothCommand == 0x01) Pause = true;
00064         else if(BluetoothCommand == 0x00) UnPause_Counts++;//Pause = false;
00065         
00066         if(UnPause_Counts >= 3) 
00067         {
00068             Pause = false;
00069             UnPause_Counts = 0;
00070         }
00071         
00072         //pc.printf("\n  %f",TargetCoordinate[0]);
00073         //pc.printf("  %f",TargetCoordinate[1]);
00074         //pc.printf("  %d",Stra1_State);
00075         //pc.printf("  %f",Goal_1_Coordinate[0]);
00076         //pc.printf("  %f",Goal_1_Coordinate[1]);
00077         //pc.printf("  %d",BluetoothCommand);
00078         //pc.printf("  %d",GREEN_BALL_MODE);
00079        //pc.printf("\n  %f",BallSpeed);
00080        
00081       // pc.printf("\n  %d",WatchDogCounts);
00082         //pc.printf("\n  %f",Car2XforStra1);
00083         //pc.printf("  %f",Car2YforStra1);
00084         //pc.printf("  %f",Car1DirectionforStra1);
00085         //pc.printf("  %f",BlueBallXforStra1);
00086         //pc.printf("  %f",BlueBallYforStra1);
00087         //pc.printf(" \n D: %f",PrintTemp);
00088         //pc.printf("A:  %f",PrintTemp2);
00089     }
00090     //LED_Red = GREEN_BALL_MODE;
00091     LED_2 = GREEN_BALL_MODE;
00092     if(BatteryVot.read() < 0.841) LED_Red = 1;
00093     else LED_Red = 0;
00094     //pc.printf("  %d",GREEN_BALL_MODE);
00095     /*
00096     OutputLeftWheel(LeftWheelPower);
00097     OutputRightWheel(RightWheelPower);
00098     if(temp == true)
00099     {
00100         LeftWheelPower += 0.005;
00101         RightWheelPower += 0.005;
00102     }
00103     else
00104     {
00105         LeftWheelPower -= 0.005;
00106         RightWheelPower -= 0.005;
00107     }
00108     if(LeftWheelPower >= 0.5)  temp = false;
00109     if(LeftWheelPower <= -0.5) temp = true;
00110     
00111     */
00112     Stra1_Nrtloop();
00113     GetBluetoothData();
00114     
00115     
00116     
00117 }