Added to PID loop for wrist

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

@@ -2,18 +2,16 @@
 package org.usfirst.frc4388.robot;
 
 
-public class RobotMap {
+public class RobotMap 
+{
 	// USB Port IDs
 	public static final int DRIVER_JOYSTICK_1_USB_ID = 0;
 	public static final int OPERATOR_JOYSTICK_1_USB_ID = 1;
 
-
 	// MOTORS 
-	
 	public static final int DRIVETRAIN_LEFT_MOTOR1_CAN_ID = 2;
 	public static final int DRIVETRAIN_LEFT_MOTOR2_CAN_ID = 3;
 
-
 	public static final int DRIVETRAIN_RIGHT_MOTOR1_CAN_ID = 4;
 	public static final int DRIVETRAIN_RIGHT_MOTOR2_CAN_ID = 5;
 	
@@ -29,8 +27,6 @@ public class RobotMap {
 	public static final int CLIMBER_LEFT = 12;
 	public static final int CLIMBER_RIGHT = 13;
 
-	
-	
 	// Pneumatics
 	public static final int DRIVETRAIN_SPEEDSHIFT_PCM_ID = 0;
 	public static final int DRIVETRAIN_SPEEDSHIFT2_PCM_ID = 1;
@@ -40,7 +36,4 @@ public class RobotMap {
 	public static final int LOWER_HATCH_INTAKE_PCM_ID = 5;
 	public static final int END_EFECTOR_EXPAND_PCM_ID = 6;
 	public static final int END_EFECTOR_CONTRACT_PCM_ID = 7;
-	
-
-
 }

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

@@ -79,6 +79,8 @@ public class Wrist extends Subsystem
 
   public static final double jumpBarAngle = 50;   //hard limit switch?
   public static final double armAngleForPIDSwitch = -45;   ///Change values
+
+  public static final boolean ballIntakeOut = true;
   
   //Misc
   private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL;
@@ -185,14 +187,21 @@ public class Wrist extends Subsystem
 		synchronized (Wrist.this) {
 			switch(getWristControlMode() ) {
         case PID:
+        
           if(armAngleDegrees > armAngleForPIDSwitch)
           {
             controlPID();
           } 
           else if(armAngleDegrees <= armAngleForPIDSwitch)
           {
+            if(ballIntakeOut)
+            {
+            }
+            else
+            {
+            }
           }
-					controlPID();
+
 					break;
 				case JOYSTICK_MANUAL:
 					controlManualWithJoystick();
@@ -240,11 +249,25 @@ public class Wrist extends Subsystem
   public void controlPIDBallIn()
   {
     updatePositionPID(targetAngleDegreesBallIn);
+    //Needs limit to lift up (so it can get over bar) in command group
   }
 
   public void controlPIDBallOut()
   {
+    updatePositionPID(targetAngleDegreesBallOut);
+    //Needs to be tuned a lot
+  }
 
+  public void controlPIDHatchIn()
+  {
+    updatePositionPID(targetAngleDegreesHatchIn);
+    //Needs limit to lift up (so it can get over bar) in command group
+  }
+
+  public void controlPIDHatchOut()
+  {
+    updatePositionPID(targetAngleDegreesHatchOut);
+    //Needs to be tuned a lot
   }
 
   public synchronized boolean isFinished()