PID, Drivetrain, cleanup, other little things

.gitignore

@@ -2,5 +2,3 @@ bin/
 build/
 dist/
 .DS_Store
-*auto*
-*Auto*

Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java

@@ -24,43 +24,16 @@
  * interface to the commands and command groups that allow control of the robot.
  */
 public class OI {
-	//// CREATING BUTTONS
-	// One type of button is a joystick button which is any button on a
-	//// joystick.
-	// You create one by telling it which joystick it's on and which button
-	// number it is.
-	// Joystick stick = new Joystick(port);
-	// Button button = new JoystickButton(stick, buttonNumber);
-
-	// There are a few additional built in buttons you can use. Additionally,
-	// by subclassing Button you can create custom triggers and bind those to
-	// commands the same as any other Button.
-
-	//// TRIGGERING COMMANDS WITH BUTTONS
-	// Once you have a button, it's trivial to bind it to a button in one of
-	// three ways:
-
-	// Start the command when the button is pressed and let it run the command
-	// until it is finished as determined by it's isFinished method.
-	// button.whenPressed(new ExampleCommand());
-
-	// Run the command while the button is being held down and interrupt it once
-	// the button is released.
-	// button.whileHeld(new ExampleCommand());
-
-	// Start the command when the button is released and let it run the command
-	// until it is finished as determined by it's isFinished method.
-	// button.whenReleased(new ExampleCommand());
 
 	public Joystick leftJoy;
-	private JoystickButton shiftLowGear;
-	private JoystickButton shiftHighGear;
-	private JoystickButton shiftDriveType;
-	private JoystickButton PIDMove;
-	private JoystickButton PIDTurn;
+	private JoystickButton shiftLowGearButton;
+	private JoystickButton shiftHighGearButton;
+	private JoystickButton shiftDriveTypeButton;
+	private JoystickButton PIDMoveButton;
+	private JoystickButton PIDTurnButton;
 	public Joystick rightJoy;
-	private JoystickButton updatePidConstants;
-	private JoystickButton updateEncoderDPP;
+	private JoystickButton updatePIDConstantsButton;
+	private JoystickButton updateEncoderDPPButton;
 	public Joystick manipulator;
 
 	public int getButton(String key, int def) {
@@ -72,22 +45,22 @@ public int getButton(String key, int def) {
 
 	public OI() {
 		leftJoy = new Joystick(0);
-		shiftLowGear = new JoystickButton(leftJoy, getButton("Shift Low Gear", 3));
-		shiftLowGear.whenPressed(new ShiftLowGear());
-		shiftHighGear = new JoystickButton(leftJoy, getButton("Shift High Gear", 5));
-		shiftHighGear.whenPressed(new ShiftHighGear());
-		shiftDriveType = new JoystickButton(leftJoy, getButton("Shift Drive Type", 2));
-		shiftDriveType.whenPressed(new ShiftDriveType());
-		PIDMove = new JoystickButton(leftJoy, getButton("PID Move", 7));
-		PIDMove.whenPressed(new PIDMove(40, Robot.dt));
-		PIDTurn = new JoystickButton(leftJoy, getButton("PID Turn", 8));
-		PIDTurn.whenPressed(new PIDTurn(30, Robot.dt));
+		shiftLowGearButton = new JoystickButton(leftJoy, getButton("Shift Low Gear", 3));
+		shiftLowGearButton.whenPressed(new ShiftLowGear());
+		shiftHighGearButton = new JoystickButton(leftJoy, getButton("Shift High Gear", 5));
+		shiftHighGearButton.whenPressed(new ShiftHighGear());
+		shiftDriveTypeButton = new JoystickButton(leftJoy, getButton("Shift Drive Type", 2));
+		shiftDriveTypeButton.whenPressed(new ShiftDriveType());
+		PIDMoveButton = new JoystickButton(leftJoy, getButton("PID Move", 7));
+		PIDMoveButton.whenPressed(new PIDMove(40, Robot.dt, RobotMap.distEncAvg));
+		PIDTurnButton = new JoystickButton(leftJoy, getButton("PID Turn", 8));
+		PIDTurnButton.whenPressed(new PIDTurn(30, Robot.dt, RobotMap.fancyGyro));
 
 		rightJoy = new Joystick(1);
-		updatePidConstants = new JoystickButton(rightJoy, getButton("Get PID Constants", 8));
-		updatePidConstants.whenPressed(new UpdatePIDConstants());
-		updateEncoderDPP = new JoystickButton(rightJoy, getButton("Get Encoder Dist Per Pulse", 9));
-		updateEncoderDPP.whenPressed(new SetDistancePerPulse());
+		updatePIDConstantsButton = new JoystickButton(rightJoy, getButton("Get PID Constants", 8));
+		updatePIDConstantsButton.whenPressed(new UpdatePIDConstants());
+		updateEncoderDPPButton = new JoystickButton(rightJoy, getButton("Get Encoder Dist Per Pulse", 9));
+		updateEncoderDPPButton.whenPressed(new SetDistancePerPulse());
 
 		manipulator = new Joystick(2);
 	}

Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java

@@ -13,9 +13,7 @@
 import org.usfirst.frc.team199.Robot2018.subsystems.ClimberAssist;
 import org.usfirst.frc.team199.Robot2018.subsystems.Drivetrain;
 import org.usfirst.frc.team199.Robot2018.subsystems.IntakeEject;
-import org.usfirst.frc.team199.Robot2018.subsystems.LeftDrive;
 import org.usfirst.frc.team199.Robot2018.subsystems.Lift;
-import org.usfirst.frc.team199.Robot2018.subsystems.RightDrive;
 
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Preferences;
@@ -40,8 +38,6 @@ public class Robot extends TimedRobot {
 	public static Lift lift;
 	public static RobotMap rmap;
 	public static Drivetrain dt;
-	public static LeftDrive ld;
-	public static RightDrive rd;
 	public static Listener listen;
 
 	public static OI oi;
@@ -79,9 +75,6 @@ public void robotInit() {
 		intakeEject = new IntakeEject();
 		lift = new Lift();
 		dt = new Drivetrain();
-		rmap.initPIDControllers();
-		ld = new LeftDrive();
-		rd = new RightDrive();
 		oi = new OI();
 
 		// auto position chooser

Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java

@@ -7,61 +7,57 @@
 
 package org.usfirst.frc.team199.Robot2018;
 
-
-import edu.wpi.first.wpilibj.SpeedController;
+import org.usfirst.frc.team199.Robot2018.autonomous.PIDSourceAverage;
+import org.usfirst.frc.team199.Robot2018.autonomous.VelocityPIDController;
 
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
 import com.kauailabs.navx.frc.AHRS;
 
-import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DigitalSource;
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.PIDSourceType;
 import edu.wpi.first.wpilibj.SerialPort;
 import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
-
 /**
  * The RobotMap is a mapping from the ports sensors and actuators are wired into
  * to a variable name. This provides flexibility changing wiring, makes checking
  * the wiring easier and significantly reduces the number of magic numbers
  * floating around.
  */
 public class RobotMap {
-	// For example to map the left and right motors, you could define the
-	// following variables to use with your drivetrain subsystem.
-	// public static int leftMotor = 1;
-	// public static int rightMotor = 2;
-
-	// If you are using multiple modules, make sure to define both the port
-	// number and the module. For example you with a rangefinder:
-	// public static int rangefinderPort = 1;
-	// public static int rangefinderModule = 1;
-
+
 	public static WPI_TalonSRX intakeMotor;
 	public static WPI_TalonSRX liftMotor;
 	public static WPI_TalonSRX climberMotor;
-
-	public static Encoder leftEnc;
-	public static WPI_TalonSRX dtLeftDrive;
+
+	public static DigitalSource leftEncPort1;
+	public static DigitalSource leftEncPort2;
+	public static Encoder leftEncDist;
+	public static Encoder leftEncRate;
+	public static WPI_TalonSRX dtLeftMaster;
 	public static WPI_VictorSPX dtLeftSlave;
 	public static SpeedControllerGroup dtLeft;
+	public static VelocityPIDController leftVelocityController;
 
-	public static Encoder rightEnc;
-	public static WPI_TalonSRX dtRightDrive;
+	public static DigitalSource rightEncPort1;
+	public static DigitalSource rightEncPort2;
+	public static Encoder rightEncDist;
+	public static Encoder rightEncRate;
+	public static WPI_TalonSRX dtRightMaster;
 	public static WPI_VictorSPX dtRightSlave;
 	public static SpeedControllerGroup dtRight;
+	public static VelocityPIDController rightVelocityController;
+
 	public static DifferentialDrive robotDrive;
-	public static PIDController turnController;
-	// public static PIDController moveController;
-	public static PIDController moveLeftController;
-	public static PIDController moveRightController;
+	public static PIDSourceAverage distEncAvg;
 
-	public static AHRS ahrs;
-	public static AnalogGyro dtGyro;
+	public static AHRS fancyGyro;
 	public static DoubleSolenoid dtGear;
 
 	/**
@@ -95,69 +91,61 @@ private void configSPX(WPI_VictorSPX mc) {
 	}
 
 	public RobotMap() {
-		
+
 		intakeMotor = new WPI_TalonSRX(getPort("IntakeTalonSRX", 4));
 		configSRX(intakeMotor);
 		liftMotor = new WPI_TalonSRX(getPort("LiftTalonSRX", 5));
 		configSRX(liftMotor);
 		climberMotor = new WPI_TalonSRX(getPort("ClimberTalonSRX", 6));
 		configSRX(climberMotor);
 
-		leftEnc = new Encoder(getPort("1LeftEnc", 0), getPort("2LeftEnc", 1));
-		dtLeftDrive = new WPI_TalonSRX(getPort("LeftTalonSRXDrive", 0));
-		configSRX(dtLeftDrive);
+		leftEncPort1 = new DigitalInput(getPort("1LeftEnc", 0));
+		leftEncPort2 = new DigitalInput(getPort("2LeftEnc", 1));
+		leftEncDist = new Encoder(leftEncPort1, leftEncPort2);
+		leftEncDist.setPIDSourceType(PIDSourceType.kDisplacement);
+		leftEncRate = new Encoder(leftEncPort1, leftEncPort2);
+		leftEncRate.setPIDSourceType(PIDSourceType.kRate);
+		dtLeftMaster = new WPI_TalonSRX(getPort("LeftTalonSRXMaster", 0));
+		configSRX(dtLeftMaster);
 		dtLeftSlave = new WPI_VictorSPX(getPort("LeftTalonSPXSlave", 1));
 		configSPX(dtLeftSlave);
-		dtLeft = new SpeedControllerGroup(dtLeftDrive, dtLeftSlave);
-
-		rightEnc = new Encoder(getPort("1RightEnc", 2), getPort("2RightEnc", 3));
-		dtRightDrive = new WPI_TalonSRX(getPort("RightTalonSRXDrive", 2));
-		configSRX(dtRightDrive);
+		dtLeft = new SpeedControllerGroup(dtLeftMaster, dtLeftSlave);
+
+		leftVelocityController = new VelocityPIDController(Robot.getConst("MoveLeftkP", 1),
+				Robot.getConst("MoveLeftkI", 0), Robot.getConst("MoveLeftkD", 0), 1 / Robot.getConst("MaxSpeed", 17),
+				leftEncRate, dtLeft);
+		leftVelocityController.enable();
+		leftVelocityController.setInputRange(0, Double.MAX_VALUE);
+		leftVelocityController.setOutputRange(-1.0, 1.0);
+		leftVelocityController.setContinuous(false);
+		leftVelocityController.setAbsoluteTolerance(Robot.getConst("MoveToleranceLeft", 2));
+
+		rightEncPort1 = new DigitalInput(getPort("1RightEnc", 2));
+		rightEncPort2 = new DigitalInput(getPort("2RightEnc", 3));
+		rightEncDist = new Encoder(leftEncPort1, leftEncPort2);
+		rightEncDist.setPIDSourceType(PIDSourceType.kDisplacement);
+		rightEncRate = new Encoder(leftEncPort1, leftEncPort2);
+		rightEncRate.setPIDSourceType(PIDSourceType.kRate);
+		dtRightMaster = new WPI_TalonSRX(getPort("RightTalonSRXMaster", 2));
+		configSRX(dtRightMaster);
 		dtRightSlave = new WPI_VictorSPX(getPort("RightTalonSPXSlave", 3));
 		configSPX(dtRightSlave);
-		dtRight = new SpeedControllerGroup(dtRightDrive, dtRightSlave);
-
-		robotDrive = new DifferentialDrive(dtLeft, dtRight);
-
-		// moveController = new PIDController(Robot.getConst("MovekP", 1),
-		// Robot.getConst("MovekI", 0),
-		// Robot.getConst("MovekD", 0), Robot.dt, Robot.dt);
-		// moveController.disable();
-		// moveController.setInputRange(0, Double.MAX_VALUE);
-		// moveController.setOutputRange(-1.0, 1.0);
-		// moveController.setContinuous(false);
-		// moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 2));
-
-
-		ahrs = new AHRS(SerialPort.Port.kMXP);
-		dtGyro = new AnalogGyro(getPort("Gyro", 0));
-		dtGear = new DoubleSolenoid(getPort("1dtGearSolenoid", 0), getPort("2dtGearSolenoid", 1));
+		dtRight = new SpeedControllerGroup(dtRightMaster, dtRightSlave);
 
-	}
-
-	public void initPIDControllers() {
-		turnController = new PIDController(Robot.getConst("TurnkP", 1), Robot.getConst("TurnkI", 0),
-				Robot.getConst("TurnkD", 0), ahrs, Robot.dt);
-		turnController.disable();
-		turnController.setInputRange(-180, 180);
-		turnController.setOutputRange(-1.0, 1.0);
-		turnController.setContinuous();
-		turnController.setAbsoluteTolerance(Robot.getConst("TurnTolerance", 1));
-
-		moveLeftController = new PIDController(Robot.getConst("MoveLeftkP", 1), Robot.getConst("MoveLeftkI", 0),
-				Robot.getConst("MoveLeftkD", 0), Robot.ld, Robot.ld);
-		moveLeftController.disable();
-		moveLeftController.setInputRange(0, Double.MAX_VALUE);
-		moveLeftController.setOutputRange(-1.0, 1.0);
-		moveLeftController.setContinuous(false);
-		moveLeftController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceLeft", 2));
-		moveRightController = new PIDController(Robot.getConst("ConstMoveRightkP", 1),
-				Robot.getConst("ConstMoveRightkI", 0), Robot.getConst("ConstMoveRightkD", 0), Robot.rd, Robot.rd);
-		moveRightController.disable();
-		moveRightController.setInputRange(0, Double.MAX_VALUE);
-		moveRightController.setOutputRange(-1.0, 1.0);
-		moveRightController.setContinuous(false);
-		moveRightController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceRight", 2));
+		rightVelocityController = new VelocityPIDController(Robot.getConst("MoveRightkP", 1),
+				Robot.getConst("MoveRightkI", 0), Robot.getConst("MoveRightkD", 0), 1 / Robot.getConst("MaxSpeed", 17),
+				rightEncRate, dtRight);
+		rightVelocityController.enable();
+		rightVelocityController.setInputRange(0, Double.MAX_VALUE);
+		rightVelocityController.setOutputRange(-1.0, 1.0);
+		rightVelocityController.setContinuous(false);
+		rightVelocityController.setAbsoluteTolerance(Robot.getConst("MoveToleranceRight", 2));
+
+		robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController);
+
+		distEncAvg = new PIDSourceAverage(leftEncDist, rightEncDist);
+		fancyGyro = new AHRS(SerialPort.Port.kMXP);
+		dtGear = new DoubleSolenoid(getPort("1dtGearSolenoid", 0), getPort("2dtGearSolenoid", 1));
 	}
 
 	/**

Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/PIDSourceAverage.java

@@ -0,0 +1,37 @@
+package org.usfirst.frc.team199.Robot2018.autonomous;
+
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+
+public class PIDSourceAverage implements PIDSource {
+
+	private PIDSource lSrc;
+	private PIDSource rSrc;
+	private PIDSourceType type;
+
+	public PIDSourceAverage(PIDSource leftSource, PIDSource rightSource) {
+		lSrc = leftSource;
+		rSrc = rightSource;
+		if (leftSource.getPIDSourceType().equals(rightSource.getPIDSourceType())) {
+			type = leftSource.getPIDSourceType();
+		} else {
+			throw new IllegalArgumentException();
+		}
+	}
+
+	@Override
+	public void setPIDSourceType(PIDSourceType pidSource) {
+		type = pidSource;
+	}
+
+	@Override
+	public PIDSourceType getPIDSourceType() {
+		return type;
+	}
+
+	@Override
+	public double pidGet() {
+		return (lSrc.pidGet() + rSrc.pidGet()) / 2;
+	}
+
+}