Finally got this code reviewed, bugfixes

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / subsystems / DriveTrain.java

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index 779bd9dec0ad0e0c98ad0fe440d6ffc181babe78..73dcde7087954b041c3e102516b973dfa4de42d1 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -1,7 +1,6 @@
 package org.usfirst.frc.team3501.robot.subsystems;
 
 import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.Robot;
 import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
 
 import edu.wpi.first.wpilibj.CANTalon;
@@ -13,7 +12,9 @@ import edu.wpi.first.wpilibj.RobotDrive;
 import edu.wpi.first.wpilibj.command.PIDSubsystem;
 
 public class DriveTrain extends PIDSubsystem {
+  // Determines if the "front" of the robot has been reversed
   private boolean outputFlipped = false;
+
   private static double pidOutput = 0;
 
   private Encoder leftEncoder, rightEncoder;
@@ -194,16 +195,30 @@ public class DriveTrain extends PIDSubsystem {
 
   public void joystickDrive(double left, double right) {
     int type = Constants.DriveTrain.DRIVE_TYPE;
+
+    // Handle flipping of the "front" of the robot
     double k = (isFlipped() ? -1 : 1);
-    if (type == Constants.DriveTrain.TANK_DRIVE) {
+
+    if (type == Constants.DriveTrain.TANK) {
+      // TODO Test this for negation and for voltage vs. [-1,1] outputs
+      double leftSpeed = (-frontLeft.get() + -rearLeft.get()) / 2;
+      double rightSpeed = (-frontRight.get() + -rearRight.get()) / 2;
+      left = ((Constants.DriveTrain.kADJUST - 1) * leftSpeed + left)
+          / Constants.DriveTrain.kADJUST;
+      right = ((Constants.DriveTrain.kADJUST - 1) * rightSpeed + right)
+          / Constants.DriveTrain.kADJUST;
       robotDrive.tankDrive(-left * k, -right * k);
-    } else if (type == Constants.DriveTrain.ARCADE_DRIVE) {
+    } else if (type == Constants.DriveTrain.ARCADE) {
+      double speed = (-frontLeft.get() + -rearLeft.get() + -frontRight.get() + -rearRight
+          .get()) / 2;
+      left = ((Constants.DriveTrain.kADJUST - 1) * speed + left)
+          / Constants.DriveTrain.kADJUST;
       robotDrive.arcadeDrive(left * k, right);
     }
   }
 
   public void setMotorSpeeds(double left, double right) {
-    double k = (Robot.driveTrain.isFlipped() ? -1 : 1);
+    double k = (isFlipped() ? -1 : 1);
     robotDrive.tankDrive(-left * k, -right * k);
   }