Add PID subsystem

[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.Lidar;

import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.I2C;
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;
  public static Lidar lidar;
  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 * (OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION)
          / (WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;

  public AnalogInput channel;

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

    lidar = new Lidar(I2C.Port.kOnboard);
    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(INCHES_PER_PULSE);
    rightEncoder.setDistancePerPulse(INCHES_PER_PULSE);

  }

  @Override
  protected void initDefaultCommand() {
  }

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

  public double getLidarDistance() {
    return lidar.pidGet();
  }

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