From 11220abce382cd211321dd3768f43516b67796f9 Mon Sep 17 00:00:00 2001
From: mayabartels <mayabartels01@gmail.com>
Date: Fri, 1 Feb 2019 15:31:22 -0800
Subject: [PATCH] Controling flip for wrist

added control mode, changed encoder ticks to degrees, added method for controlling the flip. The angle may need to be adjusted depending on the mechanical design
---
 .../frc4388/robot/subsystems/Wrist.java       | 32 +++++++++++++++----
 1 file changed, 26 insertions(+), 6 deletions(-)

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 838ed9f..b2ba40f 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
@@ -32,10 +32,11 @@ import com.ctre.phoenix.motorcontrol.ControlMode;
 /**
  * Add your docs here.
  */
+
 public class Wrist extends Subsystem
 {
   //Control Mode Array
-  public static enum WristControlMode {PID, JOYSTICK_MANUAL, GRAB_BALL};
+  public static enum WristControlMode {PID, JOYSTICK_MANUAL, FLIP, GRAB_BALL};
 
   //Motor Controllers
   private ArrayList<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();
@@ -43,8 +44,7 @@ public class Wrist extends Subsystem
 	private CANTalonEncoder wristRight;
 
   //Encoder ticks to inches for encoders
-  public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerDegree;
-  public static final double ENCODER_TICKS_TO_DEGREES = Constants.kWristEncoderTicksPerDegree * Math.PI;
+  public static final double WRIST_ENCODER_TICKS_TO_DEGREES = ((4096/360)*(1/3));
   
   // PID controller and params
 	private MPTalonPIDController mpController;
@@ -86,7 +86,7 @@ public class Wrist extends Subsystem
     try
     {
       //PID wrist encoder and talon
-			wristRight = new CANTalonEncoder(RobotMap.WRIST_LEFT_MOTOR_CAN_ID, ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder);
+			wristRight = new CANTalonEncoder(RobotMap.WRIST_LEFT_MOTOR_CAN_ID, WRIST_ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder);
     }
     catch(Exception e)
     {
@@ -94,6 +94,14 @@ 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) 
   {
@@ -173,7 +181,10 @@ public class Wrist extends Subsystem
 					break;
 				case JOYSTICK_MANUAL:
 					controlManualWithJoystick();
-					break;
+          break;
+        case FLIP:
+          controlPIDFlipIntake();
+          break;
 				default:
 					break;
 			}
@@ -196,7 +207,16 @@ public class Wrist extends Subsystem
     
     joystickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
 		setSpeedJoystick(joystickSpeed);
-	}
+  }
+  
+  private void controlPIDFlipIntake()
+  {
+    double currentWristAngle = wristRight.getPositionWorld();
+    double targetFlipAngle = currentWristAngle - 180; 
+    //Flip angle may need to be adjusted if angle shouldn't be 180
+
+    updatePositionPID(targetFlipAngle);
+  }
 
   public synchronized boolean isFinished() 
   {