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)
void Periodic(EntropyDrive& MyRobot, DriverStationLCD* ds)
{
unsigned int ctrs = (unsigned)(Reverse? -1 : 1) * MyRobot.GetCounters();
ds->PrintfLine(DriverStationLCD::kUser_Line2, "c%d d%d t%d ",ctrs, Distance, t);
if (ctrs < (Distance - t))
MyRobot.DriveRobot(Reverse?1.0:-1.0, 0.0, ds);
else
MyRobot.DriveRobot(0.0, 0.0, ds);
if (t == 0)
{
if (MyRobot.AtTopSpeed()) t = ctrs;
if (ctrs > Distance/2) t = ctrs;
// if t is just way bigger than we meant to travel, set it to the distance (which will stop us)
if (t >= Distance) t = Distance;
}
}
void RobotInit(void) {
// Actions which would be performed once (and only once) upon initialization of the
// robot would be put here.
// Initialize SHS Subsystems here
MyRobot.Initialize();
Arm.Initialize();
printf("RobotInit() completed.\n");
}
void TeleopPeriodic(void) {
// increment the number of teleop periodic loops completed
m_telePeriodicLoops++;
//Feed joystick inputs to each subsystem here
//Using triggers to turn;
ds->Printf(DriverStationLCD::kUser_Line1,0, "GetY: %f",DriveStick->GetY());
ds->Printf(DriverStationLCD::kUser_Line2,0, "GetZ: %f",DriveStick->GetZ());
MyRobot.DriveRobot(DriveStick->GetY(),(-DriveStick->GetZ()));
//original:MyRobot.DriveRobot(DriveStick->GetY(),DriveStick->GetX());
//MyRobot.DriveRobotTrig(DriveStick->GetY(),DriveStick->GetX());
} // TeleopPeriodic(void)
void TeleopPeriodic(void) {
// increment the number of teleop periodic loops completed
m_telePeriodicLoops++;
//Feed joystick inputs to each subsystem here
MyRobot.DriveRobot(DriveStick->GetY(),DriveStick->GetX());
Arm.Update();
Arm.UpperVerticalPos(GameStick);
Arm.LowerVerticalPos(GameStick);
Arm.Extend(GameStick);
Arm.BeltEnable(GameStick);
//MyRobot.DriveRobotTrig(DriveStick->GetY(),DriveStick->GetX());
// DriverStationLCD::GetInstance()->Printf(DriverStationLCD::kUser_Line1, 1, "Sensor output: %f", InfraredSensor.GetValue());
// DriverStationLCD::GetInstance()->UpdateLCD();
} // TeleopPeriodic(void)
void DisabledPeriodic(void) {
static INT32 printSec = (INT32)GetClock() + 1;
static const INT32 startSec = (INT32)GetClock();
// increment the number of disabled periodic loops completed
m_disabledPeriodicLoops++;
//Disable Drive
MyRobot.Cleanup();
//MyShooter.Cleanup();
// while disabled, printout the duration of current disabled mode in seconds
if (GetClock() > printSec) {
// Move the cursor back to the previous line and clear it.
printf("\x1b[1A\x1b[2K");
printf("Disabled seconds: %d\r\n", printSec - startSec);
printSec++;
}
}