happy VPID!

VPID is tuned and working in both high and low gears

Author: DriverStationComputer

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

@@ -135,8 +135,8 @@ public RobotMap() {
 		dtLeft.setInverted(true);
 
 		leftVelocityController = new VelocityPIDController(Robot.getConst("VelocityLeftkI", 0), 0,
-				Robot.getConst("VelocityLeftkD", calcDefkD()), 1 / Robot.getConst("Max Low Speed", 84), leftEncRate,
-				dtLeft);
+				Robot.getConst("VelocityLeftkD", calcDefkD(Robot.getConst("Max Low Speed", 84))),
+				1 / Robot.getConst("Max Low Speed", 84), leftEncRate, dtLeft);
 		leftVelocityController.setInputRange(-Robot.getConst("Max High Speed", 204),
 				Robot.getConst("Max High Speed", 204));
 		leftVelocityController.setOutputRange(-1.0, 1.0);
@@ -152,6 +152,7 @@ public RobotMap() {
 		rightEncRate.setPIDSourceType(PIDSourceType.kRate);
 		rightEncDist.setDistancePerPulse(Robot.getConst("DPP", DIST_PER_PULSE_RATIO));
 		rightEncRate.setDistancePerPulse(Robot.getConst("DPP", DIST_PER_PULSE_RATIO));
+		rightEncRate.setReverseDirection(true);
 
 		dtRightMaster = new WPI_TalonSRX(getPort("RightTalonSRXMaster", 4));
 		configSRX(dtRightMaster);
@@ -162,18 +163,17 @@ public RobotMap() {
 		dtRight.setInverted(true);
 
 		rightVelocityController = new VelocityPIDController(Robot.getConst("VelocityRightkI", 0), 0,
-				Robot.getConst("VelocityRightkD", calcDefkD()), 1 / Robot.getConst("Max Low Speed", 84), rightEncRate,
-				dtRight);
+				Robot.getConst("VelocityRightkD", calcDefkD(Robot.getConst("Max Low Speed", 84))),
+				1 / Robot.getConst("Max Low Speed", 84), rightEncRate, dtRight);
 		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(dtLeft, dtRight);
+		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);
@@ -203,7 +203,7 @@ public int getPort(String key, int def) {
 	/**
 	 * Uses SmartDashboard and math to calculate a *great* default kD
 	 */
-	public double calcDefkD() {
+	public double calcDefkD(double maxSpeed) {
 		/*
 		 * timeConstant is proportional to max speed of the shaft (which is the max
 		 * speed of the cim divided by the gear reduction), half the mass (because the
@@ -224,7 +224,7 @@ public double calcDefkD() {
 		 * one.
 		 */
 		double denominator = Math.pow(Math.E, 1 * cycleTime / timeConstant) - 1;
-		return 1 / denominator;
+		return 1 / denominator / maxSpeed;
 	}
 
 	private double convertLbsTokG(double lbs) {

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

@@ -64,7 +64,7 @@ public void set(double speed) {
 	 */
 	@Override
 	public double get() {
-		return getSetpoint();
+		return out.get();
 	}
 
 	/**

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

@@ -23,14 +23,14 @@ public TeleopDrive() {
 	// Called just before this Command runs the first time
 	@Override
 	protected void initialize() {
-		// Robot.dt.enableVelocityPIDs();
+		Robot.dt.enableVelocityPIDs();
 	}
 
 	// Called repeatedly when this Command is scheduled to run
 	@Override
 	protected void execute() {
 		Robot.dt.resetVPIDInputRanges();
-		// Robot.dt.updatePidConstants();
+		Robot.dt.updatePidConstants();
 		Robot.dt.teleopDrive();
 	}
 

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

@@ -195,10 +195,8 @@ public double getDtRightSpeed() {
 	 */
 	@Override
 	public void updatePidConstants() {
-		leftVelocityController.setPID(Robot.getConst("VelocityLeftkI", 0), Robot.getConst("VelocityLeftkI", 0),
-				Robot.getConst("VelocityLeftkD", 0));
-		rightVelocityController.setPID(Robot.getConst("VelocityRightkI", 0), Robot.getConst("VelocityRightkI", 0),
-				Robot.getConst("VelocityRightkD", 0));
+		leftVelocityController.setPID(Robot.getConst("VelocityLeftkI", 0), 0, calcDefkD(getCurrentMaxSpeed()));
+		rightVelocityController.setPID(Robot.getConst("VelocityRightkI", 0), 0, calcDefkD(getCurrentMaxSpeed()));
 		resetVelocityPIDkFConsts();
 	}
 
@@ -366,4 +364,36 @@ public void putVelocityControllersToDashboard() {
 		SmartDashboard.putData("Left PID Controller", leftVelocityController);
 		SmartDashboard.putData("Right PID Controller", rightVelocityController);
 	}
+
+	/**
+	 * Uses SmartDashboard and math to calculate a *great* default kD
+	 */
+	public double calcDefkD(double maxSpeed) {
+		/*
+		 * timeConstant is proportional to max speed of the shaft (which is the max
+		 * speed of the cim divided by the gear reduction), half the mass (because the
+		 * half of the drivetrain only has to support half of the robot), and radius of
+		 * the drivetrain wheels squared. It's inversely proportional to the stall
+		 * torque of the shaft, which is found by multiplying the stall torque of the
+		 * motor with the gear reduction by the amount of motors.
+		 */
+		double gearReduction = Robot.getBool("High Gear", false) ? Robot.getConst("High Gear Gear Reduction", 5.392)
+				: Robot.getConst("Low Gear Gear Reduction", 12.255);
+		double radius = Robot.getConst("Radius of Drivetrain Wheel", 0.0635);
+		double timeConstant = Robot.getConst("Omega Max", 5330) / gearReduction / 60 * 2 * Math.PI
+				* convertNtokG(Robot.getConst("Weight of Robot", 342)) / 2 * radius * radius
+				/ (Robot.getConst("Stall Torque", 2.41) * gearReduction * 2);
+		double cycleTime = Robot.getConst("Code cycle time", 0.05);
+		/*
+		 * The denominator of kD is 1-(e ^ -cycleTime / timeConstant). The numerator is
+		 * one.
+		 */
+		double denominator = Math.pow(Math.E, 1 * cycleTime / timeConstant) - 1;
+		return 1 / denominator / maxSpeed;
+	}
+
+	private double convertNtokG(double newtons) {
+		// weight / accel due to grav = kg
+		return newtons / 9.81;
+	}
 }