change getCurrentSetPoint in changeSpeed method to getSpeed

[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 edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.CANTalon.TalonControlMode;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.command.Subsystem;

public class DriveTrain extends Subsystem {
  // Drivetrain related objects
  private Encoder leftEncoder, rightEncoder;
  private CANTalon frontLeft, frontRight, rearLeft, rearRight;
  private PIDController frontLeftC, frontRightC, rearLeftC, rearRightC;
  // Drivetrain specific constants that relate to the inches per pulse value for
  // the encoders
  private final static double WHEEL_DIAMETER = 6.0; // in inches
  private final static double PULSES_PER_ROTATION = 256; // in pulses
  private final static double OUTPUT_SPROCKET_DIAMETER = 2.0; // in inches
  private final static double WHEEL_SPROCKET_DIAMETER = 3.5; // in inches
  public final static double INCHES_PER_PULSE = (((Math.PI)
      * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION) / WHEEL_SPROCKET_DIAMETER)
      * WHEEL_DIAMETER;
  // Drivetrain specific constants that relate to the PID controllers
  private final static double Kp = 1.0, Ki = 0.0, Kd = 0.0;

  public DriveTrain() {
    frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
    frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
    rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT);
    rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
    this.configureTalons();

    frontLeftC = new PIDController(Kp, Ki, Kd, frontLeft, frontLeft);
    frontRightC = new PIDController(Kp, Ki, Kd, frontRight, frontRight);
    rearLeftC = new PIDController(Kp, Ki, Kd, frontLeft, rearLeft);
    rearRightC = new PIDController(Kp, Ki, Kd, frontRight, rearRight);
    this.enablePIDControllers();

    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(INCHES_PER_PULSE);
    rightEncoder.setDistancePerPulse(INCHES_PER_PULSE);
  }

  @Override
  protected void initDefaultCommand() {
  }

  public void configureTalons() {
    frontLeft.changeControlMode(TalonControlMode.Speed);
    frontRight.changeControlMode(TalonControlMode.Speed);
    rearLeft.changeControlMode(TalonControlMode.Speed);
    rearRight.changeControlMode(TalonControlMode.Speed);

    frontLeft.configEncoderCodesPerRev(256);
    frontRight.configEncoderCodesPerRev(256);
    rearLeft.configEncoderCodesPerRev(256);
    rearRight.configEncoderCodesPerRev(256);

    frontLeft.enableControl();
    frontRight.enableControl();
    rearLeft.enableControl();
    rearRight.enableControl();
  }

  public void enablePIDControllers() {
    frontLeftC.enable();
    frontRightC.enable();
    rearLeftC.enable();
    rearRightC.enable();
  }

  public void drive(double left, double right) {
    frontLeftC.setSetpoint(left);
    rearLeftC.setSetpoint(left);
    frontRightC.setSetpoint(right);
    rearRightC.setSetpoint(right);
  }

  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
  }

  public double getDistance() {
    return (getRightDistance() + getLeftDistance()) / 2.0; // in inches
  }

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

  public void setMotorSpeeds(double leftSpeed, double rightSpeed) {
    // speed passed to right motor is negative because right motor rotates in
    // opposite direction
    this.frontLeft.set(leftSpeed);
    this.frontRight.set(-rightSpeed);
    this.rearLeft.set(leftSpeed);
    this.rearRight.set(-rightSpeed);
  }
}