Merge pull request #91 from J0hnCena/master

Add toggle for several controls in the IntakeSystem and put the boole…

src/org/impact2585/frc2016/input/InputMethod.java

@@ -166,9 +166,9 @@ public boolean digitalMoveIntakeAwayFromBot() {
 	}
 
 	/**
-	 * @returns true if the intake should ignore the limit switch, false otherwise
+	 * @returns true if the state of ignoring the intake arm limit switch should be toggled, false if otherwise
 	 */
-	public boolean ignoreIntakeLimitSwitch(){
+	public boolean toggleIntakeLimitSwitch(){
 		return false;
 	}
 

src/org/impact2585/frc2016/input/PartnerXboxInput.java

@@ -127,7 +127,7 @@ public boolean toggleSpeed() {
 	 * @see org.impact2585.frc2016.input.InputMethod#ignoreIntakeLimitSwitch()
 	 */
 	@Override
-	public boolean ignoreIntakeLimitSwitch() {
+	public boolean toggleIntakeLimitSwitch() {
 		return controller2.getRawButton(XboxConstants.X_BUTTON);
 	}
 

src/org/impact2585/frc2016/systems/IntakeSystem.java

@@ -33,6 +33,10 @@ public class IntakeSystem implements RobotSystem, Runnable{
 	public static final long BACKWARDS_LEVER_TIME = 233;
 	private boolean disableSpeedMultiplier;
 	private boolean prevSpeedToggle;
+	private boolean prevIntakeLimitSwitchToggle;
+	private boolean ignoreIntakeArmLimitSwitch;
+	private boolean manualIntakeControl;
+	private boolean prevIntakeControlToggle;
 	private boolean shooting;
 	private Encoder encoder;
 	private long startTime;
@@ -45,26 +49,30 @@ public void init(Environment environ) {
 		input = environ.getInput();
 		wheels = new Talon(RobotMap.INTAKE_WHEEL);
 		leftArm = new Victor(RobotMap.INTAKE_LEFT_ARM);
-		rightArm = new Victor(RobotMap.INTAKE_RIGHT_ARM);
+		rightArm = new Talon(RobotMap.INTAKE_RIGHT_ARM);
 		rightArm.setInverted(true);
 		lever = new Victor(RobotMap.LEVER);
 		shootingLimitSwitch = new DigitalInput(RobotMap.SHOOTING_LIMIT_SWITCH);
 		leftLimitSwitch = new DigitalInput(RobotMap.LEFT_INTAKE_LIMIT_SWITCH);
 		rightLimitSwitch = new DigitalInput(RobotMap.RIGHT_INTAKE_LIMIT_SWITCH);
-		disableSpeedMultiplier = false;
+		disableSpeedMultiplier = true;
 		prevSpeedToggle = false;
 		isPIDEnabled = false;
 		armPID = new IntakePID(.3, .1, 0);
 		setEncoder(new Encoder(RobotMap.INTAKE_ARM_ENCODER_PORT_A, RobotMap.INTAKE_ARM_ENCODER_PORT_B));
+		prevIntakeLimitSwitchToggle = false;
+		ignoreIntakeArmLimitSwitch = true;
+		manualIntakeControl = false;
+		prevIntakeControlToggle = false;
 	}
-	
+
 	/**Sets the motors controlling the wheels on intake to speed
 	 * @param speed the speed to set the motors to
 	 */
 	public void spinWheels(double speed) {
 		wheels.set(speed);
 	}
-	
+
 	/**Rotates the intake arm 
 	 * @param count the encoder count that the arm should rotate until reached
 	 * @param moveTowardBot whether the intake arm should rotate toward the bot
@@ -80,7 +88,7 @@ public boolean rotateAngle(int count, boolean moveTowardBot) {
 		}
 		return encoder.get() == count;
 	}
-	
+
 	/**Enables the intake arm PID controller
 	 * @param count the encoder count to reach
 	 */
@@ -89,64 +97,64 @@ public void enableIntakeArmPID(int count) {
 		armPID.getSetpoint();
 		isPIDEnabled = true;
 	}
-	
+
 	/**Sets the motors controlling the arms for the intake to speed
 	 * @param speed the speed to set the motors to
 	 */
 	public void moveArms(double speed) {
 		leftArm.set(speed);
 		rightArm.set(speed);
 	}
-	
+
 	/**Sets the motor controlling the right arm for the intake to speed
 	 * @param speed the speed to set the motor to
 	 */
 	public void moveRightArm(double speed){
 		rightArm.set(speed);
 	}
-	
+
 	/**Sets the motor controlling the left arm for the intake to speed
 	 * @param speed the speed to set the motor to
 	 */
 	public void moveLeftArm(double speed){
 		leftArm.set(speed);
 	}
-	
+
 	/**Sets the motor controlling the lever for shooting to speed
 	 * @param speed the speed to set the motor to
 	 */
 	public void spinLever(double speed){
 		lever.set(speed);
 	}
-	
+
 	/**
 	 * @returns true if the left limit switch is pressed
 	 */
 	public boolean isLeftSwitchClosed() {
 		return leftLimitSwitch.get();
 	}
-	
+
 	/**
 	 * @returns true if the right limit switch is pressed
 	 */
 	public boolean isRightSwitchClosed() {
 		return rightLimitSwitch.get();
 	}
-	
+
 	/**
 	 * @returns true if the right or left limit switch is closed
 	 */
 	public boolean isSwitchClosed() {
 		return isRightSwitchClosed() || isLeftSwitchClosed();
 	}
-	
+
 	/**
 	 * @returns true if shooting limit switch is pressed 
 	 */
 	public boolean isShootingSwitchClosed(){
 		return shootingLimitSwitch.get();
 	}
-	
+
 	/**Sets the input of the system to newInput
 	 * @param newInput the new input to set to
 	 */
@@ -181,7 +189,7 @@ public long getStartTime() {
 	protected void setStartTime(long startTime) {
 		this.startTime = startTime;
 	}
-	
+
 	/**
 	 * @return the armPID
 	 */
@@ -202,13 +210,17 @@ public void setArmPID(IntakePID armPID) {
 	public void accessSmartDashboard() {
 		SmartDashboard.putNumber("Intake Arm Rate: ", encoder.getRate());
 		SmartDashboard.putNumber("Intake Arm Distance: ", encoder.getDistance());
+		SmartDashboard.putBoolean("Ignore Arm Limit Switch: ", ignoreIntakeArmLimitSwitch);
+		SmartDashboard.putBoolean("Disable Speed Multiplier: ", disableSpeedMultiplier);
+		SmartDashboard.putBoolean("Manual Control of Intake Arms: ", manualIntakeControl);
 	}
 
 	/* (non-Javadoc)
 	 * @see java.lang.Runnable#run()
 	 */
 	@Override
 	public void run() {
+		toggleIntakeArmLimitSwitch();
 		if(input.intake() && !input.outake()) {
 			spinWheels(1);
 		} else if(input.outake() && !input.intake()) {
@@ -217,17 +229,14 @@ public void run() {
 			spinWheels(0);
 		}
 
-		if (input.manualIntakeControl()){
-			moveRightArm(input.rightIntake());
-			moveLeftArm(input.leftIntake());
-		} else {
+		if(!manualIntakeArmControl()) {
 			double intakeArmSpeed = input.moveIntake();
 			if(input.toggleSpeed() && !prevSpeedToggle)
 				disableSpeedMultiplier = !disableSpeedMultiplier;
 			if(!disableSpeedMultiplier) {
 				intakeArmSpeed *= ARM_SPEED;
 			}
-			if(isSwitchClosed() && intakeArmSpeed < 0 && !input.ignoreIntakeLimitSwitch()) {
+			if(isSwitchClosed() && intakeArmSpeed < 0 && !ignoreIntakeArmLimitSwitch) {
 				intakeArmSpeed = 0;
 			}
 			moveArms(intakeArmSpeed);
@@ -249,20 +258,42 @@ public void run() {
 			shoot();
 		}
 		prevSpeedToggle = input.toggleSpeed();
+
 		accessSmartDashboard();
 	}
-
+
+	/**
+	 * toggles the state of ignoring the intake arm limit switch
+	 */
+	public void toggleIntakeArmLimitSwitch() {
+		if(input.toggleIntakeLimitSwitch() && !prevIntakeLimitSwitchToggle) {
+			ignoreIntakeArmLimitSwitch = !ignoreIntakeArmLimitSwitch;
+		}
+		prevIntakeLimitSwitchToggle = input.toggleIntakeLimitSwitch();
+	}
+
+	/**Manually controls separate intake arms
+	 * @return true if the intake arms were controlled separately, false if otherwise
+	 */
+	public boolean manualIntakeArmControl() {
+		if(input.manualIntakeControl() && !prevIntakeControlToggle) {
+			manualIntakeControl = !manualIntakeControl;
+		}
+		prevIntakeControlToggle = input.manualIntakeControl();
+		if(manualIntakeControl) {
+			moveRightArm(input.leftIntake());
+			moveLeftArm(input.leftIntake());
+			return true;
+		}
+		return false;
+	}
 	/**
 	 * Moves the lever forward for 250 ms, back for 233 ms, and stops if it hits the limit switch
 	 */
 	public void shoot(){
-		if(!shooting){
-			shooting = true;
-			startTime = System.currentTimeMillis();
-		}
 		if(System.currentTimeMillis()-startTime < FORWARD_LEVER_TIME){ // if the time that has passed since the lever has started rotating is less than the constant
 			spinLever(1.0);
-		} else if(isShootingSwitchClosed()) {
+		} else if(isShootingSwitchClosed() && !ignoreIntakeArmLimitSwitch) {
 			shooting = false;
 			spinLever(0);
 		} else if(System.currentTimeMillis()-startTime >= FORWARD_LEVER_TIME && System.currentTimeMillis() - startTime < FORWARD_LEVER_TIME + BACKWARDS_LEVER_TIME){ // if the time that has passed since the lever has started rotating is more than or equal to the constant
@@ -272,21 +303,21 @@ public void shoot(){
 			shooting = false;
 		}
 	}
-	
+
 	/**Sets whether or not the intake system should undergo the timed shoot
 	 * @param shoot the boolean value to set shooting to
 	 */
 	public void setShoot(boolean shoot) {
 		shooting = shoot;
 	}
-	
+
 	/**
 	 * @return if the ioshooter is undergoing a timed shoot
 	 */
 	public boolean isShooting() {
 		return shooting;
 	}
-	
+
 	/* (non-Javadoc)
 	 * @see org.impact2585.lib2585.Destroyable#destroy()
 	 */
@@ -296,17 +327,17 @@ public void destroy() {
 			SensorBase motor = (SensorBase) wheels;
 			motor.free();
 		}
-		
+
 		if(lever instanceof SensorBase) {
 			SensorBase motor = (SensorBase) lever;
 			motor.free();
 		}
-		
+
 		if(leftArm instanceof SensorBase) {
 			SensorBase motor = (SensorBase) leftArm;
 			motor.free();
 		}
-		
+
 		if(rightArm instanceof SensorBase) {
 			SensorBase motor = (SensorBase) rightArm;
 			motor.free();
@@ -317,13 +348,13 @@ public void destroy() {
 		encoder.free();
 		armPID.getPIDController().free();
 	}
-	
+
 	/**
 	 * allows methods inside abstract class PIDSubsystem to be used to move arms 
 	 *
 	 */
 	public class IntakePID extends PIDSubsystem{
-		
+
 		/**
 		 * @param p proportional constant
 		 * @param i integral constant
@@ -340,7 +371,7 @@ public IntakePID(double p, double i, double d) {
 		protected double returnPIDInput(){
 			return encoder.get();
 		}
-		
+
 		/* (non-Javadoc)
 		 * @see edu.wpi.first.wpilibj.command.PIDSubsystem#usePIDOutput(double)
 		 */

test/org/impact2585/frc2016/tests/IntakeSystemTest.java

@@ -31,7 +31,7 @@ public class IntakeSystemTest {
 	private boolean turnLeverReverse;
 	private double leverSpeed;
 	private boolean turnLeverForward;
-	private boolean manualIntake;
+	private boolean toggleManualIntake;
 	private double rightArmSpeed;
 	private double leftArmSpeed;
 	private double leftIntakeSpeed;
@@ -242,20 +242,30 @@ public void test() {
 		Assert.assertTrue(leverSpeed == 0.5);
 
 		//tests if manual intake control works
-		manualIntake = true;
+		toggleManualIntake = true;
 		rightArmSpeed = 0.5;
 		leftArmSpeed = 0.5;
 		ioshooter.run();
 		Assert.assertTrue(rightIntakeSpeed == 0.5);
 		Assert.assertTrue(leftIntakeSpeed == 0.5);
 
-		//tests if manual intake can be disabled
-		manualIntake = false;
+		//tests if manual intake stays on
+		toggleManualIntake = true;
 		analogMoveArmAwayFromBot = 0;
 		analogMoveArmTowardsBot = 0;
 		digitalMoveArmAwayFromBot = false;
 		digitalMoveArmTowardsBot = false;
 		ioshooter.run();
+		Assert.assertTrue(rightIntakeSpeed == 0.5);
+		Assert.assertTrue(leftIntakeSpeed == 0.5);
+		toggleManualIntake = false;
+		ioshooter.run();
+		Assert.assertTrue(rightIntakeSpeed == 0.5);
+		Assert.assertTrue(leftIntakeSpeed == 0.5);
+
+		//tests if manual intake can be toggled off
+		toggleManualIntake = true;
+		ioshooter.run();
 		Assert.assertTrue(armSpeed == 0);
 	}
 
@@ -420,7 +430,7 @@ public boolean toggleSpeed() {
 		 * @see org.impact2585.frc2016.input.InputMethod#ignoreIntakeLimitSwitch()
 		 */
 		@Override
-		public boolean ignoreIntakeLimitSwitch(){
+		public boolean toggleIntakeLimitSwitch(){
 			return ignoreIntakeLimitSwitch;
 		}
 
@@ -469,7 +479,7 @@ public double leftIntake() {
 		 */
 		@Override
 		public boolean manualIntakeControl() {
-			return manualIntake;
+			return toggleManualIntake;
 		}
 
 	}