Merge using meld to put PID wrist into arm-develop

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

@@ -20,6 +20,7 @@ import org.usfirst.frc4388.robot.OI;
 import org.usfirst.frc4388.robot.subsystems.*;
 import org.usfirst.frc4388.utility.ControlLooper;
 import org.usfirst.frc4388.robot.subsystems.Drive;
+import org.usfirst.frc4388.robot.subsystems.Wrist;
 
 import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;;
 
@@ -30,6 +31,7 @@ public class Robot extends IterativeRobot
 	
 	public static final Drive drive = new Drive();
 	public static final Arm arm = new Arm();
+	public static final Wrist wrist = new Wrist();
 
 
  

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

@@ -17,8 +17,9 @@ public class RobotMap {
 	public static final int DRIVETRAIN_RIGHT_MOTOR1_CAN_ID = 4;
 	public static final int DRIVETRAIN_RIGHT_MOTOR2_CAN_ID = 5;
 	
-	//Intake motors
-	public static final int INTAKE_BELT_DRIVE_CAN_ID = 8;
+	//carriage motors
+	public static final int WRIST_RIGHT_MOTOR_CAN_ID = 6;
+	public static final int INTAKE_BELT_DRIVE_CAN_ID = 7;
 	  
 	//Elevator Motors
 	public static final int ARM_MOTOR1_ID= 6;

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

@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* 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.Robot;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+/**
+* Add your docs here.
+*/
+
+public class GrabBallOutOfRobot extends CommandGroup 
+{
+  public GrabBallOutOfRobot() 
+  {
+    //Add rest of sequentials for this command group
+
+    addSequential(new FlipIntake());
+    //Move Arm until bar jump angle 
+
+    //Jump bar in robot
+    if(Robot.wrist.armAngleDegrees <= Robot.wrist.jumpBarArmAngle && Robot.wrist.armAngleDegrees >= Robot.wrist.jumpBarArmAngle)
+    {
+      addParallel(new WaitCommand(2));
+      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/commands/WristSetMode.java

@@ -0,0 +1,47 @@
+package org.usfirst.frc4388.robot.commands;
+
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.robot.subsystems.Wrist.WristControlMode;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * Description 
+ */
+public class WristSetMode extends Command {
+
+	private WristControlMode controlMode;
+	
+    public WristSetMode(WristControlMode controlMode) {
+    	this.controlMode = controlMode;
+        requires(Robot.wrist);
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    	if (controlMode == WristControlMode.PID) {
+    		Robot.wrist.setPositionPID(Robot.wrist.getPositionDegrees());
+    	}
+    	else if (controlMode == WristControlMode.JOYSTICK_MANUAL) {
+    		Robot.wrist.setSpeedJoystick(0);
+    	}
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    protected void execute() {
+    }
+
+    // Make this return true when this Command no longer needs to run execute()
+    protected boolean isFinished() {
+        return true;
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    }
+}

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

@@ -1,15 +1,23 @@
 package org.usfirst.frc4388.robot.subsystems;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+
 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;
-import org.usfirst.frc4388.utility.ControlLoopable;
-import org.usfirst.frc4388.utility.Loop;
 import org.usfirst.frc4388.utility.MPTalonPIDController;
+import org.usfirst.frc4388.robot.commands.*;
+import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
+import org.usfirst.frc4388.utility.CANTalonEncoder;
+import org.usfirst.frc4388.utility.ControlLoopable;
 import org.usfirst.frc4388.utility.PIDParams;
-import org.usfirst.frc4388.utility.TalonSRXEncoder;
-import org.usfirst.frc4388.utility.TalonSRXFactory;
+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.ControlMode;
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
@@ -20,329 +28,312 @@ import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource;
 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 edu.wpi.first.wpilibj.Solenoid;
-import edu.wpi.first.wpilibj.command.Subsystem;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.Timer;
+import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
+import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource;
+import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
 
-public class Wrist extends Subsystem implements ControlLoopable
+import com.ctre.phoenix.motorcontrol.ControlMode;
+
+public class Wrist extends Subsystem
 {
 	private static Wrist instance;
 
-	public static enum WristControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL };
+  //Control Mode Array
+  public static enum WristControlMode {PID, JOYSTICK_MANUAL, GRAB_BALL};
 
-	// One revolution of the 30T Drive 1.880" PD pulley = Pi * PD inches = 36/24 revs due to pulleys * 34/24 revs due to gears * 36/12 revs due mag encoder gear on ball shifter * 4096 ticks 
-	public static final double ENCODER_TICKS_TO_INCHES = ((360/4096)/(3))-60;  
-	
+  //Motor Controllers
+  private ArrayList<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();
 
-	private double periodMs;
-	private double lastControlLoopUpdatePeriod = 0.0;
-	private double lastControlLoopUpdateTimestamp = 0.0;
-	// Defined speeds
-	public static final double CLIMB_SPEED = -1.0;
-	public static final double TEST_SPEED_UP = 0.3;
-	public static final double TEST_SPEED_DOWN = -0.3;
-	public static final double AUTO_ZERO_SPEED = -0.3;
-	public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75;
-	public static final double JOYSTICK_INCHES_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8;
-	
-	// Defined positions
-	public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0;
-	public static final double ZERO_POSITION_INCHES = -0.25;
-	public static final double NEAR_ZERO_POSITION_INCHES = 0.0;
-	public static final double MIN_POSITION_INCHES = 0.0;
-	public static final double MAX_POSITION_INCHES = 83.4;
-	public static final double AFTER_INTAKE_POSITION_INCHES = 4.0;
+	private CANTalonEncoder wristRight;
 
-	public static final double SWITCH_POSITION_INCHES = 24.0;
-	public static final double SWITCH_POSITION_HIGH_INCHES = 36.0; //Switch Position for First Cube APR
-	public static final double SCALE_LOW_POSITION_INCHES = 56.0;
-	public static final double SCALE_FIRST_CUBE_POSITION_INCHES = 78.0; //72.0
-	public static final double SCALE_SECOND_CUBE_POSITION_INCHES = 77.0;
-	public static final double SCALE_HIGH_POSITION_INCHES = MAX_POSITION_INCHES;
-	public static final double CLIMB_BAR_POSITION_INCHES = 70.0;
-	public static final double CLIMB_HIGH_POSITION_INCHES = 10.0;
-	public static final double CLIMB_ASSIST_POSITION_INCHES = 50.0;
-
-	// Motion profile max velocities and accel times
-	public static final double MP_MAX_VELOCITY_INCHES_PER_SEC =  60; 
-	public static final double MP_T1 = 400;  // Fast = 300
-	public static final double MP_T2 = 150;  // Fast = 150
-	
-	// Motor controllers
-	private ArrayList<TalonSRXEncoder> motorControllers = new ArrayList<TalonSRXEncoder>();	
-
-	private TalonSRXEncoder wristmotor1;
-	
-	// PID controller and params
+  //Encoder ticks to inches for encoders
+  public static final double WRIST_ENCODER_TICKS_TO_DEGREES = ((4096/360)*(1/3));
+  
+  // PID controller and params
 	private MPTalonPIDController mpController;
 
 	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 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;
-	LimitSwitchSource limitSwitchSource;
-	// Pneumatics
-	private Solenoid speedShift;
+  public static final double PID_ERROR_INCHES = 1.0;
+  private long periodMs = (long)(Constants.kLooperDt * 1000.0);
 
-	private WristControlMode controlMode = WristControlMode.JOYSTICK_MANUAL;
-	// Misc
-	public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0;	
-	private boolean isFinished;
-	private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL;
-	private double targetPositionInchesPID = 0;
-	private boolean firstMpPoint;
-	private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
-	
-	public Wrist() {
-		try {
-			wristmotor1 = TalonSRXFactory.createTalonEncoder(RobotMap.WRIST_MOTOR_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder);
-			
-			
-			wristmotor1.setInverted(true);
-										
-//	        if (motor1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) {
-//	            Driver.reportError("Could not detect elevator motor 1 encoder encoder!", false);
-//	        }
-			wristmotor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
-			wristmotor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
-			wristmotor1.setNeutralMode(NeutralMode.Brake);
-			
-			motorControllers.add(wristmotor1);
-			
-			
-		}
-		catch (Exception e) {
-			System.err.println("An error occurred in the Wrist constructor");
-		}
-	}
+  // Defined positions
+  public static final double MIN_POSITION_INCHES = 0.0;
+  public static final double MAX_POSITION_INCHES = 83.4;
 
-	@Override
-	public void initDefaultCommand() {
-	}
-		
-	public void resetZeroPosition(double position) {
-		mpController.resetZeroPosition(position);
-	}	
-	
-	private synchronized void setWristControlMode(WristControlMode controlMode) {
+  public static final double MIN_ANGLE_DEGREES = -3;   ////FIND ANGLE VALUES
+  public static final double MAX_ANGLE_DEGREES = 3;
+
+  public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75;
+  public static final double JOYSTICK_INCHES_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8;
+  public static final double JOYSTICK_Degrees_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8;
+
+  public double armAngleDegrees = 90;
+
+  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 = 130;
+  public static final double targetAngleDegreesHatchOut = 0;
+
+  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;
+  private boolean isFinished;
+  private double targetPositionInchesPID = 0;
+  private double targetAngleDegreesPID = -(180 - armAngleDegrees);
+
+  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_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)
+    {
+      System.err.println("You thought the code would work, but it was me, error. An error occurred in the Wrist Construtor");
+    }
+  }
+
+  //Jump bar by putting power to the motors for a specific amount of time
+  //Jump bar output
+  public void jumpBar()
+  {
+    wristRight.set(0.8);
+  }
+
+  //Stop wrist motor
+  public void stopMotor()
+  {
+    wristRight.set(0);
+  }
+
+  //Method for setting the control mode for the wrist
+  private synchronized void setWristControlMode(WristControlMode controlMode) 
+  {
 		this.wristControlMode = controlMode;
-	}
-	
-	private synchronized WristControlMode getWristControlMode() {
+  }
+  
+  //Getting the control mode for the wrist
+  private synchronized WristControlMode getWristControlMode() 
+  {
 		return this.wristControlMode;
-	}
+  }
 
-	public void setSpeed(double speed) {
-		wristmotor1.set(ControlMode.PercentOutput, speed*0.3);
-		setWristControlMode(WristControlMode.MANUAL);
-	}
-		
-	public void setSpeedJoystick(double speed) {
-		wristmotor1.set(ControlMode.PercentOutput, speed);
+  //Setting the speed for the motor on the wrist along with setting the control mode to manual
+  public void setSpeed(double speed) 
+  {
+		wristRight.set(ControlMode.PercentOutput, speed);
 		setWristControlMode(WristControlMode.JOYSTICK_MANUAL);
 	}
-		
-	public void setPositionPID(double targetPositionInches) {
-		mpController.setPIDSlot(PID_SLOT);
-		updatePositionPID(targetPositionInches);
-		setWristControlMode(WristControlMode.JOYSTICK_PID);	
+
+  //Setting the target position for the PID loop and set the control mode to PID
+  public void setPositionPID(double targetAngleInches) 
+  {
+    mpController.setPIDSlot(PID_SLOT);
+		updatePositionPID(targetAngleInches);
+		setWristControlMode(WristControlMode.PID);	
 		setFinished(false);
-	}
-	
-	public void updatePositionPID(double targetPositionInches) {
- 		targetPositionInchesPID = limitPosition(targetPositionInches);
-		double startPositionInches = wristmotor1.getPositionWorld();
-		mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); 
-	}
-	
-	public void setPositionMP(double targetPositionInches) {
-		double startPositionInches = wristmotor1.getPositionWorld();
-		mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2); 
-		setFinished(false);
-		firstMpPoint = true;
-		setWristControlMode(WristControlMode.MOTION_PROFILE);
- 	}
-	
-	private double limitPosition(double targetPosition) {
-		if (targetPosition < MIN_POSITION_INCHES) {
-			return MIN_POSITION_INCHES;
+  }
+  
+  //Setting range for target position
+  private double limitPosition(double targetAngle) 
+  {
+		if (targetAngle < MIN_ANGLE_DEGREES) {
+			return MIN_ANGLE_DEGREES;
 		}
-		else if (targetPosition > MAX_POSITION_INCHES) {
-			return MAX_POSITION_INCHES;
+		else if (targetAngle > MAX_ANGLE_DEGREES) {
+			return MAX_ANGLE_DEGREES;
 		}
 		
-		return targetPosition;
+		return targetAngle;
 	}
-	@Override
-	public void setPeriodMs(long periodMs) {
-		mpController = new MPTalonPIDController(periodMs, motorControllers);
-		mpController.setPID(mpPIDParams, MP_SLOT);
-		mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
-		mpController.setPIDSlot(PID_SLOT);
-		mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
-		mpController.setPIDSlot(PID_SLOT);
-		this.periodMs = periodMs;
-	}
-	/*@Override
-	public void onStart(double timestamp) {
-		mpController = new MPTalonPIDController(periodMs, motorControllers);
-		mpController.setPID(mpPIDParams, MP_SLOT);
-		mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
-		mpController.setPIDSlot(PID_SLOT);
-		mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
+  
+  //Method for updating the PID target position
+  public void updatePositionPID(double targetAngleDegrees) 
+  {
+ 		targetAngleDegreesPID = limitPosition(targetAngleDegrees);
+		double startAngleDegrees = wristRight.getPositionWorld();
+		mpController.setTarget(targetPositionInchesPID, targetAngleDegreesPID > startAngleDegrees ? KF_UP : KF_DOWN); 
+  }
+
+  //Getting the current encoder position
+  public double getPositionDegrees() 
+  {
+		return wristRight.getPositionWorld();
+  }
+  
+  //Setting the speed for the motors in manual mode
+  public void setSpeedJoystick(double speed) 
+  {
+		wristRight.set(ControlMode.PercentOutput, speed);
+		setWristControlMode(wristControlMode.JOYSTICK_MANUAL);
+  }
+
+  public void onStart(double timestamp) 
+  {
+		//mpController.setPID(mpPIDParams);
+	//	mpController.setPID(pidPIDParamsLevel);
 		mpController.setPIDSlot(PID_SLOT);
 	}
 
-	@Override
-	public void onStop(double timestamp) {
-		// TODO Auto-generated method stub
-		
-	}
-
-	@Override
-	public void onLoop(double timestamp) {
+  //@Override
+  public void onLoop(double timestamp) 
+  {
 		synchronized (Wrist.this) {
-			switch( getElevatorControlMode() ) {
-				case JOYSTICK_PID: 
-					controlPidWithJoystick();
+			switch(getWristControlMode() ) {
+        case PID:
+          if(armAngleDegrees > armAngleForPIDSwitch)
+          {
+            controlPID();
+          } 
+          else if(armAngleDegrees <= armAngleForPIDSwitch)
+          {
+            if(ballIntakeOut)
+            {
+              if(armAngleDegrees > targetAngleDegreesBallIn)
+              {
+                controlPIDBallIn();
+              }
+              else
+              {
+                controlPIDBallOut();
+              }
+            }
+            else
+            {
+              if(armAngleDegrees > targetAngleDegreesHatchIn)
+              {
+                controlPIDBallIn();
+              }
+              else
+              {
+                controlPIDHatchOut();
+              }
+            }
+          }
 					break;
 				case JOYSTICK_MANUAL:
 					controlManualWithJoystick();
-					break;
-				case MOTION_PROFILE: 
-					if (!isFinished()) {
-						if (firstMpPoint) {
-							mpController.setPIDSlot(MP_SLOT);
-							firstMpPoint = false;
-						}
-						setFinished(mpController.controlLoopUpdate()); 
-						if (isFinished()) {
-							mpController.setPIDSlot(PID_SLOT);
-						}
-					}
-					break;
+          break;
+        /*
+        case FLIP_INTAKE:
+          controlPIDFlipIntake();
+          break;
+        */
 				default:
 					break;
 			}
 		}
+  }
+  
+  private void controlPID() 
+  {
+		double joystickAngle = -Robot.oi.getOperatorController().getLeftYAxis();
+    double deltaAngle = joystickAngle * joystickDegreesPerMs;
+
+		targetAngleDegreesPID = targetAngleDegreesPID + deltaAngle;
+    updatePositionPID(targetAngleDegreesPID);
 	}
+  
+  //Controlling the motor with the joystick
+  private void controlManualWithJoystick() 
+  {
+    double joystickSpeed;
+    
+    joystickSpeed = -Robot.oi.getOperatorController().getRightYAxis();
+		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);
+    //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 void controlLoopUpdate() {
-		// Measure *actual* update period
-		double currentTimestamp = Timer.getFPGATimestamp();
-		if (lastControlLoopUpdateTimestamp > 0.001)	{	// ie, if this is NOT the first time
-			lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp;
-		}
-		lastControlLoopUpdateTimestamp = currentTimestamp;
-		
-		// Do the update
-		if (controlMode == WristControlMode.JOYSTICK_MANUAL) {
-			controlManualWithJoystick();
-		}
-		else if (!isFinished) {
-			if (controlMode == WristControlMode.MOTION_PROFILE) {
-				isFinished = mpController.controlLoopUpdate(getPositionInches()); 
-			}
-			
-			/*else if (controlMode == WristControlMode.MP_PATH_VELOCITY) {
-				isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); 
-			}
-			else if (controlMode == WristControlMode.ADAPTIVE_PURSUIT) {
-				updatePose();
-				isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); 
-			}*/
-		}
-	}
-
-
-
-
-
-
-	
-	private void controlPidWithJoystick() {
-		double joystickPosition = -Robot.oi.getOperatorController().getRightYAxis();
-		double deltaPosition = joystickPosition * joystickInchesPerMs;
-		targetPositionInchesPID = targetPositionInchesPID + deltaPosition;
-		updatePositionPID(targetPositionInchesPID);
-	}
-
-	private void ControlWithJoystickhold(){
-		double holdPosition = wristmotor1.getPositionWorld();
-		updatePositionPID(holdPosition);
-
-	}
-	
-	private void controlManualWithJoystick() {
-		double joyStickSpeed = -Robot.oi.getOperatorController().getRightYAxis();
-		setSpeedJoystick(joyStickSpeed*.10);
-	}
-	/*
-	public void setShiftState(ElevatorSpeedShiftState state) {
-
-			joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_HI;
-			speedShift.set(true);
-			mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
-		}
-		else if(state == ElevatorSpeedShiftState.LO) {
-			joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
-			speedShift.set(false);
-			mpController.setPID(pidPIDParamsLoGear, PID_SLOT);
-		}
-	}
-	
-	public ElevatorSpeedShiftState getShiftState() {
-		return shiftState;
-	}
-*/
-	public double getPositionInches() {
-		return wristmotor1.getPositionWorld();
-	}
-	
-//	public double getAverageMotorCurrent() {
-//		return (Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID)) / 3;
-//	}
-		
-	public double getAverageMotorCurrent() {
-		return (wristmotor1.getOutputCurrent());
-	}
-		
-	public synchronized boolean isFinished() {
+  public synchronized boolean isFinished() 
+  {
 		return isFinished;
 	}
-	
-	public synchronized void setFinished(boolean isFinished) {
+  
+  public synchronized void setFinished(boolean isFinished) 
+  {
 		this.isFinished = isFinished;
 	}
-		
-	public double getPeriodMs() {
+
+  public double getPeriodMs() 
+  {
 		return periodMs;
 	}
-	
-	public void updateStatus(Robot.OperationMode operationMode) {
-		if (operationMode == Robot.OperationMode.TEST) {
-			try {
-				SmartDashboard.putNumber("Elevator Position Inches", wristmotor1.getPositionWorld());
-				SmartDashboard.putNumber("Elevator Motor 1 Amps", wristmotor1.getOutputCurrent());
-				SmartDashboard.putNumber("Elevator Average Amps", getAverageMotorCurrent());
-//				SmartDashboard.putNumber("Elevator Motor 1 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID));
-//				SmartDashboard.putNumber("Elevator Motor 2 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID));
-//				SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID));
-				SmartDashboard.putNumber("Elevator Target PID Position", targetPositionInchesPID);
+
+  @Override
+  public void initDefaultCommand() 
+  {
+  }
+
+  public void updateStatus(Robot.OperationMode operationMode) 
+	{
+		if (operationMode == Robot.OperationMode.TEST) 
+		{
+			try 
+			{
 			}
-			catch (Exception e) {
+			catch (Exception e)
+			{
+				System.err.println("Wrist update status error");
 			}
 		}
 	}	

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

@@ -0,0 +1,80 @@
+package org.usfirst.frc4388.utility;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+public class CANTalonEncoder extends WPI_TalonSRX
+{
+	public static int TIMEOUT_MS = 0;
+
+	private double encoderTicksToWorld;
+	private boolean isRight = true;
+	
+
+	
+	public CANTalonEncoder(int deviceNumber, double encoderTicksToWorld, FeedbackDevice feedbackDevice) {
+		this(deviceNumber, encoderTicksToWorld, false, feedbackDevice);
+	}
+
+	public CANTalonEncoder(int deviceNumber, double encoderTicksToWorld, boolean isRight, FeedbackDevice feedbackDevice) {
+		super(deviceNumber);
+		//this.setFeedbackDevice(feedbackDevice);
+		this.configSelectedFeedbackSensor(feedbackDevice, 0, 0);
+		this.encoderTicksToWorld = encoderTicksToWorld;
+		this.isRight = isRight;
+	}
+
+    public boolean isRight() {
+		return isRight;
+	}
+
+	public void setRight(boolean isRight) {
+		this.isRight = isRight;
+	}
+
+	public double convertEncoderTicksToWorld(double encoderTicks) 
+	{
+    	return encoderTicks / encoderTicksToWorld;
+	}
+	
+	public void setWorld(ControlMode mode, double worldValue) 
+	{
+    	this.set(mode, convertEncoderWorldToTicks(worldValue));
+    }
+
+	public double convertEncoderWorldToTicks(double worldValue) 
+	{
+    	return worldValue * encoderTicksToWorld;
+    }
+    
+	public void setWorld(double worldValue) 
+	{
+    	//this.set(convertEncoderWorldToTicks(worldValue));
+    	this.set(getControlMode(), convertEncoderWorldToTicks(worldValue));		//TODO: change to explicit mode set?
+    }
+    
+	public void setPositionWorld(double worldValue) 
+	{
+    	//this.setPosition(convertEncoderWorldToTicks(worldValue));
+    	this.setSelectedSensorPosition((int)convertEncoderWorldToTicks(worldValue), 0, 0);	//TODO: verify
+    }
+    
+	public double getPositionWorld() 
+	{
+    	//return convertEncoderTicksToWorld(this.getPosition());
+    	return convertEncoderTicksToWorld(this.getSelectedSensorPosition(0));	//TODO: verify want 0="Primary closed-loop"
+	}
+    
+	public void setVelocityWorld(double worldValue) 
+	{
+    	//this.set(convertEncoderWorldToTicks(worldValue) * 0.1);
+    	this.set(getControlMode(), convertEncoderWorldToTicks(worldValue) * 0.1);	//TODO: change to explicit mode set?
+    }
+    
+	public double getVelocityWorld() 
+	{
+    	//return convertEncoderTicksToWorld(this.getSpeed() / 0.1);
+    	return convertEncoderTicksToWorld(this.getSelectedSensorVelocity(0) / 0.1);	//TODO: verify want 0="Primary closed-loop"
+    }
+}

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

@@ -0,0 +1,85 @@
+
+package org.usfirst.frc4388.utility;
+
+import java.util.ArrayList;
+
+import org.usfirst.frc4388.robot.Robot;
+import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+import edu.wpi.first.wpilibj.Timer;
+
+public class SoftwarePIDPositionController
+{	
+	//protected ArrayList<CANTalonEncoder> motorControllers;	
+	protected WPI_TalonSRX motorController;
+	protected PIDParams pidParams;
+	
+	protected PIDParams PValue;
+	
+	protected double targetEncoderPosition;
+	
+	protected double minTurnOutput = 0.002;
+	protected double maxError;
+	protected double minError;
+	protected double maxPrevError; ///??
+	protected double prevError = 0.0; // the prior error (used to compute velocity)
+	protected double totalError = 0.0; // the sum of the errors for use in the integral calc
+	
+	public SoftwarePIDPositionController(PIDParams PValue, WPI_TalonSRX elevatorLeft) 
+	{
+		this.motorController = elevatorLeft;
+		setP(PValue);
+	}
+    
+	public void setP(PIDParams PValue) 
+	{
+		this.PValue = PValue;
+	}
+	
+	public void setPIDPositionTarget(double targetPosition, double maxError, double minError)
+	{
+		this.targetEncoderPosition = targetPosition;
+		
+		this.maxError = maxError;
+		this.minError = minError;
+		//this.maxPrevError = maxPrevError;
+		
+		prevError = 0.0;
+		totalError = 0.0;
+	}
+	
+	public boolean controlLoopUpdate() 
+	{
+		return controlLoopUpdate(0);
+	}
+	
+	public boolean controlLoopUpdate(double currentEncoderPosition) 
+	{
+		// Calculate the motion profile feed forward and error feedback terms
+		double error = targetEncoderPosition - currentEncoderPosition;
+		//double deltaLastError = (error - prevError);
+		
+		//Updating the error
+		//totalError += error;
+		
+		// Check if we are finished
+		if (Math.abs(error) < maxError && Math.abs(error) > minError) 
+		{
+			//Robot.elevator.holdInPos();
+			
+			return true;
+		}
+		
+		double output =  pidParams.kP * error; // + pidParams.kI * totalError + pidParams.kD * deltaLastError;
+		
+		prevError = error;
+		
+		// Update the controllers set point.
+		motorController.set(ControlMode.PercentOutput, output);
+		
+		return false;
+	}
+}