adding in drive with arm pid and man

change to oi, arm, add arm set mode

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

@@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj.buttons.*;
 import org.usfirst.frc4388.robot.subsystems.*;
 import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
 import org.usfirst.frc4388.robot.subsystems.Drive;
+import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode;
 import org.usfirst.frc4388.robot.subsystems.Wrist.WristControlMode;
 import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
 
@@ -43,12 +44,7 @@ public class OI
 			m_operatorXbox = new XboxController(RobotMap.OPERATOR_JOYSTICK_1_USB_ID);
 			
 	      /*  XBoxTriggerButton CarriageIntake = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.LEFT_TRIGGER);
-	        CarriageIntake.whenPressed(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
-	        CarriageIntake.whenReleased(new IntakeSetSpeed(0.0));
-	        
-	        XBoxTriggerButton CarriageEject = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.RIGHT_TRIGGER);
-	        CarriageEject.whenPressed(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED));
-	        CarriageEject.whenReleased(new IntakeSetSpeed(0.0));
+
 	        */
 	        JoystickButton climbUp = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON);
 	        climbUp.whenPressed(new InitiateClimber(true));
@@ -66,7 +62,11 @@ public class OI
 		   JoystickButton wristManualMode = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.A_BUTTON);
 		   wristManualMode.whenPressed(new WristSetMode(WristControlMode.JOYSTICK_MANUAL));
 			
-		   
+		   JoystickButton ArmAimAssist = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_JOYSTICK_BUTTON);
+		   ArmAimAssist.whenPressed(new ArmSetMode(ArmControlMode.PID));
+
+
+
 // uncoment the line above
 
 

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

@@ -0,0 +1,47 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * Description 
+ */
+public class ArmSetMode extends Command {
+
+	private ArmControlMode controlMode;
+	
+    public ArmSetMode(ArmControlMode controlMode) {
+    	this.controlMode = controlMode;
+        requires(Robot.arm);
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    	if (controlMode == ArmControlMode.PID) {
+    		Robot.wrist.setPositionPID(27);
+    	}
+    	else if (controlMode == ArmControlMode.JOYSTICK_MANUAL) {
+    		Robot.wrist.setSpeedJoystick(0);
+    	}
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    protected void execute() {
+    }
+
+    // Make this return true when this Command no longer needs to run execute()
+    protected boolean isFinished() {
+        return true;
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    }
+}

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

@@ -28,8 +28,6 @@ public class Arm extends Subsystem implements Loop
 	// One revolution of the 1-3 GEAR RATION ON THE ARM	* 4096 ticks 
 	public static final double ENCODER_TICKS_TO_DEGREES = (36.0 / 12.0) * (36.0 / 24.0) * (34.0 / 24.0) * 4096.0 / (1.88 * Math.PI);   
 	
-	// Defined speeds
-	public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75;
 	
 	// Defined positions
 	public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0;
@@ -151,19 +149,18 @@ public class Arm extends Subsystem implements Loop
 				case PID: 
 					controlPidWithJoystick();
 					break;
-				case JOYSTICK_MANUAL:
+				default: JOYSTICK_MANUAL:
 					controlManualWithJoystick();
 					break;
-				default:
-					break;
+				
 			}
 		}
 	}
 	
 	private void controlPidWithJoystick() {
-		double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis();
-		double deltaPosition = joystickPosition *.5;
-		targetPositionInchesPID = targetPositionInchesPID + deltaPosition;
+		//double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis();
+		//double deltaPosition = joystickPosition *.5;
+		targetPositionInchesPID = targetPositionInchesPID;// + deltaPosition;
 		updatePositionPID(targetPositionInchesPID);
 	}