void teleDrive()
{
if(!isWait)
{
drive->setLinVelocity(oi->getDriveJoystick()->GetY(Joystick::kRightHand));
drive->setTurnSpeed(oi->getDriveJoystick()->GetX(Joystick::kRightHand), oi->getDriveJoystickButton(3));
drive->drive();
drive->shift(oi->getDriveJoystickButton(8), oi->getDriveJoystickButton(9));
manipulator->moveArm(oi->getManipJoystickButton(6), oi->getManipJoystickButton(7));
manipulator->intakeBall(oi->getManipJoystickButton(3), oi->getManipJoystickButton(2), (oi->getManipJoystick()->GetThrottle()+1)/2);
launcher->launchBall(oi->getDriveJoystickButton(1), oi->getDriveJoystickButton(2), oi->getDriveJoystickButton(10), oi->getDriveJoystickButton(11));
toggleCompressor(oi->getDriveJoystickButton(6), oi->getDriveJoystickButton(7));
}
//camera motor mount
if(oi->getManipJoystickButton(10))
{
bCameraLatch = true;
}
else if(oi->getManipJoystickButton(11))
{
bCameraLatch = false;
}
manipulator->toggleCameraPosition(bCameraLatch);
}
/**
* Periodic code for teleop mode should go here.
*
* Use this method for code which will be called periodically at a regular
* rate while the robot is in teleop mode.
*/
void RobotDemo::TeleopPeriodic() {
/*ALTERNATE OPTION: use constants at the top of the code to pass in buttons associated
* with various actions.
*
*FOR ALTERNATE LAYOUTS:
*have one set of layouts commented out?
*take input from Driver Station I/O tab to determine which set is used?
* */
//driver joystick: Extreme 3D
bool compressor = stickDrive->GetRawButton(10);
//drive-related
bool stopDrive = stickDrive->GetRawButton(1);
bool slowSpeed = stickDrive->GetRawButton(2);
//arm controls--solenoids involved
bool extendAll_driver = stickDrive->GetRawButton(9);
bool retractAll_driver = stickDrive->GetRawButton(11);
//spinner controls
bool leftDrop_driver = stickDrive->GetRawButton(3);
bool leftPick_driver = stickDrive->GetRawButton(5);
bool rightDrop_driver = stickDrive->GetRawButton(4);
bool rightPick_driver = stickDrive->GetRawButton(6);
//other joystick: Attack 3
//arm controls--solenoids involved
bool extendAll_other = stickOther->GetRawButton(3);
bool retractAll_other = stickOther->GetRawButton(2);
bool leftArm_other = (stickOther->GetRawButton(6) || stickOther->GetRawButton(10));
bool rightArm_other = (stickOther->GetRawButton(7) || stickOther->GetRawButton(11));
//spinner controls
bool leftDrop_other = (stickOther->GetRawButton(4) && !stickOther->GetRawButton(1));
bool leftPick_other = (stickOther->GetRawButton(4) && stickOther->GetRawButton(1));
bool rightDrop_other = (stickOther->GetRawButton(5) && !stickOther->GetRawButton(1));
bool rightPick_other = (stickOther->GetRawButton(5) && stickOther->GetRawButton(1));
SmartDashboard::PutBoolean("Left-Spinner", spinnerStatusLeft);
SmartDashboard::PutBoolean("Right-Spinner", spinnerStatusLeft);
SmartDashboard::PutBoolean("S1-Status", s1Status);
SmartDashboard::PutBoolean("S2-Status", s2Status);
SmartDashboard::PutBoolean("S3-Status", s3Status);
SmartDashboard::PutNumber("Compressor-Pressure", this->compressor->GetPressureSwitchValue());
if(compressor) {
toggleCompressor();
}
++periodicCounter;
float x = stickDrive->GetX();
float y = stickDrive->GetY();
float z = (stickDrive->GetRawAxis(3)) / 1.5; //actually twist--capped at 2/3 for speed control
if(abs(x) < .125) x =0;
if(abs(y) < .125) y =0;
if(abs(z) < .125) z =0;
if(!slowSpeed) {
//myRobot.MecanumDrive_Cartesian(x, y, z, 0.0);
MecanumDrive(x, y, z, 0.0);
}
else if(slowSpeed) {
if (x < 0) x = x/3;
else x = x/3;
if (y < 0) y = y/3;
else y = y/3;
if (z < 0) z = z/3;
else z = z/3;
//myRobot.MecanumDrive_Cartesian(x, y, z, 0.0);
MecanumDrive(x, y, z, 0.0);
}
//stop robot
if (stopDrive){
stop();
return;
}
//extend all arms
if(extendAll_driver){
extend(s1,s1Status); //spinner arms go out first
extend(s3,s3Status);
Wait(.2);
extend(s2,s2Status);
return;
}
//retract all arms
if(retractAll_driver){
retract(s2,s2Status); //catcher arms come in first
Wait(.2);
retract(s3,s3Status);
retract(s1,s1Status);
return;
}
//toggle left set of spinners
if(leftDrop_driver){
toggleLeftDrop(SPINNER_SPEED);
return;
}
if(leftPick_driver){
toggleLeftPick(SPINNER_SPEED);
return;
}
//toggle right set of spinners
if(rightDrop_driver){
toggleRightDrop(SPINNER_SPEED);
return;
}
if(rightPick_driver){
toggleRightPick(SPINNER_SPEED);
return;
}
//attack3 joystick code below
//extend all arms
if(extendAll_other){
extend(s1,s1Status); //spinner arms go out first
extend(s3,s3Status);
Wait(.2);
extend(s2,s2Status);
Wait(0.5);
return;
}
//retract all arms
if(retractAll_other){
retract(s2,s2Status); //catcher arms come in first
Wait(.2);
retract(s3,s3Status);
retract(s1,s1Status);
Wait(0.5);
return;
}
if(leftDrop_other){
spinnerL1->Set(SPINNER_SPEED); //may need to
spinnerL2->Set(SPINNER_SPEED); //reverse these
return;
}
if(leftPick_other){
spinnerL1->Set(-SPINNER_SPEED); //may need to
spinnerL2->Set(-SPINNER_SPEED); //reverse these
return;
}
//toggle right set of spinners
if(rightDrop_other){
spinnerR1->Set(SPINNER_SPEED);
spinnerR2->Set(SPINNER_SPEED);
return;
}
if(rightPick_other){
spinnerR1->Set(-SPINNER_SPEED);
spinnerR2->Set(-SPINNER_SPEED);
return;
}
if (!(leftDrop_driver || leftPick_driver || leftDrop_other || leftPick_other))
{
stopSpinnersLeft();
}
if (!(rightDrop_driver || rightPick_driver || rightDrop_other || rightPick_other))
{
stopSpinnersRight();
}
//toggle left spinner arm
if(leftArm_other) {
toggle(s1,s1Status);
return;
}
//toggle right spinner arm
if(rightArm_other) {
toggle(s3,s3Status);
return;
}
}
