package org.usfirst.frc4959.StaleyRobotics2014.subsystems;

import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.command.Subsystem;
import org.usfirst.frc4959.StaleyRobotics2014.RobotMap;
import org.usfirst.frc4959.StaleyRobotics2014.commands.DriveWithJoysticks;

/**
 * Lucas Wyland & Ian Collins
 */

public class DriveTrain extends Subsystem
    {

    SpeedController rightVictor = RobotMap.driveTrainRightVictor;
    SpeedController leftVictor = RobotMap.driveTrainLeftVictor;
    RobotDrive robotDrive = RobotMap.robotDriveTrain;
    Gyro gyro = RobotMap.gyro;
    
    private int sensitivityLevel = 5;

    public void initDefaultCommand()
        {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
        setDefaultCommand(new DriveWithJoysticks());
        }

    public void falconDrive(double left, double right)
        {
        double forwardSpeed = changeSpeed(left, sensitivity(sensitivityLevel));
        double rotateSpeed = changeSpeed(right, sensitivity(sensitivityLevel));
        
        robotDrive.arcadeDrive(forwardSpeed, rotateSpeed);
        }
    
    public void stop()
        {
        robotDrive.drive(0, 0);
        }

    public void decreaseSensitivity()
        {
        if (sensitivityLevel > 1) 
            {
            sensitivityLevel--;
            }
        }

    public void increaseSensitivity()
        {
        if (sensitivityLevel < 5)
            {
            sensitivityLevel++;
            }
        }
    
    public double sensitivity(int sensitivityLevel)
        {
        
        double sensitivity;

        switch (sensitivityLevel) {

            case 1:
                sensitivity = 0.6;
                break;
            case 2:
                sensitivity = 0.7;
                break;
            case 3:
                sensitivity = 0.8;
                break;
            case 4:
                sensitivity = 0.9;
                break;
            case 5:
                sensitivity = 1.0;
                break;
            default:
                sensitivity = 1.0;
                break;
        }
        return (sensitivity);
        }

    public static double changeSpeed(double input, double sensitivity)
        {
        return (sensitivity * input);
        }
    }

@@ -1003,28 +1003,25 @@
  double 
-forwardSpeed
+moveValue
  = chang
@@ -1089,65 +1089,47 @@
 otat
-eSpeed = changeSpeed(right, sensitivity(sensitivityLevel)
+ionValue = gyroRotationAdjustment(right
 );%0A 
@@ -1171,33 +1171,32 @@
 ive(
-forwardSpeed, rotateSpeed
+moveValue, rotationValue
 );%0A 
@@ -1429,32 +1429,275 @@
      %7D%0A        %7D
+%0A    %0A    public double gyroRotationAdjustment(double right)%0A        %7B%0A        if (right %3C 0.15 %7C%7C right %3E -0.15)%0A            %7B%0A            return gyro.getAngle() * -0.03;%0A            %7D else %7B%0A            return right; %0A            %7D%0A        %7D
 %0A%0A    public voi

Move Forward With Gyro