public void setSuckingLED() {
if (sol4.get() == false && sol5.get() == true) {
suckingLED.set(true);
} else {
suckingLED.set(false);
}
}
protected void checkState() {
if (solenoid.get()) {
direction = Direction.EXTENDING;
} else if (!solenoid.get()) {
direction = Direction.RETRACTING;
}
}
public MySolenoid(int solDownChannel, int solUpChannel, boolean bDefault) {
// True means its backwards, so the default is on
m_bDefaultStat = bDefault;
m_solUp = new Solenoid(solUpChannel);
m_solDown = new Solenoid(solDownChannel);
m_solUp.set(m_bDefaultStat);
m_solDown.set(!m_bDefaultStat);
}
/**
* Sets the solenoids based on argument, true means active
*
* @param bVal
*/
public void set(boolean bVal) {
if (bVal) {
m_solUp.set(m_bDefaultStat);
m_solDown.set(!m_bDefaultStat);
} else {
m_solUp.set(!m_bDefaultStat);
m_solDown.set(m_bDefaultStat);
}
}
/**
* Move the flopper
*
* @param down Button input
*/
public void flop(boolean down) {
if (down && !flopped) {
position = !position;
flopperA.set(position);
flopperB.set(!position);
flopped = true;
} else if (!down) {
flopped = false;
}
}
@Override
public HardwareSingleSolenoid retract() {
solenoid.set(false);
direction = Direction.RETRACTING;
checkState();
return this;
}
/** Initializes the instance variables. Done here because this can be called twice. */
protected void initialize() {
if (!RobotMap.isPrototype) {
// final robot code goes here
} else {
gathererOn = false;
gathererSwitch = new Relay(RobotMap.gathererMotorRelay);
pullGatherer = new Solenoid(RobotMap.pullGatherer);
pushGatherer = new Solenoid(RobotMap.pushGatherer);
leftExhaust = new Solenoid(RobotMap.leftExhaust);
rightExhaust = new Solenoid(RobotMap.rightExhaust);
pullGatherer.set(false);
pushGatherer.set(true);
}
}
@Override
public HardwareSingleSolenoid extend() {
solenoid.set(true);
direction = Direction.EXTENDING;
checkState();
return this;
}
HardwareSingleSolenoid(edu.wpi.first.wpilibj.Solenoid solenoid, Direction initialDirection) {
assert solenoid != null;
assert initialDirection != null;
this.solenoid = solenoid;
this.direction = initialDirection;
solenoid.set(initialDirection == Direction.EXTENDING);
checkState();
}
public void foldGatherer(boolean pull) {
if (pull) {
pushGatherer.set(false);
rightExhaust.set(true);
leftExhaust.set(true);
pullGatherer.set(true);
} else {
pullGatherer.set(false);
rightExhaust.set(false);
leftExhaust.set(false);
pushGatherer.set(true);
}
}
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();
}
protected void execute() {
if (time.get() < 3.0) {
talonLeft.set(1);
talonRight.set(1);
} else if (time.get() < 5.0) {
talonLeft.set(0);
talonRight.set(0);
soleLiftDown.set(true);
} else if (time.get() < 6.0) {
talonLeft.set(-1);
talonRight.set(-1);
} else talonLeft.set(1);
talonRight.set(1);
}
private void updateState() {
switch (state) {
case CLAMPED:
clampOpen.set(false);
clampClose.set(true);
break;
case LIFTED:
liftUp.set(true);
liftDown.set(false);
break;
case OPEN:
liftDown.set(true);
liftUp.set(false);
clampOpen.set(true);
clampClose.set(false);
break;
default:
break;
}
}
/** This function is called once each time the robot enters operator control. */
public void operatorControl() {
drivetrain.setSafetyEnabled(true);
compressor.start();
while (isOperatorControl() && isEnabled()) {
// The throttle goes from 1 to -1, bottom to top. This
// scales it to go 0 to 1, bottom to top.
double throttle = (-leftStick.getRawAxis(3) + 1) / 2;
double drive = deadband(leftStick.getY()) * throttle;
double steer = deadband(-rightStick.getX()) * throttle;
drivetrain.arcadeDrive(drive, steer);
boolean shouldSpinWheels = leftStick.getRawButton(1);
boolean shouldFire = shouldSpinWheels && rightStick.getRawButton(1);
shooterWheels.set(shouldSpinWheels ? -1 : 0);
shooter.set(shouldFire);
Timer.delay(0.005); // wait for a motor update time
}
}
public void leftarmclose() {
leftSolOpen.set(false);
leftSolClose.set(true);
}
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 setAngleAct(boolean x) {
angleAct.set(x);
}
public void setNandoAct(boolean x) {
nandoAct.set(x);
}
public void unlock() {
discLock.set(false);
}
public void retract() {
shooterActuator.set(false);
}
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 extend() {
shooterActuator.set(true);
}
public void set(boolean activated) {
collectorArms.set(activated);
}
public void rightarmclose() {
rightSolOpen.set(false);
rightSolClose.set(true);
}
public void rightOpen(boolean open) {
rightGate.set(open);
}
public boolean getState() {
return collectorArms.get();
}
public void lock() {
discLock.set(true);
}