Wrist PID update

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

@@ -7,6 +7,7 @@
 
 package org.usfirst.frc4388.robot.commands;
 
+import org.usfirst.frc4388.robot.Robot;
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
 public class GrabBallOutOfRobot extends CommandGroup {
@@ -15,6 +16,10 @@ public class GrabBallOutOfRobot extends CommandGroup {
    */
   public GrabBallOutOfRobot() 
   {
+    if(Robot.wrist.armAngleDegrees <= Robot.wrist.jumpBarArmAngle && Robot.wrist.armAngleDegrees >= Robot.wrist.jumpBarArmAngle)
+    {
+      
+    }
     // Add Commands here:
     // e.g. addSequential(new Command1());
     // addSequential(new Command2());

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

@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* 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.Command;
+
+public class WristAvoidBar extends Command {
+  public WristAvoidBar() {
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
+  }
+
+  // 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() {
+  }
+
+  // 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/Wrist.java

@@ -78,7 +78,7 @@ public class Wrist extends Subsystem
   public static final double targetAngleDegreesHatchIn = 130;
   public static final double targetAngleDegreesHatchOut = 0;
 
-  public static final double jumpBarAngle = -50;   //hard limit switch?
+  public final double jumpBarArmAngle = -50;
   public static final double armAngleForPIDSwitch = -45;   ///Change values
 
   public static final boolean ballIntakeOut = true;
@@ -105,14 +105,6 @@ public class Wrist extends Subsystem
     }
   }
 
-  //Flipping the intake to the other side
-  public void flipIntake()
-  {
-    double currentWristAngle = wristRight.getPositionWorld();
-
-
-  }
-
   //Method for setting the control mode for the wrist
   private synchronized void setWristControlMode(WristControlMode controlMode) 
   {