Add descriptive comments to the Drivetrain

[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.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.Encoder;
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;

  // 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;
  private final static double OUTPUT_SPROCKET_DIAMETER = 2.0;
  private final static double WHEEL_SPROCKET_DIAMETER = 3.5;

  public final static double INCHES_PER_PULSE = (((Math.PI)
      * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION) / WHEEL_SPROCKET_DIAMETER)
      * WHEEL_DIAMETER;

  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);

    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 resetEncoders() {
    leftEncoder.reset();
    rightEncoder.reset();
  }

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

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

  public double getSpeed() {
    return (getLeftSpeed() + getRightSpeed()) / 2.0;
  }

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

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

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

  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);
  }

}