added changes from 3/18/19

changes for drivers.

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

@@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.GenericHID;
 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.utility.MPSoftwarePIDController.MPSoftwareTurnType;
 import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
 
@@ -79,7 +80,7 @@ public class OI
 
 			/** DEPRICATED, TRIGGERS ON THE DRIVER JOYSTICK COVER FOR THIS BUTTON */
 			//JoystickButton ratchetFlip = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.Y_BUTTON);
-			//ratchetFlip.whenPressed(new ratchetFlip(0.5));
+			//ratchetFlip.toggleWhenPressed(new ratchetFlip(0.5));
 			//ratchetFlip.whenReleased(new ratchetFlip(0));
 
 	        JoystickButton shiftUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
@@ -89,7 +90,10 @@ public class OI
 	        JoystickButton shiftDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON);
 	        shiftDown.whenPressed(new DriveSpeedShift(false));
 	       // shiftDown.whenPressed(new LEDIndicators(false));
-	        //Operator Xbox
+			//Operator Xbox
+			JoystickButton help = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.X_BUTTON);
+			help.whenPressed(new ArmSetMode(ArmControlMode.JOYSTICK_MANUAL));
+			help.whenReleased(new ArmSetMode(ArmControlMode.MOTION_MAGIC));
 			/*
 	        JoystickButton openIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON);
 			openIntake.whenPressed(new IntakePosition(true));
@@ -113,6 +117,7 @@ public class OI
 //			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/commands/ArmSetMode.java

@@ -24,7 +24,10 @@ public class ArmSetMode extends Command {
     	}
     	else if (controlMode == ArmControlMode.JOYSTICK_MANUAL) {
     		Robot.arm.setSpeedJoystick(0);
-    	}
+        }
+        else if (controlMode == ArmControlMode.MOTION_MAGIC){
+            Robot.arm.setPositionMM(Robot.arm.getPositionInches());
+        }
     	else {
     		Robot.arm.setSpeed(0.0);
     	}

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

@@ -16,6 +16,7 @@ public class HatchFlip extends Command
 	@Override
 	protected void initialize() {
 		Robot.pnumatics.setHatchIntakeState(PickingUp);
+
 	}
 
 	@Override

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

@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+
+public class togglemanuel extends Command {
+  public togglemanuel() {
+    System.err.print("togle");
+    if (Robot.arm.getArmControlMode() == ArmControlMode.MOTION_MAGIC){
+      new ArmSetMode(ArmControlMode.JOYSTICK_MANUAL);
+      System.err.print("manuel");
+    }
+    else if (Robot.arm.getArmControlMode() == ArmControlMode.JOYSTICK_MANUAL){
+      new ArmSetMode(ArmControlMode.MOTION_MAGIC);
+      System.err.print("Smart control");
+    }
+    else{
+      new ArmSetMode(ArmControlMode.JOYSTICK_MANUAL);
+    }
+
+    
+  }
+    
+
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
+  
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+    System.err.print("pls work");
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+    
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    return false;
+  }
+
+  // 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

@@ -173,7 +173,7 @@ public class Arm extends Subsystem implements ControlLoopable
 		this.armControlMode = controlMode;
 	}
 	
-	private synchronized ArmControlMode getArmControlMode() {
+	public synchronized ArmControlMode getArmControlMode() {
 		return this.armControlMode;
 	}
 
@@ -188,7 +188,7 @@ public class Arm extends Subsystem implements ControlLoopable
 	}
 	public void setPositionMM(double targetPositionInches){
 		motor1.set(ControlMode.MotionMagic, targetPositionInches);
-		System.err.println(motor1.getControlMode());
+		//System.err.println(motor1.getControlMode());
 		motor1.selectProfileSlot(MM_SLOT, 0);
 		setArmControlMode(ArmControlMode.MOTION_MAGIC);
 		updatePositionMM(targetPositionInches);
@@ -202,7 +202,7 @@ public class Arm extends Subsystem implements ControlLoopable
 		int ticks = motor1.getSelectedSensorPosition();
 		double degreesFromDown = (ticks+920)*(360.0/(4096*3));
 		double compensation = maxGravityComp * Math.sin(degreesFromDown*Math.PI/180);
-		System.err.println("comp(" + degreesFromDown + "^) = " + compensation);
+		//System.err.println("comp(" + degreesFromDown + "^) = " + compensation);
 		return compensation;
 	}
 	public void updatePositionMM(double targetPositionInches){
@@ -292,6 +292,7 @@ public class Arm extends Subsystem implements ControlLoopable
 		// Do the update
 		if (armControlMode == ArmControlMode.JOYSTICK_MANUAL) {
 			controlManualWithJoystick();
+			System.err.println(motor1.getControlMode());
 		}
 		else if (!isFinished) {
 			if (armControlMode == ArmControlMode.MOTION_PROFILE) {
@@ -304,7 +305,7 @@ public class Arm extends Subsystem implements ControlLoopable
 			}
 			if (armControlMode == ArmControlMode.MOTION_MAGIC){
 				controlMMWithJoystick();
-				//System.err.println(motor1.getControlMode());
+				System.err.println(motor1.getControlMode());
 			}
 			
 			/*else if (armControlMode == ArmControlMode.MP_PATH_VELOCITY) {

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

@@ -52,7 +52,7 @@ public class Wrist extends Subsystem implements ControlLoopable
 	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 = 0.0;
-	public static final double MAX_POSITION_INCHES = 4096;
+	public static final double MAX_POSITION_INCHES = 3350;
 	public static final double AFTER_INTAKE_POSITION_INCHES = 4.0;
 
 	public static final double SWITCH_POSITION_INCHES = 24.0;
@@ -278,7 +278,7 @@ public class Wrist extends Subsystem implements ControlLoopable
 				
 			}
 			if (wristControlMode == WristControlMode.JOYSTICK_PID){
-				System.err.println(wristMotor1.getControlMode());
+				//System.err.println(wristMotor1.getControlMode());
 				controlPidWithJoystick();