teleop working on practice bot

After much arduous troubleshooting and a bad CIM, it works!
Proper inversions for joysticks and motors, shifting gears works as
expected (changed buttons a bit though), shifting drive type works as
expected

Author: DriverStationComputer

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

@@ -45,10 +45,6 @@ public int getButton(String key, int def) {
 
 	public OI() {
 		leftJoy = new Joystick(0);
-		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));
@@ -57,6 +53,10 @@ public OI() {
 		PIDTurnButton.whenPressed(new PIDTurn(30, Robot.dt, RobotMap.fancyGyro));
 
 		rightJoy = new Joystick(1);
+		shiftHighGearButton = new JoystickButton(rightJoy, getButton("Shift High Gear", 3));
+		shiftHighGearButton.whenPressed(new ShiftHighGear());
+		shiftLowGearButton = new JoystickButton(rightJoy, getButton("Shift Low Gear", 2));
+		shiftLowGearButton.whenPressed(new ShiftLowGear());
 		updatePIDConstantsButton = new JoystickButton(rightJoy, getButton("Get PID Constants", 8));
 		updatePIDConstantsButton.whenPressed(new UpdatePIDConstants());
 		updateEncoderDPPButton = new JoystickButton(rightJoy, getButton("Get Encoder Dist Per Pulse", 9));

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

@@ -174,14 +174,17 @@ public void testPeriodic() {
 //		if(firstTime) {
 //			Robot.dt.enableVelocityPIDs();
 //			firstTime = false;
-//		}
-		Robot.dt.setVPIDs(Robot.getConst("VPID Test Set", 0.5));
-		SmartDashboard.putNumber("Drivetrain/Left VPID Targ", Robot.dt.getLeftVPIDSetpoint());
-		SmartDashboard.putNumber("Drivetrain/Right VPID Targ", Robot.dt.getRightVPIDSetpoint());
-		SmartDashboard.putNumber("Left VPID Error", Robot.dt.getLeftVPIDerror());
-		SmartDashboard.putNumber("Right VPID Error", Robot.dt.getRightVPIDerror());
-		SmartDashboard.putNumber("Left Enc Rate", Robot.dt.getLeftEncRate());
-		SmartDashboard.putNumber("Right Enc Rate", Robot.dt.getRightEncRate());
+////		}
+//		Robot.dt.setVPIDs(Robot.getConst("VPID Test Set", 0.5));
+//		SmartDashboard.putNumber("Drivetrain/Left VPID Targ", Robot.dt.getLeftVPIDSetpoint());
+//		SmartDashboard.putNumber("Drivetrain/Right VPID Targ", Robot.dt.getRightVPIDSetpoint());
+//		SmartDashboard.putNumber("Left VPID Error", Robot.dt.getLeftVPIDerror());
+//		SmartDashboard.putNumber("Right VPID Error", Robot.dt.getRightVPIDerror());
+//		SmartDashboard.putNumber("Left Enc Rate", Robot.dt.getLeftEncRate());
+//		SmartDashboard.putNumber("Right Enc Rate", Robot.dt.getRightEncRate());
+		
+		dt.dtLeft.set(-oi.leftJoy.getY());
+		dt.dtRight.set(-oi.rightJoy.getY());
 
 	}
 }

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

@@ -68,11 +68,20 @@ public class RobotMap {
 	 *            the TalonSRX to configure
 	 */
 	private void configSRX(WPI_TalonSRX mc) {
+		// stuff cole said to put
 		int kTimeout = (int) Robot.getConst("kTimeoutMs", 10);
 		mc.configNominalOutputForward(0, kTimeout);
 		mc.configNominalOutputReverse(0, kTimeout);
 		mc.configPeakOutputForward(1, kTimeout);
 		mc.configPeakOutputReverse(-1, kTimeout);
+
+		// current limiting stuff cole said to put
+		mc.configPeakCurrentLimit(0, 0);
+		mc.configPeakCurrentDuration(0, 0);
+		// 40 amps is the amp limit of a CIM. also, the PDP has 40 amp circuit breakers,
+		// so if we went above 40, the motors would stop completely
+		mc.configContinuousCurrentLimit(40, 0);
+		mc.enableCurrentLimit(true);
 	}
 
 	/**
@@ -83,6 +92,7 @@ private void configSRX(WPI_TalonSRX mc) {
 	 *            the VictorSPX to configure
 	 */
 	private void configSPX(WPI_VictorSPX mc) {
+		// stuff cole said to put
 		int kTimeout = (int) Robot.getConst("kTimeoutMs", 10);
 		mc.configNominalOutputForward(0, kTimeout);
 		mc.configNominalOutputReverse(0, kTimeout);
@@ -92,12 +102,12 @@ 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);
+		// 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);
 
 		leftEncPort1 = new DigitalInput(getPort("1LeftEnc", 1));
 		leftEncPort2 = new DigitalInput(getPort("2LeftEnc", 0));
@@ -107,12 +117,14 @@ public RobotMap() {
 		leftEncRate.setPIDSourceType(PIDSourceType.kRate);
 		leftEncDist.setDistancePerPulse(Robot.getConst("Distance Per Pulse Left", 0.184));
 		leftEncRate.setDistancePerPulse(Robot.getConst("Distance Per Pulse Left", 0.184));
-		
-		dtLeftMaster = new WPI_TalonSRX(getPort("LeftTalonSRXMaster", 2));
+
+		dtLeftMaster = new WPI_TalonSRX(getPort("LeftTalonSRXMaster", 1));
 		configSRX(dtLeftMaster);
-		dtLeftSlave = new WPI_VictorSPX(getPort("LeftVictorSPXSlave", 0));
+		dtLeftSlave = new WPI_VictorSPX(getPort("LeftVictorSPXSlave", 2));
 		configSPX(dtLeftSlave);
 		dtLeft = new SpeedControllerGroup(dtLeftMaster, dtLeftSlave);
+		//inverted bc gear boxes invert from input to output
+		dtLeft.setInverted(true);
 
 		leftVelocityController = new VelocityPIDController(Robot.getConst("VelocityLeftkP", 0),
 				Robot.getConst("VelocityLeftkI", 0), Robot.getConst("VelocityLeftkD", 0),
@@ -122,6 +134,7 @@ public RobotMap() {
 		leftVelocityController.setOutputRange(-1.0, 1.0);
 		leftVelocityController.setContinuous(false);
 		leftVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceLeft", 2));
+		SmartDashboard.putData(leftVelocityController);
 
 		rightEncPort1 = new DigitalInput(getPort("1RightEnc", 3));
 		rightEncPort2 = new DigitalInput(getPort("2RightEnc", 2));
@@ -131,13 +144,15 @@ public RobotMap() {
 		rightEncRate.setPIDSourceType(PIDSourceType.kRate);
 		rightEncDist.setDistancePerPulse(Robot.getConst("Distance Per Pulse Right", 0.184));
 		rightEncRate.setDistancePerPulse(Robot.getConst("Distance Per Pulse Right", 0.184));
-		
-		dtRightMaster = new WPI_TalonSRX(getPort("RightTalonSRXMaster", 1));
+
+		dtRightMaster = new WPI_TalonSRX(getPort("RightTalonSRXMaster", 4));
 		configSRX(dtRightMaster);
 		dtRightSlave = new WPI_VictorSPX(getPort("RightVictorSPXSlave", 3));
 		configSPX(dtRightSlave);
 		dtRight = new SpeedControllerGroup(dtRightMaster, dtRightSlave);
-		
+		//inverted bc gear boxes invert from input to output
+		dtRight.setInverted(true);
+
 		rightVelocityController = new VelocityPIDController(Robot.getConst("VelocityRightkP", 0),
 				Robot.getConst("VelocityRightkI", 0), Robot.getConst("VelocityRightkD", 0),
 				1 / Robot.getConst("Max Low Speed", 84), rightEncRate, dtRight);
@@ -147,13 +162,14 @@ public RobotMap() {
 		rightVelocityController.setContinuous(false);
 		rightVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceRight", 2));
 
-//		robotDrive = new DifferentialDrive(leftVelocityController, rightVelocityController);
-//		robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
+		// robotDrive = new DifferentialDrive(leftVelocityController,
+		// rightVelocityController);
+		// robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
 		robotDrive = new DifferentialDrive(dtLeft, dtRight);
-		
+
 		distEncAvg = new PIDSourceAverage(leftEncDist, rightEncDist);
 		fancyGyro = new AHRS(SerialPort.Port.kMXP);
-		// dtGear = new DoubleSolenoid(getPort("1dtGearSolenoid", 0), getPort("2dtGearSolenoid", 1));
+		dtGear = new DoubleSolenoid(getPort("1dtGearSolenoid", 0), getPort("2dtGearSolenoid", 1));
 	}
 
 	/**

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftHighGear.java

@@ -19,7 +19,7 @@ public ShiftHighGear() {
 	// Called just before this Command runs the first time
 	@Override
 	protected void initialize() {
-//		Robot.dt.shiftGears(true);
+		Robot.dt.shiftGears(true);
 		SmartDashboard.putBoolean("High Gear", true);
 		Robot.dt.resetVelocityPIDkFConsts();
 	}

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftLowGear.java

@@ -19,7 +19,7 @@ public ShiftLowGear() {
 	// Called just before this Command runs the first time
 	@Override
 	protected void initialize() {
-//		Robot.dt.shiftGears(false);
+		Robot.dt.shiftGears(false);
 		SmartDashboard.putBoolean("High Gear", false);
 		Robot.dt.resetVelocityPIDkFConsts();
 	}

Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java

@@ -264,15 +264,15 @@ public double getRightEncDist() {
 	 * Activates the solenoid to push the drivetrain into high or low gear.
 	 * 
 	 * @param highGear
-	 *            If the solenoid is to be pushed into high gear (true, kForward) or
-	 *            low gear (false, kReverse)
+	 *            If the solenoid is to be pushed into high gear (true, kReverse) or
+	 *            low gear (false, kForward)
 	 */
 	@Override
 	public void shiftGears(boolean highGear) {
 		if (highGear ^ Robot.getBool("Drivetrain Gear Shift Low", false)) {
-			dtGear.set(DoubleSolenoid.Value.kForward);
-		} else {
 			dtGear.set(DoubleSolenoid.Value.kReverse);
+		} else {
+			dtGear.set(DoubleSolenoid.Value.kForward);
 		}
 	}