clean up DriveForTime

[3501/2015-FRC-Spark] / src / org / usfirst / frc3501 / RiceCatRobot / subsystems / DriveTrain.java

package org.usfirst.frc3501.RiceCatRobot.subsystems;

import org.usfirst.frc3501.RiceCatRobot.RobotMap;

import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.command.Subsystem;

public class DriveTrain extends Subsystem {
  public static final double MOTOR_MAX_VAL = 1;
  public static final double MOTOR_MIN_VAL = -1;

  private CANJaguar frontLeft, frontRight, rearLeft, rearRight;
  private Encoder leftEncoder, rightEncoder;

  public DriveTrain() {
    frontLeft = new CANJaguar(RobotMap.DRIVE_FRONT_LEFT);
    frontRight = new CANJaguar(RobotMap.DRIVE_FRONT_RIGHT);
    rearLeft = new CANJaguar(RobotMap.DRIVE_REAR_LEFT);
    rearRight = new CANJaguar(RobotMap.DRIVE_REAR_RIGHT);
    leftEncoder = new Encoder(RobotMap.DRIVE_LEFT_A, RobotMap.DRIVE_LEFT_B,
        false, EncodingType.k4X);
    rightEncoder = new Encoder(RobotMap.DRIVE_RIGHT_A, RobotMap.DRIVE_RIGHT_B,
        false, EncodingType.k4X);
    leftEncoder.setDistancePerPulse(RobotMap.DISTANCE_PER_PULSE);
    rightEncoder.setDistancePerPulse(RobotMap.DISTANCE_PER_PULSE);
  }

  public void resetEncoders() {
    leftEncoder.reset();
    rightEncoder.reset();
  }

  public double getRightSpeed() {
    // Returns in per second
    return rightEncoder.getRate();
  }

  public double getLeftSpeed() {
    return leftEncoder.getRate();
  }

  public double getRightDistance() {
    // Returns distance in in
    return rightEncoder.getDistance();
  }

  public double getLeftDistance() {
    // Returns distance in in
    return leftEncoder.getDistance();
  }

  public void stop() {
    setMotorSpeeds(0, 0);
  }

  public void setMotorSpeeds(double leftSpeed, double rightSpeed) {
    if (Math.abs(leftSpeed) < RobotMap.DRIVE_DEAD_ZONE) {
      leftSpeed = 0;
    }
    if (Math.abs(rightSpeed) < RobotMap.DRIVE_DEAD_ZONE) {
      rightSpeed = 0;
    }
    this.frontLeft.set(leftSpeed);
    this.frontRight.set(-rightSpeed);
    this.rearLeft.set(leftSpeed);
    this.rearRight.set(-rightSpeed);
  }

  public void arcadeDrive(double yVal, double twist) {
    if (yVal < 0 && Math.abs(yVal) < 0.1125) {
      yVal = 0;
    } else if (yVal > 0 && Math.abs(yVal) < 0.25) {
      yVal = 0;
    } else if (yVal > 0) {
      yVal -= 0.25;
    } else if (yVal < 0) {
      yVal += 0.15;
    }
    if (Math.abs(twist) < RobotMap.DRIVE_DEAD_ZONE) {
      twist = 0;
    }

    double leftMotorSpeed;
    double rightMotorSpeed;
    // adjust the sensitivity (yVal+rootof (yval)) / 2
    yVal = (yVal + Math.signum(yVal) * Math.sqrt(Math.abs(yVal))) / 2;
    // adjust the sensitivity (twist+rootof (twist)) / 2
    twist = (twist + Math.signum(twist) * Math.sqrt(Math.abs(twist))) / 2;
    if (yVal > 0) {
      if (twist > 0) {
        leftMotorSpeed = yVal - twist;
        rightMotorSpeed = Math.max(yVal, twist);
      } else {
        leftMotorSpeed = Math.max(yVal, -twist);
        rightMotorSpeed = yVal + twist;
      }
    } else {
      if (twist > 0.0) {
        leftMotorSpeed = -Math.max(-yVal, twist);
        rightMotorSpeed = yVal + twist;
      } else {
        leftMotorSpeed = yVal - twist;
        rightMotorSpeed = -Math.max(-yVal, -twist);
      }
    }
    setMotorSpeeds(leftMotorSpeed, rightMotorSpeed);
  }

  @Override
  protected void initDefaultCommand() {
  }
}