From 97772f5b06f28ac5fa0b3f6603deb45c4fa2ed0f Mon Sep 17 00:00:00 2001
From: Brian Semrau <brian.semrau@gmail.com>
Date: Wed, 28 Feb 2018 11:51:18 -0500
Subject: [PATCH] Added voltage compensation to Drivetrain, Elevator

Allows input to be more reliable. This is especially important for autonomous driving.
---
 .../drivetrain/AutoBlindAccelerate.java       |  2 ++
 .../commands/drivetrain/AutoBlindDrive.java   |  1 +
 .../drivetrain/TeleopRacingDrive.java         |  1 +
 .../commands/drivetrain/TeleopTankDrive.java  |  1 +
 .../team4737/robot/subsystems/Drivetrain.java | 19 +++++++++++++++++++
 .../team4737/robot/subsystems/Elevator.java   |  4 ++--
 6 files changed, 26 insertions(+), 2 deletions(-)

diff --git a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindAccelerate.java b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindAccelerate.java
index 7a4bac8..7dbe617 100644
--- a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindAccelerate.java
+++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindAccelerate.java
@@ -43,6 +43,8 @@ protected void initialize() {
 		lastTime = 0;
 		percent = startPercent;
 		startLow = startPercent < targetPercent;
+		
+		Robot.DRIVETRAIN.enableVoltageCompensation();
 	}
 
 	// Called repeatedly when this Command is scheduled to run
diff --git a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindDrive.java b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindDrive.java
index 690ce04..17d23e6 100644
--- a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindDrive.java
+++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/AutoBlindDrive.java
@@ -32,6 +32,7 @@ public AutoBlindDrive(double time, double percent) {
 
 	// Called just before this Command runs the first time
 	protected void initialize() {
+		Robot.DRIVETRAIN.enableVoltageCompensation();
 	}
 
 	// Called repeatedly when this Command is scheduled to run
diff --git a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopRacingDrive.java b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopRacingDrive.java
index 5fd8fa8..fa848c2 100644
--- a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopRacingDrive.java
+++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopRacingDrive.java
@@ -17,6 +17,7 @@ public TeleopRacingDrive() {
 	// Called just before this Command runs the first time
 	protected void initialize() {
 		Robot.DRIVETRAIN.setBrakeMode();
+		Robot.DRIVETRAIN.disableVoltageCompensation();
 	}
 
 	// Called repeatedly when this Command is scheduled to run
diff --git a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopTankDrive.java b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopTankDrive.java
index 213448a..141679c 100644
--- a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopTankDrive.java
+++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopTankDrive.java
@@ -17,6 +17,7 @@ public TeleopTankDrive() {
 	// Called just before this Command runs the first time
 	protected void initialize() {
 		Robot.DRIVETRAIN.setBrakeMode();
+		Robot.DRIVETRAIN.disableVoltageCompensation();
 	}
 
 	// Called repeatedly when this Command is scheduled to run
diff --git a/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java b/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java
index c5c06bf..dca19de 100644
--- a/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java
+++ b/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java
@@ -32,6 +32,11 @@ public Drivetrain() {
 
 		leftFrontMaster.configOpenloopRamp(0.25, 30);
 		rightFrontMaster.configOpenloopRamp(0.25, 30);
+		
+		leftFrontMaster.configVoltageCompSaturation(12, 30);
+		leftBackSlave.configVoltageCompSaturation(12, 30);
+		rightFrontMaster.configVoltageCompSaturation(12, 30);
+		rightBackSlave.configVoltageCompSaturation(12, 30);
 
 		drive = new DifferentialDrive(leftFrontMaster, rightFrontMaster);
 	}
@@ -54,6 +59,20 @@ public void setCoastMode() {
 		rightFrontMaster.setNeutralMode(NeutralMode.Coast);
 		rightBackSlave.setNeutralMode(NeutralMode.Coast);
 	}
+	
+	public void enableVoltageCompensation() {
+		leftFrontMaster.enableVoltageCompensation(true);
+		leftBackSlave.enableVoltageCompensation(true);
+		rightFrontMaster.enableVoltageCompensation(true);
+		rightBackSlave.enableVoltageCompensation(true);
+	}
+	
+	public void disableVoltageCompensation() {
+		leftFrontMaster.enableVoltageCompensation(false);
+		leftBackSlave.enableVoltageCompensation(false);
+		rightFrontMaster.enableVoltageCompensation(false);
+		rightBackSlave.enableVoltageCompensation(false);
+	}
 
 	/**
 	 * Controls the drivetrain using two tank-drive joystick inputs
diff --git a/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java b/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java
index 5f7573a..3f8b442 100644
--- a/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java
+++ b/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java
@@ -32,8 +32,8 @@ public Elevator() {
 		motor.enableCurrentLimit(true);
 		
 		// Use voltage compensation to keep inputs reliable
-//		motor.configVoltageCompSaturation(12, 30);
-//		motor.enableVoltageCompensation(true);
+		motor.configVoltageCompSaturation(12, 30);
+		motor.enableVoltageCompensation(true);
 	}
 
 	public void initDefaultCommand() {