diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java index 50d28d4..5dcce49 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java @@ -11,7 +11,6 @@ public class MainArm extends PIDSubsystem { AnalogInput pot = new AnalogInput(RobotMap.armPot); DigitalInput limitSwitchBottom = new DigitalInput(RobotMap.armBottomLim); DigitalInput limitSwitchTop = new DigitalInput(RobotMap.armTopLim); - public MainArm() { super("MainArm", 0.06, 0.0, 0.002); getPIDController().setContinuous(false); @@ -38,20 +37,18 @@ public class MainArm extends PIDSubsystem { return potToDegrees(getRaw()); } private double potToDegrees(double pot) { - System.out.println(limitSwitchBottom.get()); - if (!limitSwitchBottom.get()){ - RobotMap.zeroDegrees = getRaw(); - System.out.println("====Bottom Switch====\n" + getRaw()); - } else if (!limitSwitchTop.get()){ - RobotMap.ninetyDegrees = getRaw(); - System.out.println("====Top Switch====\n" + getRaw()); - - } + System.out.println(limitSwitchBottom.get()); + if (!limitSwitchBottom.get()) { + RobotMap.zeroDegrees = getRaw(); + System.out.println("====Bottom Switch====\n" + getRaw()); + } else if (!limitSwitchTop.get()) { + RobotMap.ninetyDegrees = getRaw(); + System.out.println("====Top Switch====\n" + getRaw()); + } double min = RobotMap.zeroDegrees; double max = RobotMap.ninetyDegrees; System.out.println((pot - min) / (Math.abs(min - max) / 90)); return (pot - min) / (Math.abs(min - max) / 90); } - } // vim: sw=2:ts=2:sts=2