Ejemplo n.º 1
0
	void AutonomousPeriodic(void) {
		m_autoPeriodicLoops++;
#if 0
		static int Clock=0;
		bool correct = DriveStick->GetButton(Joystick::kTriggerButton);
		bool Reset = DriveStick->GetButton (Joystick::kTopButton);
		ds->PrintfLine(DriverStationLCD::kUser_Line1, "%s %s",
				correct ? "correct on" : "correct off",
						Reset ? "Reset": "No Reset");
		//ds->PrintfLine(DriverStationLCD::kUser_Line6, "%d %c %c", Clock, correct? "C" : "c", Reset? "R" : "r");
		if (Reset)
			Clock=0, MyRobot.ResetCounters();
		else
		{
		    ++Clock;
		    if(Clock<=100)       MyRobot.DriveRobot(1.0,0.0, ds, correct);		// drive forward
		    else if (Clock<=200) MyRobot.DriveRobot(-1.0,0.0, ds, correct);  // stop
		    else if (Clock<=250) MyRobot.DriveRobot(-1.0,0.0, ds, correct);  // drive back halfway
		    else if (Clock<=300) MyRobot.DriveRobot(1.0,0.0, ds, correct);   // stop
		    else
		    {
		    	// Real teleop mode: use the JoySticks to drive
		    	MyRobot.DriveRobot(DriveStick->GetY(),(DriveStick->GetX()), ds);
		    }
		}
#endif
	}
Ejemplo n.º 2
0
	void TeleopPeriodic(void) {
		// increment the number of teleop periodic loops completed
		m_telePeriodicLoops++;
		static AutoDrive *autoDrive = NULL;
		bool autoButton = DriveStick->GetButton(Joystick::kTriggerButton);
		if (autoButton)
		{
			if (autoDrive == NULL)
				autoDrive = new AutoDrive(m_Configuration->GetValue( m_Constant[ cAutoDrive]) * 100);
			autoDrive->Periodic(MyRobot, ds);
			ds->PrintfLine (DriverStationLCD::kUser_Line5, "autoDrive on");
		}
		else
		{
			ds->PrintfLine (DriverStationLCD::kUser_Line5, "autoDrive off");
			if (autoDrive != NULL)
			{
				MyRobot.ResetCounters();
				delete autoDrive;
				autoDrive = NULL;
			}

			if( !m_Configuration)
			{
				printf( "Configuration Initialize");
				InitializeConfiguration();
			}
			
			m_Configuration->Execute( DriveStick->GetRawButton( 2), DriveStick->GetZ(), ds);
			
			if(DriveStick->GetRawButton( 3))
			{
				ds->PrintfLine(DriverStationLCD::kUser_Line6, "Calculating distance...");
				Vision *vision = new Vision();
				double distance = vision->TakeDistancePicture( ds, m_Configuration->GetValue( m_Constant[ cHBottom]), m_Configuration->GetValue( m_Constant[ cHTop]), m_Configuration->GetValue( m_Constant[ cSBottom]), m_Configuration->GetValue( m_Constant[ cSTop]), m_Configuration->GetValue( m_Constant[ cVBottom]), m_Configuration->GetValue( m_Constant[ cVTop]));
				if( distance < 0.000001)
					ds->PrintfLine(DriverStationLCD::kUser_Line6, "No target found");
				else
					ds->PrintfLine(DriverStationLCD::kUser_Line6, "distance to target: %lf",distance);
				delete vision;
			}

			
			// Real teleop mode: use the JoySticks to drive
				MyRobot.DriveRobot(DriveStick->GetY(),(DriveStick->GetX()), ds);
		}

    	ds->UpdateLCD();
	} // TeleopPeriodic(void)