Merge branch 'Wrist' of https://github.com/Team4388/2019-Hit-or-Miss into Wrist

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

@@ -62,6 +62,7 @@ public class OI
 		   JoystickButton wristManualMode = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.A_BUTTON);
 		   wristManualMode.whenPressed(new WristSetMode(WristControlMode.JOYSTICK_MANUAL));
 			
+		   //Arm
 		   JoystickButton ArmAimAssist = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_JOYSTICK_BUTTON);
 		   ArmAimAssist.whenPressed(new ArmSetMode(ArmControlMode.PID));
 

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

@@ -15,6 +15,8 @@ import org.usfirst.frc4388.utility.CANTalonEncoder;
 import org.usfirst.frc4388.utility.ControlLoopable;
 import org.usfirst.frc4388.utility.PIDParams;
 import org.usfirst.frc4388.utility.SoftwarePIDPositionController;
+import org.usfirst.frc4388.utility.MPTalonPIDPathController;
+import org.usfirst.frc4388.utility.MPTalonPIDController;
 import org.usfirst.frc4388.robot.subsystems.Arm;
 
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
@@ -30,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_INTAKE, GRAB_BALL};
 
   //Motor Controllers
   private ArrayList<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();
@@ -41,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;
@@ -50,12 +52,12 @@ public class Wrist extends Subsystem
 	public static int PID_SLOT = 0;
 	public static int MP_SLOT = 1;
 
-	private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0);  
-	private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0);  
-	private PIDParams pidPIDParamsLoGear = new PIDParams(0.45, 0.0, 0.0, 0.0, 0.0, 0.0);  
+	  ///private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0);  
+	private PIDParams pidPIDParamsLevel = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0);
 	public static final double KF_UP = 0.005;
 	public static final double KF_DOWN = 0.0;
   public static final double PID_ERROR_INCHES = 1.0;
+  private long periodMs = (long)(Constants.kLooperDt * 1000.0);
 
   // Defined positions
 	public static final double MIN_POSITION_INCHES = 0.0;
@@ -69,6 +71,14 @@ public class Wrist extends Subsystem
   public static final double JOYSTICK_Degrees_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8;
 
   public double armAngleDegrees = Robot.arm.ARM_ANGLE_DEGREES;
+
+  public static final double targetAngleDegreesBallIn = -45;   ///Change values
+  public static final double targetAngleDegreesBallOut = 360;
+  public static final double targetAngleDegreesHatchIn = 55;
+  public static final double targetAngleDegreesHatchOut = 0;
+
+  public static final double jumpBarAngle = 50;   //hard limit switch?
+  public static final double armAngleForPIDSwitch = -45;   ///Change values
   
   //Misc
   private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL;
@@ -84,7 +94,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)
     {
@@ -92,6 +102,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) 
   {
@@ -154,17 +172,36 @@ public class Wrist extends Subsystem
 		setWristControlMode(wristControlMode.JOYSTICK_MANUAL);
   }
 
+  public void onStart(double timestamp) 
+  {
+		//mpController.setPID(mpPIDParams);
+		mpController.setPID(pidPIDParamsLevel);
+		mpController.setPIDSlot(PID_SLOT);
+	}
+
   //@Override
   public void onLoop(double timestamp) 
   {
 		synchronized (Wrist.this) {
 			switch(getWristControlMode() ) {
-				case PID: 
+        case PID:
+          if(armAngleDegrees > armAngleForPIDSwitch)
+          {
+            controlPID();
+          } 
+          else if(armAngleDegrees <= armAngleForPIDSwitch)
+          {
+          }
 					controlPID();
 					break;
 				case JOYSTICK_MANUAL:
 					controlManualWithJoystick();
-					break;
+          break;
+        /*
+        case FLIP_INTAKE:
+          controlPIDFlipIntake();
+          break;
+        */
 				default:
 					break;
 			}
@@ -180,14 +217,30 @@ public class Wrist extends Subsystem
     updatePositionPID(targetAngleDegreesPID);
 	}
   
-  //Method for controlling the motor with the joystick
+  //Controlling the motor with the joystick
   private void controlManualWithJoystick() 
   {
     double joystickSpeed;
     
     joystickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
 		setSpeedJoystick(joystickSpeed);
-	}
+  }
+  
+  //Flip the intake from hatch to ball and visa versa
+  public 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);
+  }
+
+  //Controlling the PID for the intake going into the robot with ball side facing in
+  public void controlPIDBallIn()
+  {
+    updatePositionPID(targetAngleDegreesBallIn);
+  }
 
   public synchronized boolean isFinished() 
   {
@@ -199,6 +252,11 @@ public class Wrist extends Subsystem
 		this.isFinished = isFinished;
 	}
 
+  public double getPeriodMs() 
+  {
+		return periodMs;
+	}
+
   @Override
   public void initDefaultCommand() 
   {