getting new teleop working

fixed motor ports and joystick inversion
commented out all VPID stuff for now --> next to test
changed encoder ports a bit but might need revision

Author: DriverStationComputer

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

@@ -170,5 +170,7 @@ public void teleopPeriodic() {
 	 */
 	@Override
 	public void testPeriodic() {
+//		Robot.dt.setLeft(0.2);
+		Robot.dt.setRight(0.2);
 	}
 }

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

@@ -92,63 +92,72 @@ 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);
-
-		leftEncPort1 = new DigitalInput(getPort("1LeftEnc", 0));
-		leftEncPort2 = new DigitalInput(getPort("2LeftEnc", 1));
+//		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));
 		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));
+		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));
 		configSRX(dtLeftMaster);
-		dtLeftSlave = new WPI_VictorSPX(getPort("LeftTalonSPXSlave", 1));
+		dtLeftSlave = new WPI_VictorSPX(getPort("LeftVictorSPXSlave", 0));
 		configSPX(dtLeftSlave);
 		dtLeft = new SpeedControllerGroup(dtLeftMaster, dtLeftSlave);
 
 		leftVelocityController = new VelocityPIDController(Robot.getConst("VelocityLeftkP", 1),
 				Robot.getConst("VelocityLeftkI", 0), Robot.getConst("VelocityLeftkD", 0),
 				1 / Robot.getConst("Max Low Speed", 84), leftEncRate, dtLeft);
-		leftVelocityController.enable();
+//		leftVelocityController.enable();
 		leftVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
 				Robot.getConst("Max High Speed", 204));
 		leftVelocityController.setOutputRange(-1.0, 1.0);
 		leftVelocityController.setContinuous(false);
 		leftVelocityController.setAbsoluteTolerance(Robot.getConst("VelocityToleranceLeft", 2));
 
-		rightEncPort1 = new DigitalInput(getPort("1RightEnc", 2));
-		rightEncPort2 = new DigitalInput(getPort("2RightEnc", 3));
-		rightEncDist = new Encoder(leftEncPort1, leftEncPort2);
+		rightEncPort1 = new DigitalInput(getPort("1RightEnc", 3));
+		rightEncPort2 = new DigitalInput(getPort("2RightEnc", 2));
+		rightEncDist = new Encoder(rightEncPort1, rightEncPort2);
 		rightEncDist.setPIDSourceType(PIDSourceType.kDisplacement);
-		rightEncRate = new Encoder(leftEncPort1, leftEncPort2);
+		rightEncRate = new Encoder(rightEncPort1, rightEncPort2);
 		rightEncRate.setPIDSourceType(PIDSourceType.kRate);
-		dtRightMaster = new WPI_TalonSRX(getPort("RightTalonSRXMaster", 2));
+		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));
 		configSRX(dtRightMaster);
-		dtRightSlave = new WPI_VictorSPX(getPort("RightTalonSPXSlave", 3));
+		dtRightSlave = new WPI_VictorSPX(getPort("RightVictorSPXSlave", 3));
 		configSPX(dtRightSlave);
 		dtRight = new SpeedControllerGroup(dtRightMaster, dtRightSlave);
+//		dtRight.setInverted(true);
 
 		rightVelocityController = new VelocityPIDController(Robot.getConst("VelocityRightkP", 1),
 				Robot.getConst("VelocityRightkI", 0), Robot.getConst("VelocityRightkD", 0),
 				1 / Robot.getConst("Max Low Speed", 84), rightEncRate, dtRight);
-		rightVelocityController.enable();
+//		rightVelocityController.enable();
 		rightVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
 				Robot.getConst("Max High Speed", 204));
 		rightVelocityController.setOutputRange(-1.0, 1.0);
 		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 = new DifferentialDrive(dtLeft, dtRight);
+
+//		robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
 
 		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/commands/TeleopDrive.java

@@ -28,6 +28,7 @@ protected void initialize() {
 	// Called repeatedly when this Command is scheduled to run
 	@Override
 	protected void execute() {
+		Robot.dt.resetVPIDInputRanges();
 		Robot.dt.teleopDrive();
 	}
 

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

@@ -20,16 +20,19 @@
 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;
 
 public class Drivetrain extends Subsystem implements DrivetrainInterface {
 	// Put methods for controlling this subsystem
 	// here. Call these from Commands.
 
 	private final Encoder leftEncDist = RobotMap.leftEncDist;
 	private final Encoder rightEncDist = RobotMap.rightEncDist;
+	private final Encoder leftEncRate = RobotMap.leftEncRate;
+	private final Encoder rightEncRate = RobotMap.rightEncRate;
 	private final PIDSourceAverage distEncAvg = RobotMap.distEncAvg;
-	private final SpeedControllerGroup dtLeft = RobotMap.dtLeft;
-	private final SpeedControllerGroup dtRight = RobotMap.dtRight;
+	public final SpeedControllerGroup dtLeft = RobotMap.dtLeft;
+	public final SpeedControllerGroup dtRight = RobotMap.dtRight;
 	private final DifferentialDrive robotDrive = RobotMap.robotDrive;
 	private final VelocityPIDController leftVelocityController = RobotMap.leftVelocityController;
 	private final VelocityPIDController rightVelocityController = RobotMap.rightVelocityController;
@@ -41,6 +44,14 @@ public class Drivetrain extends Subsystem implements DrivetrainInterface {
 	public void initDefaultCommand() {
 		setDefaultCommand(new TeleopDrive());
 	}
+	
+	public void setLeft(double spd) {
+		dtLeft.set(spd);
+	}
+	
+	public void setRight(double spd) {
+		dtRight.set(spd);
+	}
 
 	/**
 	 * Drives based on joystick input and SmartDashboard values
@@ -49,13 +60,21 @@ public void initDefaultCommand() {
 	public void teleopDrive() {
 		if (Robot.getBool("Arcade Drive", true)) {
 			if (Robot.getBool("Arcade Drive Default Setup", true)) {
-				Robot.dt.arcadeDrive(Robot.oi.leftJoy.getY(), Robot.oi.rightJoy.getX());
+				Robot.dt.arcadeDrive(-Robot.oi.leftJoy.getY(), Robot.oi.rightJoy.getX());
 			} else {
-				Robot.dt.arcadeDrive(Robot.oi.rightJoy.getY(), Robot.oi.leftJoy.getX());
+				Robot.dt.arcadeDrive(-Robot.oi.rightJoy.getY(), Robot.oi.leftJoy.getX());
 			}
 		} else {
-			Robot.dt.tankDrive(Robot.oi.leftJoy.getY(), Robot.oi.rightJoy.getY());
+			Robot.dt.tankDrive(-Robot.oi.leftJoy.getY(), -Robot.oi.rightJoy.getY());
 		}
+		SmartDashboard.putNumber("Drivetrain/Left VPID Targ", leftVelocityController.getSetpoint());
+		SmartDashboard.putNumber("Drivetrain/Right VPID Targ", rightVelocityController.getSetpoint());
+		SmartDashboard.putNumber("Drivetrain/Current Max Speed", getCurrentMaxSpeed());
+		SmartDashboard.putNumber("Drivetrain/Left Enc Dist", leftEncDist.getDistance());
+		SmartDashboard.putNumber("Drivetrain/Left Enc Rate", leftEncRate.getRate());
+		SmartDashboard.putNumber("Drivetrain/Right Enc Dist", rightEncDist.getDistance());
+		SmartDashboard.putNumber("Drivetrain/Right Enc Rate", rightEncRate.getRate());
+		SmartDashboard.putNumber("Drivetrain/Enc Avg Dist", distEncAvg.pidGet());
 	}
 
 	/**
@@ -249,6 +268,12 @@ public double resetVelocityPIDkFConsts() {
 		rightVelocityController.setF(newKF);
 		return newKF;
 	}
+	
+	public double resetVPIDInputRanges() {
+		double currentMaxSpd = getCurrentMaxSpeed();
+		leftVelocityController.setInputRange(-currentMaxSpd, currentMaxSpd);
+		return currentMaxSpd;
+	}
 
 	/**
 	 * Gets the current max speed of the DT based on gearing (high or low gear)