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

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

@@ -12,7 +12,7 @@ import org.usfirst.frc4388.robot.subsystems.Pnumatics;
 import org.usfirst.frc4388.robot.subsystems.Wrist;
 //import org.usfirst.frc4388.utility.ControlLooper;
 import org.usfirst.frc4388.utility.Looper;
-import org.usfirst.frc4388.utility.control.RobotStateEstimator;
+//import org.usfirst.frc4388.utility.control.RobotStateEstimator;
 import org.usfirst.frc4388.utility.math.RigidTransform2d;
 import org.usfirst.frc4388.utility.control.RobotState;
 
@@ -32,7 +32,7 @@ public class Robot extends TimedRobot
 
 	public static OI oi;
 
-	public static final Drive drive = Drive.getInstance();
+	//public static final Drive drive = Drive.getInstance();
 	public static final Arm arm = Arm.getInstance();
 	
 
@@ -71,9 +71,9 @@ public class Robot extends TimedRobot
    		setPeriod(Constants.kLooperDt * 2);
 		System.out.println("Main loop period = " + getPeriod());
 		oi = OI.getInstance();
-		controlLoop.register(drive);
+		//controlLoop.register(drive);
 		controlLoop.register(arm);
-		controlLoop.register(RobotStateEstimator.getInstance());
+		//controlLoop.register(RobotStateEstimator.getInstance());
 	
     	operationModeChooser = new SendableChooser<OperationMode>();
 	    operationModeChooser.addDefault("Competition", OperationMode.COMPETITION);
@@ -93,7 +93,7 @@ public class Robot extends TimedRobot
  
 	@Override
 	public void disabledInit() {
-		drive.setLimeLED(false);
+		//drive.setLimeLED(false);
     }
 	@Override
 	public void disabledPeriodic() {
@@ -103,7 +103,7 @@ public class Robot extends TimedRobot
 	@Override
     public void autonomousInit() {  
 		controlLoop.start();
-    	drive.setIsRed(getAlliance().equals(Alliance.Red));
+    	//drive.setIsRed(getAlliance().equals(Alliance.Red));
     	arm.resetZeroPosition(Arm.ZERO_POSITION_INCHES);
     	updateChoosers();
     	drive.endGyroCalibration();
@@ -171,7 +171,7 @@ public class Robot extends TimedRobot
 	
 	public void updateStatus() 
 	{
-    	drive.updateStatus(operationMode);
+    	//drive.updateStatus(operationMode);
 		arm.updateStatus(operationMode);
 		robotState.updateStatus(operationMode);
 

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

@@ -2,7 +2,8 @@
 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;
@@ -11,16 +12,14 @@ public class RobotMap {
 	public static final int copilot = 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;
 	
 	//carriage motors
-	public static final int WRIST_LEFT_MOTOR_CAN_ID = 6;
+	public static final int WRIST_RIGHT_MOTOR_CAN_ID = 6;
 	public static final int INTAKE_MOTOR = 7;
 	  
 	//Arm Motors
@@ -31,8 +30,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;
@@ -42,7 +39,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/commands/FlipIntake.java

@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.usfirst.frc4388.robot.*;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class FlipIntake extends Command 
+{
+  public FlipIntake() 
+  {
+    requires(Robot.wrist);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() 
+  {
+    Robot.wrist.controlPIDFlipIntake();
+  }
+
+  // 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/commands/GrabBallOutOfRobot.java

@@ -7,29 +7,35 @@
 
 package org.usfirst.frc4388.robot.commands;
 
-import edu.wpi.first.wpilibj.command.CommandGroup;
+import org.usfirst.frc4388.robot.Robot;
 
-public class GrabBallOutOfRobot extends CommandGroup {
+import edu.wpi.first.wpilibj.command.CommandGroup;
-  /**
+import edu.wpi.first.wpilibj.command.WaitCommand;
-   * Add your docs here.
+
-   */
+/**
+* Add your docs here.
+*/
+
+public class GrabBallOutOfRobot extends CommandGroup 
+{
   public GrabBallOutOfRobot() 
   {
-    // Add Commands here:
+    //Add rest of sequentials for this command group
-    // e.g. addSequential(new Command1());
-    // addSequential(new Command2());
-    // these will run in order.
 
-    // To run multiple commands at the same time,
+    addSequential(new FlipIntake());
-    // use addParallel()
+    //Move Arm until bar jump angle 
-    // e.g. addParallel(new Command1());
-    // addSequential(new Command2());
-    // Command1 and Command2 will run in parallel.
 
-    // A command group will require all of the subsystems that each member
+    //Jump bar in robot
-    // would require.
+    if(Robot.wrist.armAngleDegrees <= Robot.wrist.jumpBarArmAngle && Robot.wrist.armAngleDegrees >= Robot.wrist.jumpBarArmAngle)
-    // e.g. if Command1 requires chassis, and Command2 requires arm,
+    {
-    // a CommandGroup containing them would require both the chassis and the
+      addParallel(new WaitCommand(2));
-    // arm.
+      addSequential(new RunWristMotorJumpBar());
+      addSequential(new StopWristMotor());
+    }
+
+    //continue arm movement until ball is in intake ///SENSOR for ball in robot
+
+    //Move ball out of robot to 360 degrees
+      //move arm and wrist in parallel
   }
 }

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

@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* 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.CommandGroup;
+
+/**
+* Add your docs here.
+*/
+
+public class GrabHatchOffWall extends CommandGroup 
+{
+  public GrabHatchOffWall() {
+    // Add Commands here:
+    // e.g. addSequential(new Command1());
+    // addSequential(new Command2());
+    // these will run in order.
+
+    // To run multiple commands at the same time,
+    // use addParallel()
+    // e.g. addParallel(new Command1());
+    // addSequential(new Command2());
+    // Command1 and Command2 will run in parallel.
+
+    // A command group will require all of the subsystems that each member
+    // would require.
+    // e.g. if Command1 requires chassis, and Command2 requires arm,
+    // a CommandGroup containing them would require both the chassis and the
+    // arm.
+  }
+}

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

@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* 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.CommandGroup;
+
+/**
+* Add your docs here.
+*/
+
+public class GrabHatchOutOfFloorIntake extends CommandGroup 
+{
+  public GrabHatchOutOfFloorIntake() 
+  {
+    // Add Commands here:
+    // e.g. addSequential(new Command1());
+    // addSequential(new Command2());
+    // these will run in order.
+
+    // To run multiple commands at the same time,
+    // use addParallel()
+    // e.g. addParallel(new Command1());
+    // addSequential(new Command2());
+    // Command1 and Command2 will run in parallel.
+
+    // A command group will require all of the subsystems that each member
+    // would require.
+    // e.g. if Command1 requires chassis, and Command2 requires arm,
+    // a CommandGroup containing them would require both the chassis and the
+    // arm.
+  }
+}

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

@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.usfirst.frc4388.robot.*;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class RunWristMotorJumpBar extends Command 
+{
+  public RunWristMotorJumpBar() 
+  {
+    requires(Robot.wrist);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() 
+  {
+    Robot.wrist.jumpBar();
+  }
+
+  // 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/commands/StopWristMotor.java

@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* 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 org.usfirst.frc4388.robot.*;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class StopWristMotor extends Command 
+{
+  public StopWristMotor() 
+  {
+    requires(Robot.wrist);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() 
+  {
+    Robot.wrist.stopMotor();
+  }
+
+  // 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/Arm.java

@@ -1,6 +1,7 @@
 package org.usfirst.frc4388.robot.subsystems;
 import java.util.ArrayList;
 
+import org.usfirst.frc4388.controller.XboxController;
 import org.usfirst.frc4388.robot.Constants;
 import org.usfirst.frc4388.robot.Robot;
 import org.usfirst.frc4388.robot.RobotMap;
@@ -11,6 +12,15 @@ import org.usfirst.frc4388.utility.TalonSRXEncoder;
 import org.usfirst.frc4388.utility.TalonSRXFactory;
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 
@@ -20,7 +30,9 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class Arm extends Subsystem implements Loop
 {
-	private static Arm instance;
+	//PID encoder and motor
+	private CANTalonEncoder armMotor;
+	//private WPI_TalonSRX ArmLeft;
 
 	public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL };
 
@@ -58,10 +70,11 @@ public class Arm extends Subsystem implements Loop
 	public static final double MP_T1 = 400;  // Fast = 300
 	public static final double MP_T2 = 150;  // Fast = 150
 	
-	// Motor controllers
+	//PID controller Max Scale
-	private ArrayList<TalonSRXEncoder> motorControllers = new ArrayList<TalonSRXEncoder>();	
+	private SoftwarePIDPositionController pidPositionControllerLowest;
+	//private PIDParams PositionPIDParamsLowest = new PIDParams(2.0, 0.0, 0.0);
+	private PIDParams PositionPLowest;
 
-	private TalonSRXEncoder motor1;
 	private TalonSRX motor2;
 	
 	// PID controller and params
@@ -105,67 +118,51 @@ public class Arm extends Subsystem implements Loop
 			
 			
 		}
-		catch (Exception e) {
-			System.err.println("An error occurred in the DriveTrain constructor");
-		}
-	}
-
-	@Override
-	public void initDefaultCommand() {
-	}
-		
-	public void resetZeroPosition(double position) {
-		mpController.resetZeroPosition(position);
-	}	
-	
-	private synchronized void setElevatorControlMode(ArmControlMode controlMode) {
-		this.elevatorControlMode = controlMode;
-	}
-	
-	private synchronized ArmControlMode getElevatorControlMode() {
-		return this.elevatorControlMode;
-	}
-
-	public void setSpeed(double speed) {
-		motor1.set(ControlMode.PercentOutput, speed);
-		setElevatorControlMode(ArmControlMode.MANUAL);
-	}
-		
-	public void setSpeedJoystick(double speed) {
-		motor1.set(ControlMode.PercentOutput, speed);
-		setElevatorControlMode(ArmControlMode.JOYSTICK_MANUAL);
-	}
-		
-	public void setPositionPID(double targetPositionInches) {
-		mpController.setPIDSlot(PID_SLOT);
-		updatePositionPID(targetPositionInches);
-		setElevatorControlMode(ArmControlMode.JOYSTICK_PID);	
-		setFinished(false);
-	}
-	
-	public void updatePositionPID(double targetPositionInches) {
- 		targetPositionInchesPID = limitPosition(targetPositionInches);
-		double startPositionInches = motor1.getPositionWorld();
-		mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); 
-	}
-	
-	public void setPositionMP(double targetPositionInches) {
-		double startPositionInches = motor1.getPositionWorld();
-		mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2); 
-		setFinished(false);
 		firstMpPoint = true;
 		setElevatorControlMode(ArmControlMode.MOTION_PROFILE);
  	}
+
+    
+	//PID encoder position
+	public double getEncoderArmPosition()
+	{
+		return armMotor.getPositionWorld();
+	}
 	
-	private double limitPosition(double targetPosition) {
+	public double getArmHeightInchesAboveFloor()
-		if (targetPosition < MIN_POSITION_INCHES) {
+	{
-			return MIN_POSITION_INCHES;
+		return armMotor.getPositionWorld();
-		}
+	}
-		else if (targetPosition > MAX_POSITION_INCHES) {
+
-			return MAX_POSITION_INCHES;
+	public synchronized void setControlMode(DriveControlMode controlMode) 
-		}
+	{
-		
+ 		this.controlMode = controlMode;
-		return targetPosition;
+ 		
+ 		isFinished = false;
+	}
+	/*
+	public void setArmPIDMaxScale(double ArmPosition, double maxError, double minError)
+	{
+		double ArmTargetPos = 0;
+		this.targetPPosition = ArmTargetPos;
+		pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError);      ///////TARGET POSITION WHERE??
+		Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE);
+	}
+	
+	public void setArmPIDLowScale(double ArmPosition, double maxError, double minError)
+	{
+		double ArmTargetPos = 0;
+		this.targetPPosition = ArmTargetPos;
+		pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError);
+		Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE);
+	}
+	
+	public void setArmPIDSwitch(double ArmPosition, double maxError, double minError)
+	{
+		double ArmTargetPos = 0;
+		this.targetPPosition = ArmTargetPos;
+		pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError);
+		Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH);
 	}
 	
 	@Override
@@ -177,11 +174,9 @@ public class Arm extends Subsystem implements Loop
 		mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
 		mpController.setPIDSlot(PID_SLOT);
 	}
-
+	*/
-	@Override
+	public void rawSetOutput(double output){
-	public void onStop(double timestamp) {
+		armMotor.set(/*ControlMode.PercentOutput,*/ output);
-		// TODO Auto-generated method stub
-		
 	}
 
 	@Override
@@ -209,8 +204,24 @@ public class Arm extends Subsystem implements Loop
 				default:
 					break;
 			}
+			pressed = true;
 		}
+		else
+		{
+			if 	(controlMode == DriveControlMode.STOP_MOTORS){
+				{
+				Robot.arm.setControlMode(DriveControlMode.JOYSTICK);
+				}
+			
+			pressed = false;
+			}
+		}
+
 	}
+		
+		//pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false;
+	
+	
 	
 	private void controlPidWithJoystick() {
 		double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis();
@@ -258,11 +269,8 @@ public class Arm extends Subsystem implements Loop
 		return isFinished;
 	}
 	
-	public synchronized void setFinished(boolean isFinished) {
+	public double getPeriodMs()
-		this.isFinished = isFinished;
+	{
-	}
-		
-	public double getPeriodMs() {
 		return periodMs;
 	}
 	
@@ -278,15 +286,10 @@ public class Arm extends Subsystem implements Loop
 //				SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID));
 				SmartDashboard.putNumber("Elevator Target PID Position", targetPositionInchesPID);
 			}
-			catch (Exception e) {
+			catch (Exception e) 
+			{
+				System.err.println("Drivetrain update status error");
 			}
 		}
-	}	
-	
-	public static Arm getInstance() {
-		if(instance == null) {
-			instance = new Arm();
-		}
-		return instance;
 	}
 }

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

@@ -930,3 +930,114 @@ public class Drive extends Subsystem implements Loop {
 	}
 
 }
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+
+
+import frc.robot.commands.DriveCommand;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.ControlType;
+import com.revrobotics.CANPIDController;
+import com.revrobotics.CANEncoder;
+
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+
+/**
+ * Add your docs here.
+ */
+public class Drive extends Subsystem {
+  
+  static double LOW_GEAR_RATIO = 4.821;//6.57992
+  static double HIGH_GEAR_RATIO = ;
+  static double CIRCUMFERENCE = 15.221;
+  double HIGH_INCHES_PER_REV = CIRCUMFERENCE/HIGH_GEAR_RATIO;
+  double LOW_INCHES_PER_REV = CIRCUMFERENCE/LOW_GEAR_RATIO;
+  int timeoutMs = 10;
+
+  // Front is the follow motor, and it is based on following the primary motor of its side.
+  public CANSparkMax leftDrivePrimary = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, MotorType.kBrushless),
+                     leftDriveFront = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID, MotorType.kBrushless),
+                     rightDriveFront = new CANSparkMax(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID, MotorType.kBrushless),
+                     rightDrivePrimary = new CANSparkMax(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID, MotorType.kBrushless);
+
+  
+  public CANPIDController leftPID = new CANPIDController(leftDrivePrimary);
+  public CANPIDController rightPID = new CANPIDController(rightDrivePrimary);
+
+  public CANEncoder leftEncoder = new CANEncoder(leftDrivePrimary);
+  public CANEncoder rightEncoder = new CANEncoder(rightDrivePrimary);
+
+
+  @Override
+  public void initDefaultCommand() {
+    setDefaultCommand(new DriveCommand());
+
+    leftDriveFront.follow(leftDrivePrimary);
+    rightDriveFront.follow(rightDrivePrimary);
+  
+    leftDrivePrimary.setCANTimeout(timeoutMs);
+    rightDrivePrimary.setCANTimeout(timeoutMs);
+    leftDrivePrimary.setMotorType(MotorType.kBrushless);
+    rightDrivePrimary.setMotorType(MotorType.kBrushless);
+
+  }
+
+  public void set(double left, double right) {
+    leftDrivePrimary.set(left);
+    rightDrivePrimary.set(right);
+  }
+
+  public double getLeftRevs() {
+    return leftEncoder.getPosition();
+  }
+
+  public double getRightRevs() {
+    return rightEncoder.getPosition();
+  }
+
+  public double lowinchesToRevs(double INCHES) {
+    return INCHES/LOW_INCHES_PER_REV;
+  }
+  public double highinchesToRevs(double INCHES) {
+    return INCHES/HIGH_INCHES_PER_REV;
+  }
+
+  // Takes an input of ticks
+  public void setPID(double left, double right) {
+    leftPID.setReference(left, ControlType.kPosition);
+    rightPID.setReference(right, ControlType.kPosition);
+  }
+
+  public void stop() {
+    leftDrivePrimary.set(0);
+    rightDrivePrimary.set(0);
+  }
+}

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

@@ -26,6 +26,11 @@ import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.SensorCollection;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
+import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource;
+import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
 
@@ -36,7 +41,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode;
 public class Wrist extends Subsystem
 {
   //Control Mode Array
-  public static enum WristControlMode {PID, JOYSTICK_MANUAL, FLIP_INTAKE, GRAB_BALL};
+  public static enum WristControlMode {PID, JOYSTICK_MANUAL, GRAB_BALL};
 
   //Motor Controllers
   private ArrayList<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();
@@ -72,13 +77,19 @@ public class Wrist extends Subsystem
 
   public double armAngleDegrees = 90;
 
-  public static final double targetAngleDegreesBallIn = -45;   ///Change values
+  public static final double targetAngleDegreesBallIn = 180; //only have to flip intake and go down to get ball
+  ///Change values
   public static final double targetAngleDegreesBallOut = 360;
-  public static final double targetAngleDegreesHatchIn = 55;
+  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;
+
+  //control mode for joystick control
+	private DriveControlMode controlMode = DriveControlMode.JOYSTICK;
   
   //Misc
   private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL;
@@ -89,12 +100,18 @@ public class Wrist extends Subsystem
   private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
   private double joystickDegreesPerMs = JOYSTICK_Degrees_PER_MS_LO;
   
+  LimitSwitchSource limitSwitchSource;
+
   public Wrist()
   {
     try
     {
       //PID wrist encoder and talon
-			wristRight = new CANTalonEncoder(RobotMap.WRIST_LEFT_MOTOR_CAN_ID, WRIST_ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder);
+      wristRight = new CANTalonEncoder(RobotMap.WRIST_RIGHT_MOTOR_CAN_ID, WRIST_ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder);
+      
+      //Limit Switch
+      wristRight.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
+      wristRight.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
     }
     catch(Exception e)
     {
@@ -102,12 +119,17 @@ public class Wrist extends Subsystem
     }
   }
 
-  //Flipping the intake to the other side
+  //Jump bar by putting power to the motors for a specific amount of time
-  public void flipIntake()
+  //Jump bar output
+  public void jumpBar()
   {
-    double currentWristAngle = wristRight.getPositionWorld();
+    wristRight.set(0.8);
-
+  }
 
+  //Stop wrist motor
+  public void stopMotor()
+  {
+    wristRight.set(0);
   }
 
   //Method for setting the control mode for the wrist
@@ -191,8 +213,29 @@ public class Wrist extends Subsystem
           } 
           else if(armAngleDegrees <= armAngleForPIDSwitch)
           {
+            if(ballIntakeOut)
+            {
+              if(armAngleDegrees > targetAngleDegreesBallIn)
+              {
+                controlPIDBallIn();
+              }
+              else
+              {
+                controlPIDBallOut();
+              }
+            }
+            else
+            {
+              if(armAngleDegrees > targetAngleDegreesHatchIn)
+              {
+                controlPIDBallIn();
+              }
+              else
+              {
+                controlPIDHatchOut();
+              }
+            }
           }
-					controlPID();
 					break;
 				case JOYSTICK_MANUAL:
 					controlManualWithJoystick();
@@ -222,7 +265,7 @@ public class Wrist extends Subsystem
   {
     double joystickSpeed;
     
-    joystickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
+    joystickSpeed = -Robot.oi.getOperatorController().getRightYAxis();
 		setSpeedJoystick(joystickSpeed);
   }
   
@@ -240,6 +283,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() 

2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java

@@ -96,7 +96,7 @@ public class MMTalonPIDController
 		// Update the set points 
 		if (controlMode == MMControlMode.STRAIGHT) {
 			double gyroDelta = useGyroLock ? startGyroAngle - currentGyroAngle: 0;
-			double deltaDistance = calcTrackDistance(gyroDelta, MMTalonTurnType.TANK, Drive.TRACK_WIDTH_INCHES);
+			double deltaDistance = calcTrackDistance(gyroDelta, MMTalonTurnType.TANK, 1);
 			rightTarget = targetValue + deltaDistance;
 			leftTarget = targetValue - deltaDistance;
 			

2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java

@@ -35,7 +35,7 @@ public class PathGenerator {
         Trajectory trajectory = Pathfinder.generate(points, config);
         centerPoints = trajectory.segments;       
 
-	    TankModifier modifier = new TankModifier(trajectory).modify(Drive.TRACK_WIDTH_INCHES);
+	    TankModifier modifier = new TankModifier(trajectory).modify(1);
 	    leftPoints = modifier.getLeftTrajectory().segments;
 	    rightPoints = modifier.getRightTrajectory().segments;