public boolean isBallIn() {
if ((intakeLeftSwitch.get() && intakeRightSwitch.get())) {
return true;
}
return false;
}
// whether hooks have attached
public boolean getError(boolean goingUp) {
if (goingUp) {
return (!rightFixedHookMetalSensor.get() || !leftFixedHookMetalSensor.get());
} else {
return (!rightMovingHookMetalSensor.get() || !leftMovingHookMetalSensor.get());
}
}
/** Handles periodic operations. Should be called periodically. */
public void loop() {
ds.setDigitalOut(8, limitSwitch.get());
final int tolerance =
10; // the number of degrees, in either direction, of tolerance for motor movement
if (!isAccepting) {
if (topTarget > topEncoder.get() && Math.abs(topTarget - topEncoder.get()) > tolerance) {
int error = Math.abs(topTarget - topEncoder.get());
topMotor.set(-limit(error / ERROR_CONVERSION));
// SmartDashboard.log("Backwards", "Gripper Top Motor");
} else if (topTarget < topEncoder.get()
&& Math.abs(topTarget - topEncoder.get()) > tolerance) {
double error = Math.abs(topTarget - topEncoder.get());
topMotor.set(limit(error / ERROR_CONVERSION));
// SmartDashboard.log("Forward", "Gripper Top Motor");
} else {
topMotor.set(0);
}
if (bottomTarget > bottomEncoder.get()
&& Math.abs(bottomTarget - bottomEncoder.get()) > tolerance) {
double error = Math.abs(bottomTarget - bottomEncoder.get());
bottomMotor.set(-limit(error / ERROR_CONVERSION));
// SmartDashboard.log("Backwards", "Gripper Bottom Motor");
} else if (bottomTarget < bottomEncoder.get()
&& Math.abs(bottomTarget - bottomEncoder.get()) > tolerance) {
double error = Math.abs(bottomTarget - bottomEncoder.get());
bottomMotor.set(limit(error / ERROR_CONVERSION));
// SmartDashboard.log("Forward", "Gripper Bottom Motor");
} else {
bottomMotor.set(0);
// SmartDashboard.log("Stopped", "Gripper Bottom Motor");
}
}
// if the gripper is in accept mode, it needs to stop when the limit switch is pressed
if (isAccepting && !limitSwitch.get()) {
// and also by simply stopping them
topMotor.set(0);
bottomMotor.set(0);
isAccepting = false;
// stop the motors by making the target positions equal to the current positions
topTarget = topEncoder.get();
bottomTarget = bottomEncoder.get();
} else if (isAccepting && limitSwitch.get()) {
// if it is accepting and the limit switch is not pressed
topMotor.set(1);
bottomMotor.set(-1);
}
}
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 autoPosLights() {
if (botLim.get() == false) { // if bottom limit button is pressed, turn on botton light
bottom.set(Relay.Value.kForward);
} else { // otherwise, keep the bottom light off
bottom.set(Relay.Value.kReverse);
}
if (midLim.get() == false) { // if middle limit button is pressed, turn on middle light
middle.set(Relay.Value.kForward);
} else { // otherwise, keep the middle light off
middle.set(Relay.Value.kReverse);
}
if (topLim.get() == false) { // if top limit button is pressed, turn on top lights
top.set(Relay.Value.kForward);
} else { // otherwise, keep the top light off
top.set(Relay.Value.kReverse);
}
}
// Resets position in order to reload frisbee later
public void reload() throws CANTimeoutException {
if (backSwitch.get()) {
loadingStick.setX(0);
} else {
loadingStick.setX(-.8);
}
}
public boolean isDetensioned() {
double curr_volts = sensor.getAverageVoltage();
boolean on_target =
((curr_volts > (slack_tension_volts - threshold_volts))
&& (curr_volts < (slack_tension_volts + threshold_volts)));
boolean at_min = leftMin.get();
return (on_target || at_min);
}
// Loads the frisbee into the shooter
public void load() throws CANTimeoutException {
if (frontSwitch.get()) {
loadingStick.setX(0);
} else {
loadingStick.setX(.8);
}
}
protected void usePIDOutput(double output) {
//
// Important: invert the motor polarity!
// (Positive Motor Voltage -> Lower Tension)
//
output = -output;
//
// Only allow the arms to move in a direction away from a limit switch
// if that limit switch is closed, unless the motor command is 0.0
// (so the motor controller can be disabled)
//
if ((output > 0.0 && !leftMin.get()) || (output < 0.0 && !leftMax.get()) || (output == 0.0))
this.leftSC.set(output);
else this.leftSC.set(0);
}
private void moveRightMotor(double speed) {
// rightMotor.set(speed);
// System.out.println("limit = " + bottomLimit.getSmartDashboardType());
if (speed < 0.0) {
rightMotor.set(speed);
} else if (speed > 0.0 && !bottomLimit.get()) {
rightMotor.set(0.0);
} else {
rightMotor.set(speed);
}
}
/** @param speed */
private void moveLeftMotor(double speed) {
// leftMotor.set(speed*-1.0);
if (speed < 0.0) {
leftMotor.set(speed * -1.0);
} else if (speed > 0.0 && !bottomLimit.get()) {
leftMotor.set(0.0);
} else {
leftMotor.set(speed * -1.0);
}
}
/** This function is called periodically during operator control */
public void teleopPeriodic() {
robotDrive.mecanumDrive_Polar();
lifter.upLift();
lifter.downLift();
lifter.QuickRaise();
lifter.toteLowering(); // lowers 6 totes and a can at .1 speed
// lifter.manualUplift();
// lifter.manualDownlift();
servo.runBolt();
pusher.PushandRetract(); // push one button to push and retract
pusher.Push(); // full push with one button press
pusher.Retract(); // full retract with one button press
// pusher.ManualPush();
// pusher.ManualRetract();
SmartDashboard.putBoolean("switch 1", DigitalInput1.get());
SmartDashboard.putBoolean("switch 2", DigitalInput2.get());
SmartDashboard.putBoolean("switch 3", DigitalInput3.get());
SmartDashboard.putBoolean("switch 4", DigitalInput4.get());
SmartDashboard.putBoolean("switch 5", DigitalInput5.get());
SmartDashboard.putBoolean("switch 6", DigitalInput6.get());
SmartDashboard.putNumber("Lifting Talon Speed", LifterTalon.get());
SmartDashboard.putNumber("Pushing Talon Speed", PushingTalon.get());
SmartDashboard.putNumber("servo position zero means brake is on", servo.get());
}
public void teleopPeriodic() {
watchdog.feed(); // feed the watchdog
// Toggle drive mode
if (!driveToggle && left.getRawButton(2)) {
driveMode = (driveMode + 1) % 3;
driveToggle = true;
} else if (driveToggle && !left.getRawButton(2)) driveToggle = false;
// Print drive mode to DS & send values to Jaguars
switch (driveMode) {
case 0:
dsLCD.println(DriverStationLCD.Line.kMain6, 1, "Drive mode: Tank ");
red.set(left.getY() + gamePad.getY());
black.set(-right.getY() - gamePad.getThrottle());
break;
case 1:
dsLCD.println(DriverStationLCD.Line.kMain6, 1, "Drive mode: Arcade");
red.set(right.getX() - right.getY());
black.set(right.getX() + right.getY());
break;
case 2:
dsLCD.println(DriverStationLCD.Line.kMain6, 1, "Drive mode: Kaj ");
red.set(left.getX() - right.getY());
black.set(left.getX() + right.getY());
break;
} /**/
dsLCD.println(
DriverStationLCD.Line.kUser3,
1,
"1" + photoreceptorL.get() + "2" + photoreceptorM.get() + "3" + photoreceptorR.get());
dsLCD.println(DriverStationLCD.Line.kUser4, 1, "Encoder in 4,5: " + leftEncoder.get() + " ");
dsLCD.println(
DriverStationLCD.Line.kUser5, 1, "Encoder in 6,7: " + rightEncoder.get() + " ");
dsLCD.updateLCD();
}
/** This function is called periodically during test mode */
public void testPeriodic() {
driveTrainTester.run();
SmartDashboard.putBoolean("switch 1", DigitalInput1.get());
SmartDashboard.putBoolean("switch 2", DigitalInput2.get());
SmartDashboard.putBoolean("switch 3", DigitalInput3.get());
SmartDashboard.putBoolean("switch 4", DigitalInput4.get());
SmartDashboard.putBoolean("switch 5", DigitalInput5.get());
SmartDashboard.putBoolean("switch 6", DigitalInput6.get());
SmartDashboard.putNumber("Lifting Talon Speed", LifterTalon.get());
SmartDashboard.putNumber("Pushing Talon Speed", PushingTalon.get());
}
public void autonomousInit() {
// Timer section
timerMain = new Timer();
timerMain.reset();
timerMain.start();
timer1Left = new Timer();
timer2Left = new Timer();
timer1Right = new Timer();
timer2Right = new Timer();
timer1Center = new Timer();
timer2Center = new Timer();
controlChuteLeft = new boolean[] {false, false};
controlChuteRight = new boolean[] {false, false};
controlChuteCenter = new boolean[] {false, false};
controlChuteLeft[0] = true;
controlChuteRight[0] = true;
controlChuteCenter[0] = true;
/* Boolean setting section
* Robot Aligns Left
*/
if (DigitalInput7.get() == false && DigitalInput8.get() == true) {
b_leftAutonomous = true;
b_rightAutonomous = false;
b_centerAutonomous = false;
b_upAutonomous = false;
b_downAutonomous = true;
b_leftDriveAutonomous = false;
}
// Robot Aligns Right
if (DigitalInput7.get() == true && DigitalInput8.get() == false) {
b_leftAutonomous = false;
b_rightAutonomous = true;
b_centerAutonomous = false;
b_upAutonomous = false;
b_downAutonomous = true;
b_rightDriveAutonomous = false;
}
// Robot Aligns Center
if (DigitalInput7.get() == true && DigitalInput8.get() == true) {
b_leftAutonomous = false;
b_rightAutonomous = false;
b_centerAutonomous = true;
b_upAutonomous = false;
b_downAutonomous = true;
b_centerDriveAutonomous = false;
}
} // End autonomousInit
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
// sets climber speed to given value
public void manualControl(double d) {
if (topClimberSwitch.get() && d > 0 || bottomClimberSwitch.get() && d < 0) {
climberMotor.set(0);
}
if (topClimberSwitch.get() && d < 0) {
climberMotor.set(d);
}
if (bottomClimberSwitch.get() && d > 0) {
climberMotor.set(d);
}
if (!topClimberSwitch.get() && !bottomClimberSwitch.get()) {
climberMotor.set(d);
}
putData();
}
public boolean isUp() {
return !upLimit.get();
}
public boolean getOutSwitch() {
return outSwitch.get();
}
// whether the elevator has reached the top
public boolean getUpperLimit() {
return topClimberSwitch.get() || climberEncoder.get() > encoderMax;
}
// whether the elevator has reached the bottom
public boolean getLowerLimit() {
return bottomClimberSwitch.get() || climberEncoder.get() < encoderMin;
}
public boolean getA() {
return (!_a.get());
}
/** Reads whether the tilter is at/above its highest safe height. */
public boolean isTopLimitTriggered() {
return _stringPot.get() > StringPot.VAL_MAX_SAFE || _topLimitSwitch.get() == _topLimitOnState;
}
public boolean get() {
return limitSwitch.get();
}
public boolean isDown() {
// System.out.println("DOWN: " + !downLimit.get());
return !downLimit.get();
}
public boolean isAtUpperLimit() {
return upperLimitSwitch.get();
}
public boolean getB() {
return (!_b.get());
}
public boolean isAtLowerLimit() {
return lowerLimitSwitch.get();
}
public boolean getInSwitch() {
return inSwitch.get();
}
/**
* Reads input from the joystick in order to control the lifting part of the gatherer, which
* operates on a three-tier elevator-like system. Three different buttons are read from the
* joystick in order to move the {@link Talon} to one of the three tiers. Update periodically.
*
* @param input The joystick that will be used to control the gatherer.
*/
public void gathererTalon(Joystick input) {
if (gatherLift == null) {
System.err.println(
"The gatherer motor controllers have not been initialized. Call this class's method \"initGatherers()\" in order to do so.");
return;
}
switch (gatherState) {
case 0: // checking for input
if (input.getRawButton(Wiring.BTN_GATHERER_TO_TOP) && diGathererTop.get()) {
gatherLift.set(0.6);
System.out.println("Gatherer up");
gatherCount = 0;
gatherState = 1;
} else if (input.getRawButton(Wiring.BTN_GATHERER_TO_BOT) && diGathererBot.get()) {
gatherLift.set(-0.6);
System.out.println("Gatherer down");
gatherCount = 0;
gatherState = 3;
} else if (input.getRawButton(Wiring.BTN_GATHERER_TO_MID)) {
if (!diGathererTop.get()) {
gatherLift.set(-0.6);
System.out.println("Gatherer down");
gatherCount = 0;
gatherState = 5;
} else if (!diGathererBot.get()) {
gatherLift.set(0.6);
System.out.println("Gatherer up");
gatherCount = 0;
gatherState = 6;
}
}
break;
case 1: // should be going up, wait until it hits the top
if (!diGathererTop.get()) {
gatherLift.set(0.0);
System.out.println("Top limit");
gatherState = 2;
}
break;
case 2: // at top, waiting for driver to stop holding the button
if (!input.getRawButton(Wiring.BTN_GATHERER_TO_TOP)) {
gatherState = 0;
}
break;
case 3: // should be going down, wait until it hist the bottom
if (!diGathererBot.get()) {
gatherLift.set(0.0);
System.out.println("Bottom limit");
gatherState = 4;
}
break;
case 4: // at bottom, waiting for driver to stop holding the button
if (!input.getRawButton(Wiring.BTN_GATHERER_TO_BOT)) {
gatherState = 0;
}
break;
case 5: // go down to mid
gatherLift.set(-0.6);
if (counter.get() >= 1) {
gatherLift.set(0.0); // stahp
counter.reset();
gatherState = 0;
}
break;
case 6: // go up to mid
gatherLift.set(0.6);
if (counter.get() >= 1) {
gatherLift.set(0.0); // stahp
counter.reset();
gatherState = 0;
}
break;
}
}