public void drive() {
// Driving controls
/* flips which direction on the joysticks is 'forward'
* the variable 'controlFlipClean' needs some explanation:
* it forces the driver to release the button before it recognizes
* another click. */
if (xboxDriver.getBtnL3()) { // left stick click
if (controlFlipClean) {
controlFlipClean = false;
controlFlip = !controlFlip;
}
} else {
controlFlipClean = true;
}
// controls right side
double yRight = -xboxDriver.getRightY();
// controls left side
double yLeft = xboxDriver.getLeftY();
// trigger values will change the maximum speed of the robot
double throttle = xboxDriver.getTriggers();
// half speed
if (throttle > 0.5) {
yRight = yRight * 0.5;
yLeft = yLeft * 0.5;
}
// quarter speed
if (throttle < -0.5) {
yRight = yRight * 0.25;
yLeft = yLeft * 0.25;
}
// flip controls if we need to. Make them go 'backwards'.
// We need to swap and yRight and yLeft values.
if (controlFlip) {
double tempRight = yRight;
yRight = yLeft;
yLeft = tempRight;
}
/* DEADZONE **
* The springs in the joysticks wear out so they don't always come
* back to the centre. To fix this we're going to ignore a small region
* around the centre.
*/
if (yRight > -DEADZONE && yRight < DEADZONE) {
yRight = 0;
}
if (yLeft > -DEADZONE && yLeft < DEADZONE) {
yLeft = 0;
}
vicDriveRight.set(yRight);
vicDriveLeft.set(yLeft);
} // end of drive
public void run() {
double power;
if (shooting == false && retracting == false) {
power = 0.0;
} else if (shooting == false && retracting == true) {
power = m_retract();
} else if (shooting == true && retracting == false) {
power = m_shoot();
} else { // shooting == true && retracting == true
power = 0.0;
}
motor1.set(power);
motor2.set(power);
motor3.set(power);
}
public void autonomousInit() {
lightsOutput1.set(true);
lightsOutput2.set(true);
lightsOutput3.set(false);
canShoot = false;
vicDriveRight.set(0);
vicDriveLeft.set(0);
armVictor.set(0);
winchRelay.set(Relay.Value.kOff);
shooterWheel.set(0);
shooterLoft.set(0);
bettyRelay.set(Relay.Value.kOff);
starterDelay = 0;
}
/** Called every time tele-op mode starts */
public void teleopInit() {
vicDriveRight.set(0);
vicDriveLeft.set(0);
armVictor.set(0);
winchRelay.set(Relay.Value.kOff);
shooterWheel.set(0);
shooterLoft.set(0);
bettyRelay.set(Relay.Value.kOff);
lightsOutput1.set(true);
lightsOutput2.set(true);
lightsOutput3.set(true);
lightsTimer.reset();
lightsTimer.start();
teleopActive = false;
controlFlip = true;
controlFlipClean = true;
shootingSpeed = 0.0;
starterDelay = 0;
}
public void shooterUse() {
// Betty operation
if (xboxOperator.getBtnLB()) { // hold right bumper to fire
starterDelay++;
if (xboxOperator.getTriggers() < -0.5) { // feeder shot
if (shootingSpeed > feederShotSpeed) { // accelerating shooter speed until top speed
shootingSpeed = shootingSpeed + flyWheelAcceleration;
} else {
shootingSpeed = feederShotSpeed; // ensures desired value
}
} else { // normal shot
if (shootingSpeed > normalShotSpeed) { // accelerating shooter speed until top speed
shootingSpeed = shootingSpeed + flyWheelAcceleration;
} else {
shootingSpeed = normalShotSpeed; // ensures desired value
}
}
shooterWheel.set(shootingSpeed);
if (xboxOperator.getBtnRB() && starterDelay > 60) { // shoot normal shot
bettyRelay.setDirection(Relay.Direction.kForward);
bettyRelay.set(Relay.Value.kOn);
} else if (xboxOperator.getBtnB() && starterDelay > 60) { // shoot normal shot
bettyRelay.setDirection(Relay.Direction.kReverse);
bettyRelay.set(Relay.Value.kOn);
} else {
bettyRelay.set(Relay.Value.kOff);
}
} else if (bettyOpen.get()) { // close betty if not shooting
shootingSpeed = 0.0;
bettyRelay.setDirection(Relay.Direction.kForward);
bettyRelay.set(Relay.Value.kOn);
shooterWheel.set(0);
starterDelay = 0;
} else {
shootingSpeed = 0.0;
bettyRelay.set(Relay.Value.kOff);
shooterWheel.set(0);
starterDelay = 0;
}
} // end of shooter use
/** This function is called periodically during autonomous */
public void autonomousPeriodic() {
// int loftPotValue = loftPot.getValue();
// // ajust for far shot
// if (!canShoot) {
// if (loftPotValue < farShotValue - 8) {
// starterDelay = 0;
// shooterLoft.set(-0.5);
//
// }
// else if (loftPotValue > farShotValue + 8) {
// starterDelay = 0;
// shooterLoft.set(0.5);
//
// }
// else { // if ajusted, shoot
// canShoot = true;
// shooterLoft.set(0);
// }
// }
// if(canShoot) {
if (shootingSpeed > normalShotSpeed) { // accelerating shooter speed until top speed(1.0)
shootingSpeed = shootingSpeed + flyWheelAcceleration;
} else {
shootingSpeed = normalShotSpeed; // ensures desired value and no higher
}
shooterWheel.set(shootingSpeed);
if (shootingSpeed == normalShotSpeed) {
starterDelay++;
if (starterDelay > 60) {
bettyRelay.setDirection(Relay.Direction.kForward);
bettyRelay.set(Relay.Value.kOn);
} else if (starterDelay > 310) { // kill flywheel and betty
bettyRelay.set(Relay.Value.kOff);
shooterWheel.set(0);
} else {
bettyRelay.set(Relay.Value.kOff);
}
}
// }
}
public void teleopPeriodic() {
armpid.setPID(
SmartDashboard.getNumber("kP"),
SmartDashboard.getNumber("kI"),
SmartDashboard.getNumber("kD"));
if (joy1.getRawButton(1)) {
armpid.enable();
} else {
armpid.disable();
arm.set(0);
}
SmartDashboard.putBoolean("Checkbox 2", limit.get());
System.out.println("limit " + limit.get());
}
public void manualControl(double power) {
motor1.set(power);
}
public void setRollers(double i) {
rollers.set(i);
}
/** This function is called periodically during operator control */
public void teleopPeriodic() {
lights.set(Relay.Value.kForward);
// tracking.run();
/*try {
readString = serial.readString();
readString = readString.substring(readString.indexOf("data{"), readString.indexOf("}"));
xTurn = readString.substring(readString.indexOf("x("), readString.indexOf(")"));
xTurnVal = Integer.parseInt(xTurn);
} catch (VisaException ex) {
System.out.println("Error Reading string - " + ex);
}*/
y = drive_control.getRawAxis(1);
x = drive_control.getRawAxis(2);
t = drive_control.getRawAxis(3);
if (y > 1) y = 1;
else if (y < -1) y = -1;
if (x > 1) x = 1;
else if (x < -1) x = -1;
drive.setDriveJ(x, y, t);
/*if(drive_control.getRawButton(12))
//wheelsOn = true;
else if(drive_control.getRawButton(11));
wheelsOn = false;*/
/*try{
launcher.setX((drive_control.getRawAxis(4)-1)/2);
}catch(Exception e){}*/
// System.out.println((drive_control.getRawAxis(4)-1)*-1350);
if (!autoWheels) {
csc.setSetpoint((drive_control.getRawAxis(4) - 1) * -1500);
try {
// launcher.setX((drive_control.getRawAxis(4)+1)*2000);
System.out.println(
-launcher.getSpeed()
+ " -- "
+ csc.tmpResult
+ " -- "
+ (drive_control.getRawAxis(4) - 1) * -1500);
} catch (Exception e) {
System.out.println(e);
}
if (drive_control.getRawButton(11)) autoWheels = true;
} else {
csc.setSetpoint(2300);
if (drive_control.getRawButton(11)) autoWheels = false;
}
/*if(wheelsOn)
shoot(1);
else
shoot(0);*/
if (drive_control.getRawButton(5)) {
wedge.moveDown();
// servo1.set(1);
// servo2.set(0);
// wedge.set(Relay.Value.kForward);
} else if (drive_control.getRawButton(6)) {
wedge.moveUp();
// servo1.set(0);
// servo2.set(1);
// wedge.set(Relay.Value.kReverse);
} else {
// servo1.set(.5);
// servo2.set(.5);
// wedge.set(Relay.Value.kOff);
}
if (drive_control.getRawButton(3)) turret.set(.5);
else if (drive_control.getRawButton(4)) turret.set(-.5);
else turret.set(0);
if (drive_control.getRawButton(2)) driveMainBelt(true, true);
else if (drive_control.getRawButton(7)) driveMainBelt(false, true);
else driveMainBelt(false, false);
if (drive_control.getTrigger()) {
try {
belt2.setX(-1);
} catch (Exception e) {
}
} else if (drive_control.getRawButton(8)) {
try {
belt2.setX(1);
} catch (Exception e) {
}
} else {
try {
belt2.setX(0);
} catch (Exception e) {
}
}
if (control.getRawButton(3)) {
turret.set(.25);
} else if (control.getRawButton(4)) {
turret.set(-.25);
} else {
turret.set(0);
}
try {
station.println(
DriverStationLCD.Line.kMain6, 1, "RPM: " + String.valueOf(-launcher.getSpeed()));
} catch (CANTimeoutException ex) {
}
/*try{
if(tracking.foundTarget()){
station.println(DriverStationLCD.Line.kUser2, 1, "Distance: " + String.valueOf(tracking.getTarget("HIGH").distance));
station.println(DriverStationLCD.Line.kUser3, 1, "Suggested RPM: " + String.valueOf(tracking.getTarget("HIGH").distance*62.8));
if(x>0)
station.println(DriverStationLCD.Line.kUser4, 1, "Rotation: <--");
else if(x<0)
station.println(DriverStationLCD.Line.kUser4, 1, "Roation: -->");
else
station.println(DriverStationLCD.Line.kUser4, 1, "I don't need to rotate");
}
else{
station.println(DriverStationLCD.Line.kUser2, 1, "Help, I can't find target by myself!");
station.println(DriverStationLCD.Line.kUser3, 1, "");
station.println(DriverStationLCD.Line.kUser4, 1, "");
}
}catch(Exception e){}*/
station.updateLCD();
}
public void armUse() {
// Arm contols
int armPotValue = armPot.getValue();
if (xboxDriver.getBtnB()) { // arm for slot feeding
if (armPotValue < armSlotFeedingValue - angleTolerance) {
armVictor.set(-.2);
} else if (armPotValue < armSlotFeedingValue - focusTolerance) {
armVictor.set(-.02);
} else if (armPotValue > armSlotFeedingValue + angleTolerance) {
armVictor.set(.5);
} else if (armPotValue > armSlotFeedingValue + focusTolerance) {
armVictor.set(.05);
} else {
armVictor.set(0);
}
} else if (xboxDriver.getBtnY()) { // arm for shovel feeding
if (armPotValue < armShovelFeedingValue - angleTolerance) {
armVictor.set(-.2);
} else if (armPotValue < armShovelFeedingValue - focusTolerance) {
armVictor.set(-.02);
} else if (armPotValue > armShovelFeedingValue + angleTolerance) {
armVictor.set(.5);
} else if (armPotValue > armShovelFeedingValue + focusTolerance) {
armVictor.set(.05);
} else {
armVictor.set(0);
}
} else {
if (xboxDriver.getBtnLB() && armPotValue > minArmValue) {
armVictor.set(-.2); // driver left bumper = down, right bumper = up;
} else if (xboxDriver.getBtnRB() && armPotValue < maxArmValue) {
armVictor.set(.5);
} else {
armVictor.set(0);
}
if (xboxDriver.getBtnA()
&& armPotValue
< climbStopValue) { // when driver controls, hold button A to use winch, release to
// stop
winchRelay.set(Relay.Value.kOn);
winchRelay.setDirection(Relay.Direction.kReverse);
} else {
winchRelay.set(Relay.Value.kOff);
}
}
} // end of arm use
public void moveRollerReverse() {
// Move the roller to suck in the ball
roller.set(-1);
}
public void turnRollerOff() {
roller.set(0);
}
private void setPower(double power) {
motor1.set(-power);
}
public void shooterLoft() {
// Ajust loft
double operatorLeftY = xboxOperator.getLeftY(); // operator has loft control
int loftPotValue = loftPot.getValue();
if (xboxOperator.getBtnX()) { // hold to ajust for close shot
if (loftPotValue < closeShotValue - focusTolerance) {
if (loftPotValue < closeShotValue - angleTolerance) {
shooterLoft.set(-1);
} else {
shooterLoft.set(-0.001);
}
} else if (loftPotValue > closeShotValue + focusTolerance) {
if (loftPotValue > closeShotValue + angleTolerance) {
shooterLoft.set(1);
} else {
shooterLoft.set(0.001);
}
} else {
shooterLoft.set(0);
}
} else if (xboxOperator.getBtnY()) { // hold to ajust for far shot
if (loftPotValue < farShotValue - focusTolerance) {
if (loftPotValue < farShotValue - angleTolerance) {
shooterLoft.set(-1);
} else {
shooterLoft.set(-0.001);
}
} else if (loftPotValue > farShotValue + focusTolerance) {
if (loftPotValue > farShotValue + angleTolerance) {
shooterLoft.set(1);
} else {
shooterLoft.set(0.001);
}
} else {
shooterLoft.set(0);
}
} else if (xboxOperator.getBtnB()) { // hold to ajust for feeding
if (loftPotValue < shooterFeedingValue - focusTolerance) {
if (loftPotValue < shooterFeedingValue - angleTolerance) {
shooterLoft.set(-1);
} else {
shooterLoft.set(-0.001);
}
} else if (loftPotValue > shooterFeedingValue + focusTolerance) {
if (loftPotValue > shooterFeedingValue + angleTolerance) {
shooterLoft.set(1);
} else {
shooterLoft.set(0.001);
}
} else {
shooterLoft.set(0);
}
} else if (loftPotValue > maxHeightValue
&& loftPotValue < minHeightValue
&& ((operatorLeftY > .3) || operatorLeftY < -.3)) { // move loft with left operator stick
shooterLoft.set(operatorLeftY);
} else {
shooterLoft.set(0); // don't move loft
}
} // end of shooter loft
public void stop() {
motor1.set(0);
}
public void moveRollerForward() {
// Move the roller to eject the ball
roller.set(1);
}
