From 9cc06f7825ab3020b475bd446181aa5932386b1f Mon Sep 17 00:00:00 2001
From: mayabartels <mayabartels01@gmail.com>
Date: Fri, 1 Feb 2019 20:44:07 -0800
Subject: [PATCH] Wrist PID update

---
 .../robot/commands/GrabBallOutOfRobot.java    |  5 +++
 .../frc4388/robot/commands/WristAvoidBar.java | 44 +++++++++++++++++++
 .../frc4388/robot/subsystems/Wrist.java       | 10 +----
 3 files changed, 50 insertions(+), 9 deletions(-)
 create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristAvoidBar.java

diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java
index 2ab8874..5392859 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java
+++ b/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());
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristAvoidBar.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristAvoidBar.java
new file mode 100644
index 0000000..7e919ce
--- /dev/null
+++ b/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() {
+  }
+}
diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java
index e908a1e..6fc8e65 100644
--- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java
+++ b/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) 
   {