public void gathererSpark(Joystick input, PowerDistributionPanel pdp) {
switch (gatherState) {
case 0:
if (input.getRawButton(7)) {
gatherLift.set(0.6);
System.out.println("Gatherer up");
gatherState = 1;
} else if (input.getRawButton(8)) {
gatherLift.set(-0.6);
System.out.println("Gatherer down");
gatherState = 4;
} else if (input.getRawButton(5)) {
}
break;
case 1:
if (pdp.getCurrent(13) > 1.25) {
System.out.println("Motor Running");
gatherState = 2;
} else if (gatherCount > 3) {
gatherState = 3;
System.out.println("Gather Timeout");
}
gatherCount++;
break;
case 2:
if (pdp.getCurrent(13) <= 1.25) {
gatherLift.set(0.0);
System.out.println("Top limit");
gatherState = 3;
}
break;
case 3:
if (!input.getRawButton(7)) {
gatherCount = 0;
gatherState = 0;
}
break;
case 4:
if (pdp.getCurrent(13) > 1.25) {
System.out.println("Motor Running");
gatherState = 5;
} else if (gatherCount > 3) {
gatherState = 6;
System.out.println("Gather Timeout");
}
gatherCount++;
break;
case 5:
if (pdp.getCurrent(13) <= 1.25) {
gatherLift.set(0.0);
System.out.println("Bottom limit");
gatherState = 6;
}
break;
case 6:
if (!input.getRawButton(8)) {
gatherCount = 0;
gatherState = 0;
}
break;
}
}
public void logPower() {
SmartDashboard.putNumber("Left Power #1: ", pdp.getCurrent(kLeftDriveMotorPDP1));
SmartDashboard.putNumber("Left Power #2: ", pdp.getCurrent(kLeftDriveMotorPDP2));
SmartDashboard.putNumber("Right Power #1: ", pdp.getCurrent(kRightDriveMotorPDP1));
SmartDashboard.putNumber("Right Power #2: ", pdp.getCurrent(kLeftDriveMotorPDP2));
}