Drivetrain/auton changes

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / subsystems / DriveTrain.java

package org.usfirst.frc.team3501.robot.subsystems;

import org.usfirst.frc.team3501.robot.Constants;
import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;

import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.PIDSubsystem;

public class DriveTrain extends PIDSubsystem {
  // Determines if the "front" of the robot has been reversed
  private boolean outputFlipped = false;

  private static double pidOutput = 0;

  private Encoder leftEncoder, rightEncoder;

  private CANTalon frontLeft, frontRight, rearLeft, rearRight;
  private RobotDrive robotDrive;

  private DoubleSolenoid leftGearPiston, rightGearPiston;

  // Drivetrain specific constants that relate to the inches per pulse value for
  // the encoders

  public DriveTrain() {
    super(Constants.DriveTrain.kp, Constants.DriveTrain.ki,
        Constants.DriveTrain.kd);

    frontLeft = new CANTalon(Constants.DriveTrain.DRIVE_FRONT_LEFT);
    frontRight = new CANTalon(Constants.DriveTrain.DRIVE_FRONT_RIGHT);
    rearLeft = new CANTalon(Constants.DriveTrain.DRIVE_REAR_LEFT);
    rearRight = new CANTalon(Constants.DriveTrain.DRIVE_REAR_RIGHT);

    robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);

    leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
        Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
    rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
        Constants.DriveTrain.ENCODER_RIGHT_B, false, EncodingType.k4X);
    leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
    rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);

    leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
    rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);

    this.disable();

    leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_SHIFT_MODULE,
        Constants.DriveTrain.LEFT_SHIFT_FORWARD,
        Constants.DriveTrain.LEFT_SHIFT_REVERSE);
    rightGearPiston = new DoubleSolenoid(
        Constants.DriveTrain.RIGHT_SHIFT_MODULE,
        Constants.DriveTrain.RIGHT_SHIFT_FORWARD,
        Constants.DriveTrain.RIGHT_SHIFT_REVERSE);
  }

  @Override
  protected void initDefaultCommand() {
    setDefaultCommand(new JoystickDrive());
  }

  // Print tne PID Output
  public void printOutput() {
    System.out.println("PIDOutput: " + pidOutput);
  }

  private double getAvgEncoderDistance() {
    return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
  }

  // Whether or not the PID Controller thinks we have reached the target
  // setpoint
  public boolean reachedTarget() {
    if (this.onTarget()) {
      this.disable();
      return true;
    } else {
      return false;
    }
  }

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

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

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

  public double getLeftSpeed() {
    return leftEncoder.getRate(); // in inches per second
  }

  public double getSpeed() {
    return (getLeftSpeed() + getRightSpeed()) / 2.0; // in inches per second
  }

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

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

  // Get error between the setpoint of PID Controller and the current state of
  // the robot
  public double getError() {
    return Math.abs(this.getSetpoint() - getAvgEncoderDistance());
  }

  public void printEncoder(int i, int n) {
    if (i % n == 0) {
      System.out.println("Left: " + this.getLeftDistance());
      System.out.println("Right: " + this.getRightDistance());

    }
  }

  /*
   * returns the PID output that is returned by the PID Controller
   */
  public double getOutput() {
    return pidOutput;
  }

  // Updates the PID constants based on which control mode is being used
  public void updatePID() {
    this.getPIDController().setPID(Constants.DriveTrain.kp,
        Constants.DriveTrain.ki, Constants.DriveTrain.kd);
  }

  public CANTalon getFrontLeft() {
    return frontLeft;
  }

  public CANTalon getFrontRight() {
    return frontRight;
  }

  public CANTalon getRearLeft() {
    return rearLeft;
  }

  public CANTalon getRearRight() {
    return rearRight;
  }

  /*
   * Method is a required method that the PID Subsystem uses to return the
   * calculated PID value to the driver
   * 
   * @param Gives the user the output from the PID algorithm that is calculated
   * internally
   * 
   * Body: Uses the output, does some filtering and drives the robot
   */
  @Override
  protected void usePIDOutput(double output) {
    double left = 0;
    double right = 0;
    double drift = this.getLeftDistance() - this.getRightDistance();
    if (Math.abs(output) > 0 && Math.abs(output) < 0.3)
      output = Math.signum(output) * 0.3;
    left = output;
    right = output + drift * Constants.DriveTrain.kp / 10;
    setMotorSpeeds(left, right);
    pidOutput = output;
  }

  @Override
  protected double returnPIDInput() {
    return sensorFeedback();
  }

  /*
   * Checks the drive mode
   * 
   * @return the current state of the robot in each state Average distance from
   * both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE
   */
  private double sensorFeedback() {
    return getAvgEncoderDistance();
  }

  public void joystickDrive(double left, double right) {
    int type = Constants.DriveTrain.DRIVE_TYPE;

    // Handle flipping of the "front" of the robot
    double k = (isFlipped() ? -1 : 1);

    robotDrive.tankDrive(-left, -right);

    // if (type == Constants.DriveTrain.TANK) {
    // // TODO Test this for negation and for voltage vs. [-1,1] outputs
    // double leftSpeed = (-frontLeft.get() + -rearLeft.get()) / 2;
    // double rightSpeed = (-frontRight.get() + -rearRight.get()) / 2;
    // left = ((Constants.DriveTrain.kADJUST - 1) * leftSpeed + left)
    // / Constants.DriveTrain.kADJUST;
    // right = ((Constants.DriveTrain.kADJUST - 1) * rightSpeed + right)
    // / Constants.DriveTrain.kADJUST;
    // robotDrive.tankDrive(-left * k, -right * k);
    // } else if (type == Constants.DriveTrain.ARCADE) {
    // double speed = (-frontLeft.get() + -rearLeft.get() + -frontRight.get() +
    // -rearRight
    // .get()) / 2;
    // left = ((Constants.DriveTrain.kADJUST - 1) * speed + left)
    // / Constants.DriveTrain.kADJUST;
    // robotDrive.arcadeDrive(left * k, right);
    // }
  }

  public void setMotorSpeeds(double left, double right) {
    double k = (isFlipped() ? -1 : 1);
    robotDrive.tankDrive(-left * k, -right * k);
  }

  /**
   * @return a value that is the current setpoint for the piston (kReverse or
   *         kForward)
   */
  public Value getGearPistonValue() {
    return leftGearPiston.get(); // Pistons should always be in the same state
  }

  /**
   * Changes the ball shift gear assembly to high
   */
  public void setHighGear() {
    changeGear(Constants.DriveTrain.HIGH_GEAR);
  }

  /**
   * Changes the ball shift gear assembly to low
   */
  public void setLowGear() {
    changeGear(Constants.DriveTrain.LOW_GEAR);
  }

  /**
   * Changes the gear to a DoubleSolenoid.Value
   */
  public void changeGear(DoubleSolenoid.Value gear) {
    leftGearPiston.set(gear);
    rightGearPiston.set(gear);
  }

  /**
   * Switches drivetrain gears from high to low or low to high
   */
  public void switchGear() {
    Value currentValue = getGearPistonValue();
    Value setValue = (currentValue == Constants.DriveTrain.HIGH_GEAR) ? Constants.DriveTrain.LOW_GEAR
        : Constants.DriveTrain.HIGH_GEAR;
    changeGear(setValue);
  }

  /**
   * Toggle whether the motor outputs are flipped, effectively switching which
   * side of the robot is the front.
   */
  public void toggleFlipped() {
    outputFlipped = !outputFlipped;
  }

  public boolean isFlipped() {
    return outputFlipped;
  }

}