Made DriveStraight a method correctStraightness() in drive train. NOTE: Tends to...

[3501/2017steamworks] / 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 364340e106f2901ab65720f71d0f402df0aa950d..9dd8d8c9c6a708f828707a8262bf70afa56e8031 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -1,6 +1,7 @@
 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 com.ctre.CANTalon;
@@ -64,6 +65,62 @@ public class DriveTrain extends Subsystem {
     setMotorValues(0, 0);
   }
 
+  public void correctStraightness() {
+
+    double speedL, speedR, tempSpeedR;
+
+    // Max speed at mv of 0.2 (RIGHT)
+    final double maxSpeedOneR = 16.1;
+    // Max speed at mv of 0.4 (RIGHT)
+    final double maxSpeedTwoR = 49;
+    // Max speed at mv of 0.6 (RIGHT)
+    final double maxSpeedThreeR = 75.2;
+    // Max speed at mv of 0.2 (LEFT)
+    final double maxSpeedOneL = 13.8;
+    // Max speed at mv of 0.4 (LEFT)
+    final double maxSpeedTwoL = 46.9;
+    // Max speed at mv of 0.6 (LEFT)
+    final double maxSpeedThreeL = 74;
+
+    speedL = Robot.getDriveTrain().getLeftSpeed();
+    speedR = Robot.getDriveTrain().getRightSpeed();
+
+    // Motor values for both motors
+    Double mvR = null, mvL = null;
+
+    // NOTE: Make threshold. Robot tends to go haywire.
+    // getting speed for both motors and exchanging them
+    speedL = Robot.getDriveTrain().getLeftSpeed();
+    speedR = Robot.getDriveTrain().getRightSpeed();
+    tempSpeedR = speedR;
+    speedR = speedL;
+    speedL = tempSpeedR;
+
+    // converting right motor speed to motor value
+    if (speedR <= maxSpeedOneR) {
+      mvR = speedR / maxSpeedOneR * 5;
+    } else if (speedR <= maxSpeedTwoR) {
+      mvR = speedR / maxSpeedTwoR * 2.5;
+    } else if (speedR <= maxSpeedThreeR) {
+      mvR = speedR / maxSpeedThreeR * 1.66;
+    }
+
+    // converting left motor speed to motor value
+    if (speedL <= maxSpeedOneL) {
+      mvL = speedL / maxSpeedOneL * 5;
+    } else if (speedL <= maxSpeedTwoL) {
+      mvL = speedL / maxSpeedTwoL * 2.5;
+    } else if (speedL <= maxSpeedThreeL) {
+      mvL = speedL / maxSpeedThreeL * 1.66;
+    }
+
+    // Setting motors at new motor values
+    if (mvR != null && mvL != null) {
+      Robot.getDriveTrain().setMotorValues(mvR, mvL); // we changed move to //
+    } // speed
+
+  }
+
   public double getFrontLeftMotorVal() {
     return frontLeft.get();
   }