More competition updates, intake speed and joystick scaling

[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 eeac4900e1b9a5f44b8ca2e1951012c9b3b55376..3d406d7cbe506b886f86ed305d052d93e4953b9f 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -195,8 +195,18 @@ public class DriveTrain extends PIDSubsystem {
     int type = Constants.DriveTrain.DRIVE_TYPE;
     double k = (isFlipped() ? -1 : 1);
     if (type == Constants.DriveTrain.TANK) {
+      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) {
+      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);
     }
   }