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++; 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)