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
}
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)