diff --git a/src/org/usfirst/frc/team2059/robot/RobotMap.java b/src/org/usfirst/frc/team2059/robot/RobotMap.java index 810eb06..ea4924e 100644 --- a/src/org/usfirst/frc/team2059/robot/RobotMap.java +++ b/src/org/usfirst/frc/team2059/robot/RobotMap.java @@ -10,6 +10,8 @@ public class RobotMap { //Arm public static double zeroDegrees = 2.019; public static double ninetyDegrees = 3.512; + public static int armBottomLim = 4; + public static int armTopLim = 5; public static int armPot = 0; public static int armLeftMotor = 5; public static int armRightMotor = 6; diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java index e2bae37..014de27 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java @@ -3,11 +3,15 @@ import org.usfirst.frc.team2059.robot.RobotMap; import org.usfirst.frc.team2059.robot.commands.Drive; import edu.wpi.first.wpilibj.command.PIDSubsystem; import edu.wpi.first.wpilibj.CANTalon; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.AnalogInput; public class MainArm extends PIDSubsystem { CANTalon armMotorLeft = new CANTalon(RobotMap.armLeftMotor); CANTalon armMotorRight = new CANTalon(RobotMap.armRightMotor); 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); @@ -34,10 +38,19 @@ public class MainArm extends PIDSubsystem { return potToDegrees(getRaw()); } private double potToDegrees(double pot) { + 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