/** * 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() { drive->TeleopDrive(driverPad->GetLeftY() *DRIVER_LEFT_DIRECTION, driverPad->GetRightY() *DRIVER_RIGHT_DIRECTION); shooter->handle(); shooter->debug(); //shooter->setShooterPower(operatorPad->GetRightY()); if((intakeMode == IN_INTAKE_MANUAL_MODE) && operatorPad->GetNumberedButton(INTAKE_AUTO_MODE_BUTTON)) { intakeMode = IN_INTAKE_AUTO_MODE; printf ("set into auto roller mode"); intake->setExtenderMode(IN_INTAKE_AUTO_MODE); } //sets intake mode to auto if previously in manual and operator presses auto button else if((intakeMode == IN_INTAKE_AUTO_MODE) && operatorPad->GetNumberedButton(INTAKE_MANUAL_MODE_BUTTON)) { intakeMode = IN_INTAKE_MANUAL_MODE; printf ("set into manual roller mode"); intake->setExtenderMode(IN_INTAKE_MANUAL_MODE); } intake->handle(); if(intakeMode == IN_INTAKE_MANUAL_MODE) { intake->move(operatorPad->GetLeftY()*OPERATOR_LEFT_DIRECTION); } else { //Extender DPad control curPadDir = operatorPad->GetDPad(); if(lastPadDir != curPadDir) { switch(curPadDir) { case Gamepad::kUp: intake->setExtenderTarget(FULL_EXTEND_DISTANCE); break; case Gamepad::kDown: intake->setExtenderTarget(FULL_RETRACT_DISTANCE); break; default: break; } } lastPadDir = curPadDir; } //intake->spinRoller(operatorPad->GetRightY()*OPERATOR_RIGHT_DIRECTION); intake->getCurrentPosition(); //if operator taps in button roller continually spins inward if(operatorPad->GetNumberedButton(ROLLER_IN_BUTTON)) { intakeCont = INTAKE_ROLLER_IN_STATE; } //if operator taps stop button roller stops continually else if(operatorPad->GetNumberedButton(ROLLER_OFF_BUTTON)) { intakeCont = INTAKE_ROLLER_STOP_STATE; } //if operator holds spit button roller spins backwards no matter what if(operatorPad->GetNumberedButton(ROLLER_OUT_BUTTON)) { intake->spinRoller(INTAKE_ROLLER_SPIT_POWER); } //not holding spit button else { //roller either spins or is stopped depending on previous code if(intakeCont) { intake->spinRoller(INTAKE_ROLLER_IN_POWER); } else { intake->spinRoller(INTAKE_ROLLER_STOP_POWER); } } if(operatorPad->GetNumberedButton(RIGHT_TRIGGER_BUTTON)) // low shot = Right Trigger { shooter->initShot(SHOT_LOW_ANGLE, SHOT_LOW_SPEED); printf("test button 1 is pressed"); } else if(operatorPad->GetNumberedButton(LEFT_TRIGGER_BUTTON)) // pass = Left Trigger { shooter->initShot(SHOT_PASS_ANGLE, SHOT_PASS_SPEED); printf("test button 2 is pressed"); } else if(operatorPad->GetNumberedButton(RIGHT_BUMPER_BUTTON)) // high shot = Right Bumper { shooter->initShot(SHOT_HIGH_ANGLE, SHOT_HIGH_SPEED); printf("test button 3 is pressed"); } else if(operatorPad->GetNumberedButton(LEFT_BUMPER_BUTTON)) // truss = Left Bumper { shooter->initShot(SHOT_TRUSS_ANGLE, SHOT_TRUSS_SPEED); printf("test button 4 is pressed"); } //printf("right Joystick distance: %f left Joystick distance: %f \n", driverPad->GetRightY(), driverPad->GetLeftY()); }
/** * Periodic code for autonomous mode should go here. * * Use this method for code which will be called periodically at a regular * rate while the robot is in autonomous mode. */ void RobotDemo::AutonomousPeriodic() { shooter->handle(); intake->handle(); switch(curAutoState) { case AUTO_EXTEND: if(curAutoState != preAutoState) { preAutoState = curAutoState; intake->move(INTAKE_EXTEND_POWER); printf("extending, pre %d, cur %d, \n", preAutoState, curAutoState); } if(intake->getCurrentPosition() >= EXTENDER_MAX_DISTANCE) { intake->move(STOP); curAutoState = AUTO_DRIVE; printf("pre %d, cur %d, stopped extender \n", preAutoState, curAutoState); } break; case AUTO_DRIVE: if(curAutoState != preAutoState) { preAutoState = curAutoState; drive->InitAutoDrive(AUTO_DRIVE_DISTANCE_RIGHT, AUTO_DRIVE_DISTANCE_LEFT, AUTO_DRIVE_SPEED); printf("We're moving! pre %d, cur %d, \n", preAutoState, curAutoState); } /* if(HotAutoSensor->Get()) { HotAutoStarts = true; } */ if(drive->autoDrive() == true) { curAutoState = AUTO_STOP_DRIVING; } break; case AUTO_STOP_DRIVING: if(curAutoState != preAutoState) { preAutoState = curAutoState; printf("now stopping pre %d, cur %d, \n", preAutoState, curAutoState); drive->TeleopDrive(AUTO_STOPPING_SPEED_LEFT, AUTO_STOPPING_SPEED_RIGHT); } if(drive->getSpeed() > 0.01) { drive->TeleopDrive(AUTO_STOPPED_SPEED_LEFT, AUTO_STOPPED_SPEED_RIGHT); // if(HotAutoStarts || HotAutoTimer->Get() > 5.0) // { curAutoState = AUTO_SHOOT; // } } break; case AUTO_SHOOT: if(curAutoState != preAutoState) { preAutoState = curAutoState; printf("now shooting pre %d, cur %d, \n", preAutoState, curAutoState); shooter->initShot(SHOT_HIGH_ANGLE, SHOT_HIGH_SPEED); } if(!shooter->isShooting()) { curAutoState = AUTO_STOP; } break; /* case AUTO_MOBILITY: if(curAutoState != preAutoState) { drive->InitAutoDrive(AUTO_SETUP_SHOT_DISTANCE_RIGHT, AUTO_SETUP_SHOT_DISTANCE_LEFT, AUTO_SETUP_SHOT_SPEED); } if(drive->autoDrive()) { curAutoState = AUTO_STOP; } break; */ case AUTO_STOP: preAutoState = curAutoState; printf("auto is done pre %d, cur %d, \n", preAutoState, curAutoState); break; // HotAutoTimer->Stop(); default: break; } }