Merge branch 'develop-2-electric-boogaloo' into develop

2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java

@@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.buttons.*;
 import org.usfirst.frc4388.robot.subsystems.*;
 import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode;
+import org.usfirst.frc4388.robot.subsystems.Arm.PlaceMode;
 import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
 import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
 
@@ -78,6 +79,10 @@ public class OI
 			JoystickButton climbUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_TRIGGER_AXIS);
 			JoystickButton climbDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_TRIGGER_AXIS);
 
+			JoystickButton resetArmEncoder = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.START_BUTTON);
+			resetArmEncoder.whenPressed(new ResetArmEncoder());
+			resetArmEncoder.whenPressed(new ResetWristEncoder());
+
 			/** DEPRICATED, TRIGGERS ON THE DRIVER JOYSTICK COVER FOR THIS BUTTON */
 			//JoystickButton ratchetFlip = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.Y_BUTTON);
 			//ratchetFlip.toggleWhenPressed(new ratchetFlip(0.5));
@@ -107,17 +112,19 @@ public class OI
 
 
 			JoystickButton lowHeight = new JoystickButton(m_operatorXbox.getJoyStick(),XboxController.LEFT_JOYSTICK_BUTTON);
-			lowHeight.whenPressed(new GrabFromLoadingSatation());
+			lowHeight.whenPressed(new SetPositionArmWrist(550, 450));
 
-			JoystickButton stow = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON);
+			JoystickButton cargoPlaceMode = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON);
-			stow.whenPressed(new StowArm());
+			cargoPlaceMode.whenPressed(new SwitchArmPlaceMode(PlaceMode.CARGO));
-			stow.whenPressed(new setLEDPattern(LEDPatterns.SOLID_GREEN));
+			cargoPlaceMode.whenReleased(new SwitchArmPlaceMode(PlaceMode.HATCH));
+			//stow.whenPressed(new StowArm());
+			//stow.whenPressed(new setLEDPattern(LEDPatterns.SOLID_GREEN));
 
 			SmartDashboard.putData("switch to manuel", new SetManual());
 //			SmartDashboard.putData("run turn test", new TestTurn());
 			SmartDashboard.putData("grab from station", new GrabFromLoadingSatation());
 			SmartDashboard.putData("wrist test", new wristTest());
-			
+
 
 	        //SmartDashboard.putData("PRE GAME!!!!", new PreGame());
 		  } catch (Exception e) {

2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java

@@ -118,6 +118,8 @@ public class Robot extends TimedRobot
 		//System.err.println("TeleopInit after resetEncoders");
 		drive.endGyroCalibration();
 		System.err.println("TeleopInit after endGyroCalibrations");
+		arm.targetPositionInchesMM = arm.motor1.getPositionWorld();
+		wrist.targetPositionInchesPID = wrist.wristMotor1.getPositionWorld();
 
         updateStatus();
     }

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ResetArmEncoder.java

@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class ResetArmEncoder extends Command {
+  public ResetArmEncoder() {
+    requires(Robot.arm);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+    Robot.arm.resetEncoder();
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    return true;
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() {
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  @Override
+  protected void interrupted() {
+  }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ResetWristEncoder.java

@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class ResetWristEncoder extends Command {
+  public ResetWristEncoder() {
+    requires(Robot.wrist);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+    Robot.wrist.resetencoder();
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    return true;
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() {
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  @Override
+  protected void interrupted() {
+  }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetMMAndPID.java

@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode;
+import org.usfirst.frc4388.robot.subsystems.Wrist.WristControlMode;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class SetMMAndPID extends CommandGroup {
+  /**
+   * Add your docs here.
+   */
+  public SetMMAndPID() {
+    addSequential(new ArmSetMode(ArmControlMode.MOTION_MAGIC));
+    addParallel(new WristSetMode(WristControlMode.JOYSTICK_PID));
+    // Add Commands here:
+    // e.g. addSequential(new Command1());
+    // addSequential(new Command2());
+    // these will run in order.
+
+    // To run multiple commands at the same time,
+    // use addParallel()
+    // e.g. addParallel(new Command1());
+    // addSequential(new Command2());
+    // Command1 and Command2 will run in parallel.
+
+    // A command group will require all of the subsystems that each member
+    // would require.
+    // e.g. if Command1 requires chassis, and Command2 requires arm,
+    // a CommandGroup containing them would require both the chassis and the
+    // arm.
+  }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetPositionArmWrist.java

@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package org.usfirst.frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class SetPositionArmWrist extends CommandGroup {
+  /**
+   * Add your docs here.
+   */
+  public SetPositionArmWrist(double arm, double wrist) {
+    
+    addParallel(new WristSetPositionPID(wrist));
+    addSequential(new ArmSetPositionMM(arm));
+
+    // Add Commands here:
+    // e.g. addSequential(new Command1());
+    // addSequential(new Command2());
+    // these will run in order.
+
+    // To run multiple commands at the same time,
+    // use addParallel()
+    // e.g. addParallel(new Command1());
+    // addSequential(new Command2());
+    // Command1 and Command2 will run in parallel.
+
+    // A command group will require all of the subsystems that each member
+    // would require.
+    // e.g. if Command1 requires chassis, and Command2 requires arm,
+    // a CommandGroup containing them would require both the chassis and the
+    // arm.
+  }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java

@@ -17,8 +17,9 @@ public class StowArm extends CommandGroup {
   public StowArm() {
     addSequential(new HatchFlip(false));
     addParallel(new WristPlacement(true));
-    addParallel(new WristSetPositionPID(200), 2);
+    addParallel(new WristSetPositionPID(110), 2);
-    addSequential(new ArmSetPositionMM(-10), 4);
+    addSequential(new ArmSetPositionMM(420), 4);
+    addSequential(new ArmSetPositionMM(5));
     // Add Commands here:
     // e.g. addSequential(new Command1());
     // addSequential(new Command2());

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SwitchArmPlaceMode.java

@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.subsystems.Arm.PlaceMode;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+
+
+public class SwitchArmPlaceMode extends Command {
+
+  public PlaceMode placeMode;
+
+  public SwitchArmPlaceMode(PlaceMode placeMode) {
+    requires(Robot.arm);
+    this.placeMode = placeMode;
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+    Robot.arm.placeMode = placeMode;
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    return true;
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() {
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  @Override
+  protected void interrupted() {
+  }
+}

2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java

@@ -4,6 +4,9 @@ import java.util.ArrayList;
 import org.usfirst.frc4388.robot.Constants;
 import org.usfirst.frc4388.robot.Robot;
 import org.usfirst.frc4388.robot.RobotMap;
+import org.usfirst.frc4388.robot.commands.ArmSetPositionMM;
+import org.usfirst.frc4388.robot.commands.SetPositionArmWrist;
+import org.usfirst.frc4388.robot.commands.StowArm;
 import org.usfirst.frc4388.utility.ControlLoopable;
 import org.usfirst.frc4388.utility.Loop;
 import org.usfirst.frc4388.utility.MPTalonPIDController;
@@ -33,10 +36,11 @@ public class Arm extends Subsystem implements ControlLoopable
 	private static Arm instance;
 
 	public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL, MOTION_MAGIC};
+	public static enum PlaceMode { HATCH, CARGO };
+
+	// One revolution of the 30T Drive 1.880" PD pulley = Pi * PD inches = 36/24 revs due to pulleys * 34/24 revs due to gears * 36/12 revs due mag encoder gear on ball shifter * 4096 ticks
+	public static final double ENCODER_TICKS_TO_INCHES = (1);
 
-	// One revolution of the 30T Drive 1.880" PD pulley = Pi * PD inches = 36/24 revs due to pulleys * 34/24 revs due to gears * 36/12 revs due mag encoder gear on ball shifter * 4096 ticks 
-	public static final double ENCODER_TICKS_TO_INCHES = (1);  
-	
 
 	private double periodMs;
 	private double lastControlLoopUpdatePeriod = 0.0;
@@ -48,13 +52,13 @@ public class Arm extends Subsystem implements ControlLoopable
 	public static final double AUTO_ZERO_SPEED = -0.3;
 	public static final double JOYSTICK_INCHES_PER_MS_HI = 35;
 	public static final double JOYSTICK_INCHES_PER_MS_LO = 35;
-	
+
 	// Defined positions
 	public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0;
 	public static final double ZERO_POSITION_INCHES = -0.25;
 	public static final double NEAR_ZERO_POSITION_INCHES = 0.0;
-	public static final double MIN_POSITION_INCHES = -10;
+	public static final double MIN_POSITION_INCHES = -25;
-	public static final double MAX_POSITION_INCHES = 3900;
+	public static final double MAX_POSITION_INCHES = 4400;
 	public static final double AFTER_INTAKE_POSITION_INCHES = 4.0;
 
 	public static final double SWITCH_POSITION_INCHES = 24.0;
@@ -68,16 +72,16 @@ public class Arm extends Subsystem implements ControlLoopable
 	public static final double CLIMB_ASSIST_POSITION_INCHES = 50.0;
 
 	// Motion profile max velocities and accel times
-	public static final double MP_MAX_VELOCITY_INCHES_PER_SEC =  60; 
+	public static final double MP_MAX_VELOCITY_INCHES_PER_SEC =  60;
 	public static final double MP_T1 = 300;  // Fast = 300
 	public static final double MP_T2 = 150;  // Fast = 150
-	
-	// Motor controllers
-	private ArrayList<TalonSRXEncoder> motorControllers = new ArrayList<TalonSRXEncoder>();	
 
-	private TalonSRXEncoder motor1;
+	// Motor controllers
+	private ArrayList<TalonSRXEncoder> motorControllers = new ArrayList<TalonSRXEncoder>();
+
+	public TalonSRXEncoder motor1;
 	private TalonSRX motor2;
-	
+
 	// PID controller and params
 	private MPTalonPIDController mpController;
 
@@ -85,8 +89,8 @@ public class Arm extends Subsystem implements ControlLoopable
 	public static int MM_SLOT = 1;
 	public static int MP_SLOT = 2;
 
-	private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0);  
+	private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0);
-	private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0);  
+	private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0);
 	public static final double KF_UP = 1;//0.01;
 	public static final double KF_DOWN = 0;// 0.0;
 	public static final double P_Value = 0.5;// 2;
@@ -104,34 +108,35 @@ public class Arm extends Subsystem implements ControlLoopable
 	private PIDParams armPIDParams = new PIDParams(P_Value, I_Value, D_Value, KF_DOWN);	// KF gets updated later
 	public static final double PID_ERROR_INCHES = 5;
 	LimitSwitchSource limitSwitchSource;
-	
+
 	// Pneumatics
 	private Solenoid speedShift;
 
 	// Misc
-	public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0;	
+	public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0;
 	private boolean isFinished;
 	private ArmControlMode armControlMode = ArmControlMode.MOTION_MAGIC;
-	private double targetPositionInchesPID = 0;
+	public PlaceMode placeMode = PlaceMode.HATCH;
-	private double targetPositionInchesMM = 0;
+	public double targetPositionInchesPID = 0;
+	public double targetPositionInchesMM = 0;
 	private boolean firstMpPoint;
 	private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
 	private double p = 0;
 
-	
+
 	public Arm() {
 		try {
 			motor1 = TalonSRXFactory.createTalonEncoder(RobotMap.ARM_MOTOR1_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder);
 			motor2 = TalonSRXFactory.createPermanentSlaveTalon(RobotMap.ARM_MOTOR2_ID, RobotMap.ARM_MOTOR1_ID);
-			
+
-			
+
 			motor1.setInverted(true);
 			motor2.setInverted(true);
-									
+
 //	        if (motor1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) {
 //	            Driver.reportError("Could not detect elevator motor 1 encoder encoder!", false);
 //	        }
-			
+
 			motor1.configNominalOutputForward(0, TalonSRXEncoder.TIMEOUT_MS);
 			motor1.configNominalOutputReverse(0, TalonSRXEncoder.TIMEOUT_MS);
 			motor1.configPeakOutputForward(1, TalonSRXEncoder.TIMEOUT_MS);
@@ -150,8 +155,8 @@ public class Arm extends Subsystem implements ControlLoopable
 			motor1.enableCurrentLimit(true);
 			motorControllers.add(motor1);
 			//motor1.setSelectedSensorPosition(0, , );
-			
+
-			
+
 		}
 		catch (Exception e) {
 			System.err.println("An error occurred in the Arm constructor");
@@ -161,19 +166,22 @@ public class Arm extends Subsystem implements ControlLoopable
 	@Override
 	public void initDefaultCommand() {
 	}
-		
+
 	public void resetZeroPosition(double position) {
 		mpController.resetZeroPosition(position);
-	}	
+	}
+
 	public void resetEncoder(){
 		motor1.setPosition(0);
+		targetPositionInchesMM = 0;
+		targetPositionInchesPID = 0;
 	}
-	
+
 	private synchronized void setArmControlMode(ArmControlMode controlMode) {
 		this.armControlMode = controlMode;
 	}
-	
+
-	public synchronized ArmControlMode getArmControlMode() {
+	private synchronized ArmControlMode getArmControlMode() {
 		return this.armControlMode;
 	}
 
@@ -181,7 +189,7 @@ public class Arm extends Subsystem implements ControlLoopable
 		motor1.set(ControlMode.PercentOutput, speed);
 		setArmControlMode(ArmControlMode.MANUAL);
 	}
-		
+
 	public void setSpeedJoystick(double speed) {
 		motor1.set(ControlMode.PercentOutput, speed);
 		setArmControlMode(ArmControlMode.JOYSTICK_MANUAL);
@@ -193,9 +201,6 @@ public class Arm extends Subsystem implements ControlLoopable
 		setArmControlMode(ArmControlMode.MOTION_MAGIC);
 		updatePositionMM(targetPositionInches);
 		setFinished(false);
-
-
-
 	}
 
 	public double calcGravityCompensationAtCurrentPosition() {
@@ -215,25 +220,25 @@ public class Arm extends Subsystem implements ControlLoopable
 		//System.err.println(motor1.getControlMode());
 		motor1.configMotionCruiseVelocity(CV_value, TalonSRXEncoder.TIMEOUT_MS);
 		motor1.configMotionAcceleration(A_value, TalonSRXEncoder.TIMEOUT_MS);
-			
+
 
 	}
-		
+
 	public void setPositionPID(double targetPositionInches) {
 		motor1.set(ControlMode.Position, targetPositionInches);
 		mpController.setPIDSlot(PID_SLOT);	//TODO: verify that motor's selectProfileSlot() should be called AFTER its control mode is set
 		updatePositionPID(targetPositionInches);
-		setArmControlMode(ArmControlMode.JOYSTICK_PID);	
+		setArmControlMode(ArmControlMode.JOYSTICK_PID);
 		setFinished(false);
 	}
-	
+
 	public void updatePositionPID(double targetPositionInches) {
 		targetPositionInchesPID = limitPosition(targetPositionInches);
 		if (limitPosition(motor1.getPositionWorld()) == MIN_POSITION_INCHES){
 			resetEncoder();
 		}
 		double startPositionInches = motor1.getPositionWorld();
-		//mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); 
+		//mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN);
 		motor1.set(ControlMode.Position, targetPositionInches);
 		motor1.configClosedloopRamp(0);
 		//motor1.configPeakCurrentLimit(5);
@@ -245,23 +250,23 @@ public class Arm extends Subsystem implements ControlLoopable
 		//System.err.println(motor1.getControlMode());
 		//System.err.print(motor1.getClosedLoopError());
 	}
-	
+
 	public void setPositionMP(double targetPositionInches) {
 		double startPositionInches = motor1.getPositionWorld();
-		mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2); 
+		mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2);
 		setFinished(false);
 		firstMpPoint = true;
 		setArmControlMode(ArmControlMode.MOTION_PROFILE);
  	}
-	
+
 	private double limitPosition(double targetPosition) {
-		if (targetPosition < MIN_POSITION_INCHES) {
+		/*if (targetPosition < MIN_POSITION_INCHES) {
 			return MIN_POSITION_INCHES;
-		}
+		}*/
-		else if (targetPosition > MAX_POSITION_INCHES) {
+		if (targetPosition > MAX_POSITION_INCHES) {
 			return MAX_POSITION_INCHES;
 		}
-		
+
 		return targetPosition;
 	}
 	@Override
@@ -273,9 +278,38 @@ public class Arm extends Subsystem implements ControlLoopable
 		this.periodMs = periodMs;
 	}
 
+	private int lastDPadAngle = -1;
+	public void dPadButtons(){
+		int dPadAngle = Robot.oi.getOperatorController().getDpadAngle();
+		if (placeMode == PlaceMode.HATCH){
+			if (dPadAngle == 0 && lastDPadAngle == -1){
+				new SetPositionArmWrist(3605, 1144).start();
+			}
+			if (dPadAngle == 90 && lastDPadAngle == -1){
+				new SetPositionArmWrist(2000, 750).start();
+			}
+			if (dPadAngle == 180 && lastDPadAngle == -1){
+				new SetPositionArmWrist(590, 450).start();
+			}
+		}
+		if (placeMode == PlaceMode.CARGO) {
+			if (dPadAngle == 0 && lastDPadAngle == -1){
+				new SetPositionArmWrist(4298, 3243).start();
+			}
+			if (dPadAngle == 90 && lastDPadAngle == -1){
+				new SetPositionArmWrist(2830, 2830).start();
+			}
+			if (dPadAngle == 180 && lastDPadAngle == -1){
+				new SetPositionArmWrist(1388, 2500).start();
+			}
+		}
 
-
+		if (dPadAngle == 270 && lastDPadAngle == -1){
-
+			new StowArm().start();
+		}
+		SmartDashboard.putNumber("DPad Angle", dPadAngle);
+		lastDPadAngle = dPadAngle;
+	}
 
 	public synchronized void controlLoopUpdate() {
 		// Measure *actual* update period
@@ -284,7 +318,9 @@ public class Arm extends Subsystem implements ControlLoopable
 			lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp;
 		}
 		lastControlLoopUpdateTimestamp = currentTimestamp;
-		
+
+		dPadButtons();
+
 		if (motor1.getSensorCollection().isRevLimitSwitchClosed()){
 			resetEncoder();
 		}
@@ -296,8 +332,8 @@ public class Arm extends Subsystem implements ControlLoopable
 		}
 		else if (!isFinished) {
 			if (armControlMode == ArmControlMode.MOTION_PROFILE) {
-				isFinished = mpController.controlLoopUpdate(getPositionInches()); 
+				isFinished = mpController.controlLoopUpdate(getPositionInches());
-				
+
 			}
 			if (armControlMode == ArmControlMode.JOYSTICK_PID){
 					controlPidWithJoystick();
@@ -307,13 +343,13 @@ public class Arm extends Subsystem implements ControlLoopable
 				controlMMWithJoystick();
 				System.err.println(motor1.getControlMode());
 			}
-			
+
 			/*else if (armControlMode == ArmControlMode.MP_PATH_VELOCITY) {
-				isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); 
+				isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg());
 			}
 			else if (armControlMode == ArmControlMode.ADAPTIVE_PURSUIT) {
 				updatePose();
-				isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); 
+				isFinished = adaptivePursuitController.controlLoopUpdate(currentPose);
 			}*/
 		}
 	}
@@ -323,7 +359,7 @@ public class Arm extends Subsystem implements ControlLoopable
 
 
 
-	
+
 	private void controlPidWithJoystick() {
 		double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis();
 		double deltaPosition = joystickPosition * joystickInchesPerMs;
@@ -335,8 +371,10 @@ public class Arm extends Subsystem implements ControlLoopable
 		double deltaPosition = joystickPosition * joystickInchesPerMs;
 		targetPositionInchesMM = targetPositionInchesMM + deltaPosition;
 		updatePositionMM(targetPositionInchesMM);
-		
+		//Robot.wrist.targetPositionInchesPID = targetPositionInchesPID - (deltaPosition/3);
-		
+		Robot.wrist.updatePositionPID(Robot.wrist.targetPositionInchesPID);
+
+
 	}
 
 	private void ControlWithJoystickhold(){
@@ -344,7 +382,7 @@ public class Arm extends Subsystem implements ControlLoopable
 		updatePositionPID(holdPosition);
 
 	}
-	
+
 	private void controlManualWithJoystick() {
 		double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
 		setSpeedJoystick((joyStickSpeed*.3)+.08);
@@ -362,7 +400,7 @@ public class Arm extends Subsystem implements ControlLoopable
 			mpController.setPID(pidPIDParamsLoGear, PID_SLOT);
 		}
 	}
-	
+
 	public ElevatorSpeedShiftState getShiftState() {
 		return shiftState;
 	}
@@ -370,31 +408,31 @@ public class Arm extends Subsystem implements ControlLoopable
 	public double getPositionInches() {
 		return motor1.getPositionWorld();
 	}
-	
+
 //	public double getAverageMotorCurrent() {
 //		return (Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID)) / 3;
 //	}
-		
+
 	public double getAverageMotorCurrent() {
 		return (motor1.getOutputCurrent() + motor2.getOutputCurrent()) / 2;
 	}
-		
+
 	public synchronized boolean isFinished() {
 		return isFinished;
 	}
-	
+
 	public synchronized void setFinished(boolean isFinished) {
 		this.isFinished = isFinished;
 	}
-		
+
 	public double getPeriodMs() {
 		return periodMs;
 	}
-	
+
 	public void updateStatus(Robot.OperationMode operationMode) {
 		//System.err.println("the encoder is right after this");
 			try {
-				
+
 				SmartDashboard.putNumber("Arm Position Ticks", motor1.getPositionWorld());
 				//SmartDashboard.putNumber("Arm Motor 1 Amps", motor1.getOutputCurrent());
 				//SmartDashboard.putNumber("Arm Motor 2 Amps", motor2.getOutputCurrent());
@@ -412,9 +450,9 @@ public class Arm extends Subsystem implements ControlLoopable
 			catch (Exception e) {
 				System.err.println("Arm update status error" +e.getMessage());
 			}
-		
+
-	}	
+	}
-	
+
 	public static Arm getInstance() {
 		if(instance == null) {
 			instance = new Arm();

2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java

@@ -32,9 +32,9 @@ public class Wrist extends Subsystem implements ControlLoopable
 
 	public static enum WristControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL };
 
-	// One revolution of the 30T Drive 1.880" PD pulley = Pi * PD inches = 36/24 revs due to pulleys * 34/24 revs due to gears * 36/12 revs due mag encoder gear on ball shifter * 4096 ticks 
+	// One revolution of the 30T Drive 1.880" PD pulley = Pi * PD inches = 36/24 revs due to pulleys * 34/24 revs due to gears * 36/12 revs due mag encoder gear on ball shifter * 4096 ticks
-	public static final double ENCODER_TICKS_TO_INCHES = (1);  
+	public static final double ENCODER_TICKS_TO_INCHES = (1);
-	
+
 
 	private double periodMs;
 	private double lastControlLoopUpdatePeriod = 0.0;
@@ -46,7 +46,7 @@ public class Wrist extends Subsystem implements ControlLoopable
 	public static final double AUTO_ZERO_SPEED = -0.3;
 	public static final double JOYSTICK_INCHES_PER_MS_HI = 20;
 	public static final double JOYSTICK_INCHES_PER_MS_LO = 27;
-	
+
 	// Defined positions
 	public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0;
 	public static final double ZERO_POSITION_INCHES = -0.25;
@@ -66,23 +66,23 @@ public class Wrist extends Subsystem implements ControlLoopable
 	public static final double CLIMB_ASSIST_POSITION_INCHES = 50.0;
 
 	// Motion profile max velocities and accel times
-	public static final double MP_MAX_VELOCITY_INCHES_PER_SEC =  60; 
+	public static final double MP_MAX_VELOCITY_INCHES_PER_SEC =  60;
 	public static final double MP_T1 = 400;  // Fast = 300
 	public static final double MP_T2 = 150;  // Fast = 150
-	
-	// Motor controllers
-	private ArrayList<TalonSRXEncoder> motorControllers = new ArrayList<TalonSRXEncoder>();	
 
-	private TalonSRXEncoder wristMotor1;
+	// Motor controllers
-	
+	private ArrayList<TalonSRXEncoder> motorControllers = new ArrayList<TalonSRXEncoder>();
+
+	public TalonSRXEncoder wristMotor1;
+
 	// PID controller and params
 	private MPTalonPIDController mpController;
 
 	public static int PID_SLOT = 0;
 	public static int MP_SLOT = 1;
 
-	private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0);  
+	private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0);
-	private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0);  
+	private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0);
 	public static final double KF_UP = 0.01;
 	public static final double KF_DOWN = 0.0;
 	public static final double P_Value = 1;
@@ -96,33 +96,33 @@ public class Wrist extends Subsystem implements ControlLoopable
 	private Solenoid speedShift;
 
 	// Misc
-	public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0;	
+	public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0;
 	private boolean isFinished;
 	private WristControlMode wristControlMode = WristControlMode.JOYSTICK_PID;
-	private double targetPositionInchesPID = 0;
+	public double targetPositionInchesPID = 0;
 	private boolean firstMpPoint;
 	private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
 	private double p = 0;
 
-	
+
 	public Wrist() {
 		try {
-			wristMotor1 = TalonSRXFactory.createTalonEncoder(RobotMap.WRIST_MOTOR_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder);	
+			wristMotor1 = TalonSRXFactory.createTalonEncoder(RobotMap.WRIST_MOTOR_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder);
 			wristMotor1.setInverted(false);
 
-									
+
 //	        if (wristMotor1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) {
 //	            Driver.reportError("Could not detect elevator motor 1 encoder encoder!", false);
 //	        }
-			
+
 			wristMotor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
 			wristMotor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
 			wristMotor1.setNeutralMode(NeutralMode.Brake);
 			wristMotor1.enableCurrentLimit(true);
 			//wristMotor1.setSensorPhase(true);
 			motorControllers.add(wristMotor1);
-			
+
-			
+
 		}
 		catch (Exception e) {
 			System.err.println("An error occurred in the Wrist constructor");
@@ -132,19 +132,20 @@ public class Wrist extends Subsystem implements ControlLoopable
 	@Override
 	public void initDefaultCommand() {
 	}
-		
+
 	public void resetZeroPosition(double position) {
 		mpController.resetZeroPosition(position);
-	}	
+	}
 	public void resetencoder(){
 		wristMotor1.setPosition(0);
+		targetPositionInchesPID = 0;
 	}
-	
+
-	
+
 	private synchronized void setWristControlMode(WristControlMode controlMode) {
 		this.wristControlMode = controlMode;
 	}
-	
+
 	private synchronized WristControlMode getWristControlMode() {
 		return this.wristControlMode;
 	}
@@ -153,24 +154,24 @@ public class Wrist extends Subsystem implements ControlLoopable
 		wristMotor1.set(ControlMode.PercentOutput, speed);
 		setWristControlMode(WristControlMode.MANUAL);
 	}
-		
+
 	public void setSpeedJoystick(double speed) {
 		wristMotor1.set(ControlMode.PercentOutput, speed);
 		setWristControlMode(WristControlMode.JOYSTICK_MANUAL);
 	}
-		
+
 	public void setPositionPID(double targetPositionInches) {
 		wristMotor1.set(ControlMode.Position, targetPositionInches);
 		mpController.setPIDSlot(PID_SLOT);	//TODO: verify that motor's selectProfileSlot() should be called AFTER its control mode is set
 		updatePositionPID(targetPositionInches);
-		setWristControlMode(WristControlMode.JOYSTICK_PID);	
+		setWristControlMode(WristControlMode.JOYSTICK_PID);
 		setFinished(false);
 	}
-	
+
 	public void updatePositionPID(double targetPositionInches) {
  		targetPositionInchesPID = limitPosition(targetPositionInches);
 		double startPositionInches = wristMotor1.getPositionWorld();
-		//mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); 
+		//mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN);
 		wristMotor1.set(ControlMode.Position, targetPositionInches);
 		wristMotor1.configClosedloopRamp(.02);
 		//wristMotor1.configPeakCurrentLimit(5);
@@ -182,23 +183,24 @@ public class Wrist extends Subsystem implements ControlLoopable
 		//System.err.println(wristMotor1.getControlMode());
 		//System.err.print(wristMotor1.getClosedLoopError());
 	}
-	
+
 	public void setPositionMP(double targetPositionInches) {
 		double startPositionInches = wristMotor1.getPositionWorld();
-		mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2); 
+		mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2);
 		setFinished(false);
 		firstMpPoint = true;
 		setWristControlMode(WristControlMode.MOTION_PROFILE);
  	}
-	
+
 	private double limitPosition(double targetPosition) {
+
 		if (targetPosition < MIN_POSITION_INCHES) {
 			return MIN_POSITION_INCHES;
 		}
 		else if (targetPosition > MAX_POSITION_INCHES) {
 			return MAX_POSITION_INCHES;
 		}
-		
+
 		return targetPosition;
 	}
 	@Override
@@ -222,26 +224,26 @@ public class Wrist extends Subsystem implements ControlLoopable
 	@Override
 	public void onStop(double timestamp) {
 		// TODO Auto-generated method stub
-		
+
 	}
 
 	@Override
 	public void onLoop(double timestamp) {
 		synchronized (Wrist.this) {
 			switch( getElevatorControlMode() ) {
-				case JOYSTICK_PID: 
+				case JOYSTICK_PID:
 					controlPidWithJoystick();
 					break;
 				case JOYSTICK_MANUAL:
 					controlManualWithJoystick();
 					break;
-				case MOTION_PROFILE: 
+				case MOTION_PROFILE:
 					if (!isFinished()) {
 						if (firstMpPoint) {
 							mpController.setPIDSlot(MP_SLOT);
 							firstMpPoint = false;
 						}
-						setFinished(mpController.controlLoopUpdate()); 
+						setFinished(mpController.controlLoopUpdate());
 						if (isFinished()) {
 							mpController.setPIDSlot(PID_SLOT);
 						}
@@ -266,30 +268,30 @@ public class Wrist extends Subsystem implements ControlLoopable
 			lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp;
 		}
 		lastControlLoopUpdateTimestamp = currentTimestamp;
-		
+
 		// Do the update
 		if (wristControlMode == WristControlMode.JOYSTICK_MANUAL) {
 			controlManualWithJoystick();
-			
+
 		}
 		else if (!isFinished) {
 			if (wristControlMode == WristControlMode.MOTION_PROFILE) {
-				isFinished = mpController.controlLoopUpdate(getPositionInches()); 
+				isFinished = mpController.controlLoopUpdate(getPositionInches());
-				
+
 			}
 			if (wristControlMode == WristControlMode.JOYSTICK_PID){
 				//System.err.println(wristMotor1.getControlMode());
 				controlPidWithJoystick();
-				
+
-	
+
 			}
-			
+
 			/*else if (wristControlMode == WristControlMode.MP_PATH_VELOCITY) {
-				isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); 
+				isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg());
 			}
 			else if (wristControlMode == WristControlMode.ADAPTIVE_PURSUIT) {
 				updatePose();
-				isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); 
+				isFinished = adaptivePursuitController.controlLoopUpdate(currentPose);
 			}*/
 		}
 	}
@@ -299,14 +301,14 @@ public class Wrist extends Subsystem implements ControlLoopable
 
 
 
-	
+
 	private void controlPidWithJoystick() {
 		double joystickPosition = -Robot.oi.getOperatorController().getRightYAxis();
 		double deltaPosition = joystickPosition * joystickInchesPerMs;
 		targetPositionInchesPID = targetPositionInchesPID + deltaPosition;
 		updatePositionPID(targetPositionInchesPID);
-		
+
-		
+
 	}
 
 	private void ControlWithJoystickhold(){
@@ -314,7 +316,7 @@ public class Wrist extends Subsystem implements ControlLoopable
 		updatePositionPID(holdPosition);
 
 	}
-	
+
 	private void controlManualWithJoystick() {
 		double joyStickSpeed = -Robot.oi.getOperatorController().getRightYAxis();
 		setSpeedJoystick(joyStickSpeed*.5);
@@ -332,7 +334,7 @@ public class Wrist extends Subsystem implements ControlLoopable
 			mpController.setPID(pidPIDParamsLoGear, PID_SLOT);
 		}
 	}
-	
+
 	public ElevatorSpeedShiftState getShiftState() {
 		return shiftState;
 	}
@@ -340,34 +342,34 @@ public class Wrist extends Subsystem implements ControlLoopable
 	public double getPositionInches() {
 		return wristMotor1.getPositionWorld();
 	}
-	
+
 //	public double getAverageMotorCurrent() {
 //		return (Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID)) / 3;
 //	}
-		
 
-		
+
+
 	public synchronized boolean isFinished() {
 		return isFinished;
 	}
-	
+
 	public synchronized void setFinished(boolean isFinished) {
 		this.isFinished = isFinished;
 	}
-		
+
 	public double getPeriodMs() {
 		return periodMs;
 	}
-	
+
 	public void updateStatus(Robot.OperationMode operationMode) {
 		//System.err.println("the encoder is right after this");
 			try {
-				
+
 				SmartDashboard.putNumber("Wrist Position Ticks", wristMotor1.getPositionWorld());
 				SmartDashboard.putNumber("Wrist Motor 1 Amps", wristMotor1.getOutputCurrent());
 				SmartDashboard.putNumber("wrist pid error", wristMotor1.getClosedLoopError());
 				SmartDashboard.putNumber("wrist motor output", wristMotor1.getMotorOutputPercent());
-				
+
 //				SmartDashboard.putNumber("Elevator Motor 1 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID));
 //				SmartDashboard.putNumber("Elevator Motor 2 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID));
 //				SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID));
@@ -376,9 +378,9 @@ public class Wrist extends Subsystem implements ControlLoopable
 			catch (Exception e) {
 				System.err.println("Drivetrain update status error" +e.getMessage());
 			}
-		
+
-	}	
+	}
-	
+
 	public static Wrist getInstance() {
 		if(instance == null) {
 			instance = new Wrist();