public void setSuckingLED() {
if (sol4.get() == false && sol5.get() == true) {
suckingLED.set(true);
} else {
suckingLED.set(false);
}
}
public void teleopPeriodic() {
smart.putNumber("distance", ultrasonic.getVoltage());
setSuckingLED();
setLEDTeamColour();
// smart dashboard stuff
smart.putBoolean("Fully Pressurized", compressor.getPressureSwitchValue());
smart.putBoolean("fast gear", sol1.get() == true && sol2.get() == false);
smart.putBoolean("slow gear", sol1.get() == false && sol2.get() == true);
if (gyro.getAngle() > 360 || gyro.getAngle() < -360) {
gyro.reset();
}
smart.putBoolean("good angle", gyro.getAngle() < 30 && gyro.getAngle() > -30);
smart.putBoolean(
"too far right", (gyro.getAngle() > 30 && gyro.getAngle() < 180) || gyro.getAngle() < -180);
smart.putBoolean(
"too far left", (gyro.getAngle() < -30 && gyro.getAngle() > -180) || gyro.getAngle() > 180);
if (ultrasonic.getVoltage() < 0.55) {
tooClose = true;
} else {
tooClose = false;
}
smart.putBoolean("Too close", tooClose);
if (ultrasonic.getVoltage() > 0.8) {
tooFar = true;
} else {
tooFar = false;
}
smart.putBoolean("Too far", tooFar);
if (ultrasonic.getVoltage() >= 0.55 && ultrasonic.getVoltage() <= 0.8) {
inRange = true;
} else {
inRange = false;
}
smart.putBoolean("In range", inRange);
}
/** This function is called periodically during autonomous */
public void autonomousInit() {
autoTimer.reset();
autoTimer.start();
gyro.reset();
relay.set(Relay.Value.kOn);
setSpeedFast();
setLEDTeamColour();
sol4.set(false);
sol5.set(true);
sol7.set(true);
sol8.set(false);
autoStop = false;
autonomousStopped = false;
autoTimeAtStop = 99999;
drive.setRun(false);
loadAndShoot.setRun(false);
autoForward();
}
public void shoot() {
sol7.set(false);
sol8.set(true);
}
public void setSpeedSlow() {
sol1.set(false);
sol2.set(true);
}
public void setSpeedFast() {
sol1.set(true);
sol2.set(false);
}
public void setNandoAct(boolean x) {
nandoAct.set(x);
}
public void operatorControl() {
while (this.isEnabled()) {
if (controller.getRawButton(1)) {
aButtonPressed = true;
} else if (aButtonPressed) {
aButtonPressed = false;
if (!aButtonPressed) {
in.set(false);
out.set(false);
}
}
if (controller.getRawButton(2)) {
bButtonPressed = true;
} else if (bButtonPressed) {
bButtonPressed = false;
if (!bButtonPressed) {
in.set(false);
out.set(false);
}
}
if (controller.getRawButton(3)) {
xButtonPressed = true;
} else if (xButtonPressed) {
xButtonPressed = false;
if (!xButtonPressed) valve.set(Relay.Value.kOff);
}
if (controller.getRawButton(4)) {
yButtonPressed = true;
} else if (yButtonPressed) {
yButtonPressed = false;
valve.set(Relay.Value.kOff);
}
if (aButtonPressed) {
out.set(false);
in.set(true);
}
if (bButtonPressed) {
in.set(false);
out.set(true);
}
if (xButtonPressed) {
SmartDashboard.putBoolean("compressorOn", true);
valve.set(Relay.Value.kOn);
}
SmartDashboard.putBoolean("compressorOn", xButtonPressed);
SmartDashboard.putBoolean("pistonOut", aButtonPressed);
SmartDashboard.putBoolean("pistonIn", bButtonPressed);
SmartDashboard.putBoolean("pressure relief", input.get());
// System.out.println("output " + input.get());
if (input.get()) {
System.out.println("INPUT RETURNED TRUE, SETTING VALVE TO OFF");
valve.set(Relay.Value.kOff);
}
Timer.delay(.1);
}
}
public void setAngleAct(boolean x) {
angleAct.set(x);
}
public void unlock() {
discLock.set(false);
}
public void lock() {
discLock.set(true);
}
public void retract() {
shooterActuator.set(false);
}
public void extend() {
shooterActuator.set(true);
}
