add method to calculate angle to turn after passing through defense to shoot

[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.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.command.Subsystem;

public class DriveTrain extends Subsystem {
  // Drivetrain related objects
  private Encoder leftEncoder, rightEncoder;
  public static Lidar leftLidar;
  public static Lidar rightLidar;
  private CANTalon frontLeft, frontRight, rearLeft, rearRight;

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

    leftLidar = new Lidar(I2C.Port.kOnboard);
    rightLidar = new Lidar(I2C.Port.kOnboard); // TODO: find port for second
                                               // lidar
    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);
  }

  @Override
  protected void initDefaultCommand() {
  }

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

  public double getLeftLidarDistance() {
    return leftLidar.pidGet();
  }

  public double getRightLidarDistance() {
    return rightLidar.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);
  }
}