package org.usfirst.frc.team4828;


import com.ctre.CANTalon;

public class DriveTrain {
    private CANTalon frontLeft;
    private CANTalon frontRight;
    private CANTalon backLeft;
    private CANTalon backRight;

    private static final double TWIST_THRESHOLD = 0.15;

    DriveTrain(int frontLeftPort, int backLeftPort, int frontRightPort, int backRightPort) {
        frontLeft = new CANTalon(frontLeftPort);
        frontRight = new CANTalon(frontRightPort);
        backLeft = new CANTalon(backLeftPort);
        backRight = new CANTalon(backRightPort);
    }

    private static void normalize(double[] wheelSpeeds) {
        double maxMagnitude = Math.abs(wheelSpeeds[0]);
        for (int i = 1; i < 4; i++) {
            double temp = Math.abs(wheelSpeeds[i]);
            if (maxMagnitude < temp) {
                maxMagnitude = temp;
            }
        }
        if (maxMagnitude > 1.0) {
            for (int i = 0; i < 4; i++) {
                wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
            }
        }
    }

    /** rotateVector - Rotate a vector in Cartesian space.
     *
     * @param xcomponent     X component of the vector
     * @param ycomponent     Y component of the vector
     * @param angle Angle by which to rotate the vector
     * @return The resultant vector as a double[2]
     */
    public static double[] rotateVector(double xcomponent, double ycomponent, double angle) {
        double cosA = Math.cos(angle * (3.14159 / 180.0));
        double sinA = Math.sin(angle * (3.14159 / 180.0));
        double[] out = new double[2];
        out[0] = xcomponent * cosA - ycomponent * sinA;
        out[1] = xcomponent * sinA + ycomponent * cosA;
        return out;
    }

    /** mecanumDrive - Adjust motor speeds according to joystick input.
     */
    public void mecanumDrive(double xcomponent, double ycomponent, double rotation) {
        // Ignore tiny inadvertent joystick rotations
        if (Math.abs(rotation) <= TWIST_THRESHOLD) {
             rotation = 0.0;
        }

        // Negate y for the joystick.
        ycomponent = -ycomponent;

        double[] wheelSpeeds = new double[4];
        wheelSpeeds[0] = xcomponent + ycomponent + rotation;
        wheelSpeeds[1] = -xcomponent + ycomponent - rotation;
        wheelSpeeds[2] = -xcomponent + ycomponent + rotation;
        wheelSpeeds[3] = xcomponent + ycomponent - rotation;

        normalize(wheelSpeeds);
        frontLeft.set(wheelSpeeds[0]);
        frontRight.set(wheelSpeeds[1]);
        backLeft.set(wheelSpeeds[2]);
        backRight.set(wheelSpeeds[3]);
    }

    /** mecanumDrive - Adjust motor speeds according to heading and joystick input.
     * Uses input from the gyroscope to determine field orientation.
     */
    public void mecanumDrive(double xcomponent, double ycomponent,
        double rotation, double gyroAngle) {
        // Ignore tiny inadvertent joystick rotations
        if (Math.abs(rotation) <= TWIST_THRESHOLD) {
             rotation = 0.0;
        }

        // Negate y for the joystick.
        ycomponent = -ycomponent;
        // Compensate for gyro angle.
        double[] rotated = rotateVector(xcomponent, ycomponent, gyroAngle);
        xcomponent = rotated[0];
        ycomponent = rotated[1];

        double[] wheelSpeeds = new double[4];
        wheelSpeeds[0] = xcomponent + ycomponent + rotation;
        wheelSpeeds[1] = -xcomponent + ycomponent - rotation;
        wheelSpeeds[2] = -xcomponent + ycomponent + rotation;
        wheelSpeeds[3] = xcomponent + ycomponent - rotation;

        normalize(wheelSpeeds);
        frontLeft.set(wheelSpeeds[0]);
        frontRight.set(wheelSpeeds[1]);
        backLeft.set(wheelSpeeds[2]);
        backRight.set(wheelSpeeds[3]);
    }

    /** testMotors - Turn all wheels slowly for testing purposes.
     */
    public void testMotors() {
        frontLeft.set(.2);
        frontRight.set(.2);
        backLeft.set(.2);
        backRight.set(.2);
    }

}

@@ -560,24 +560,91 @@
 rt);%0A    %7D%0A%0A
+    /**%0A     * Ensures that wheel speeds are valid numbers%0A     */%0A
     private 
@@ -1117,23 +1117,15 @@
  /**
- rotateVector -
+%0A     *
  Rot
@@ -1189,20 +1189,16 @@
 mponent 
-    
 X compon
@@ -1240,20 +1240,16 @@
 mponent 
-    
 Y compon
@@ -1286,16 +1286,21 @@
 m angle 
+     
 Angle by
@@ -1778,39 +1778,31 @@
   %7D%0A%0A    /**
- mecanumDrive -
+%0A     *
  Adjust moto
@@ -1945,476 +1945,21 @@
     
-// Ignore tiny inadvertent joystick rotations%0A        if (Math.abs(rotation) %3C= TWIST_THRESHOLD) %7B%0A             rotation = 0.0;%0A        %7D%0A%0A        // Negate y for the joystick.%0A        ycomponent = -ycomponent;%0A%0A        double%5B%5D wheelSpeeds = new double%5B4%5D;%0A        wheelSpeeds%5B0%5D = xcomponent + ycomponent + rotation;%0A        wheelSpeeds%5B1%5D = -xcomponent + ycomponent - rotation;%0A        wheelSpeeds%5B2%5D = -xcomponent + ycomponent + rotation;%0A        wheelSpeeds%5B3%5D = 
+mecanumDrive(
 xcom
@@ -1956,34 +1956,33 @@
 Drive(xcomponent
- +
+,
  ycomponent - ro
@@ -1968,34 +1968,33 @@
 nent, ycomponent
- -
+,
  rotation;%0A%0A    
@@ -1990,228 +1990,35 @@
 tion
-;%0A%0A        normalize(wheelSpeeds);%0A        frontLeft.set(wheelSpeeds%5B0%5D
+, 0
 );%0A    
-  
-  frontRight.set(wheelSpeeds%5B1%5D);%0A        backLeft.set(wheelSpeeds%5B2%5D);%0A        backRight.set(wheelSpeeds%5B3%5D);%0A    %7D%0A%0A    /** mecanumDrive -
+%7D%0A%0A    /**%0A     *
  Adj
@@ -2219,16 +2219,37 @@
 ponent,%0A
+                     
         
@@ -2404,17 +2404,16 @@
         
- 
 rotation
@@ -3183,21 +3183,15 @@
  /**
- testMotors -
+%0A     *
  Tur

Delete unused code and add comments