[pimathbrainiac]

I redid the Drivetrain code to normalize the motor values. How is the code? (Just ctrl-A ctrl-V'd it)

// RobotBuilder Version: 1.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.
package org.usfirst.frc1127.DevBot2014_2015.subsystems;
import org.usfirst.frc1127.DevBot2014_2015.RobotMap;
import org.usfirst.frc1127.DevBot2014_2015.commands.*;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.command.Subsystem;
/**
 *
 */
public class Drivetrain extends Subsystem
{
    //constants
    final double DISTANCE_X = 1.0;
    final double DISTANCE_Y = 1.0;
    
    //only needed for field-centric
    final double PI = 3.1415926;
    
    //wheel velocity values | positive x values are right | positive y values are forwards
    private double[] motorValues = {0.0, 0.0, 0.0, 0.0};
    
    //rotation is cw
    private double rotationX;
    private double rotationY;
    
    //field-centric stuff
    private double curr_gyro_angle_degrees = 0.0;
    private double curr_gyro_angle_radians = 0.0;
    private double cos;
    private double sin;
    private double temp;
    
    //for the for loop
    private int i;
    double maxValue = 1.0;
    
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    CANJaguar cANJaguar1 = RobotMap.drivetrainCANJaguar1;
    CANJaguar cANJaguar2 = RobotMap.drivetrainCANJaguar2;
    RobotDrive robotDrive2X = RobotMap.drivetrainRobotDrive2X;
    CANJaguar cANJaguar3 = RobotMap.drivetrainCANJaguar3;
    CANJaguar cANJaguar4 = RobotMap.drivetrainCANJaguar4;
    RobotDrive robotDrive2Y = RobotMap.drivetrainRobotDrive2Y;
    Gyro gyro1 = RobotMap.drivetrainGyro1;
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    public void initDefaultCommand()
    {
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
        setDefaultCommand(new DriveWithJoysticks());
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
	
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }
    
    public void TakeJoystickInputsAndDrive(Joystick left, Joystick right)
    {
        calculatedDrive(left.getY(), left.getX(), right.getX());
    }
    
    void calculatedDrive(double y, double x, double rotation)
    {
        //Field-centric adjustments. Comment out for robot-centric.
        curr_gyro_angle_degrees = gyro1.getAngle();
        curr_gyro_angle_radians = curr_gyro_angle_degrees * (PI / 180);
        cos = Math.cos(curr_gyro_angle_radians);
        sin = Math.sin(curr_gyro_angle_radians);
        temp = y * cos - x * sin;
        x = y * sin + x * cos;
        y = temp;
        
        //This code is needed for both robot-centric and field-centric
        rotationX = rotation * DISTANCE_X;
        rotationY = rotation * DISTANCE_Y;
        motorValues[0] = y - rotationY;
        motorValues[1] = y + rotationY;
        motorValues[2] = x - rotationX;
        motorValues[3] = x + rotationX;
        
        maxValue = 1.0;
        for(i = 0; i < 4; i++)
        {
            if(motorValues[i] > maxValue)
                maxValue = motorValues[i];
        }
        
        if(maxValue > 1.0)
            for(i = 0; i < 4; i++)
                motorValues[i] = motorValues[i] / maxValue;
        
        drive(motorValues[0], motorValues[1], motorValues[2], motorValues[3]);
    }
    
    void drive(double y1, double y2, double x1, double x2)
    {
        //left, right
        robotDrive2Y.drive(y1, y2);
        //front, back
        robotDrive2X.drive(x1, x2);
    }
    
    public void stop()
    {
        robotDrive2Y.drive(0, 0);
        robotDrive2X.drive(0, 0);
    }
}