challenge-bot.com Git - 3501/2017steamworks/blob - src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java

   1 package org.usfirst.frc.team3501.robot.subsystems;
   2
   3 import org.usfirst.frc.team3501.robot.Constants;
   4
   5 import com.ctre.CANTalon;
   6
   7 import edu.wpi.first.wpilibj.Encoder;
   8 import edu.wpi.first.wpilibj.RobotDrive;
   9 import edu.wpi.first.wpilibj.command.Subsystem;
  10
  11 public class DriveTrain extends Subsystem {
  12     private static DriveTrain driveTrain;
  13     private CANTalon frontLeft, frontRight, rearLeft, rearRight;
  14     private RobotDrive robotDrive;
  15     private Encoder leftEncoder, rightEncoder;
  16
  17     private DriveTrain() {
  18         // MOTOR CONTROLLERS
  19         frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
  20         frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
  21         rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT);
  22         rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
  23
  24         // ENCODERS
  25         leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
  26                 Constants.DriveTrain.ENCODER_LEFT_B);
  27         rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
  28                 Constants.DriveTrain.ENCODER_RIGHT_B);
  29
  30         leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  31         rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  32
  33         // ROBOT DRIVE
  34         robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
  35     }
  36
  37     public static DriveTrain getDriveTrain() {
  38         if (driveTrain == null)
  39             driveTrain = new DriveTrain();
  40         return driveTrain;
  41     }
  42
  43     public void setMotorSpeeds(double left, double right) {
  44         robotDrive.tankDrive(left, right);
  45     }
  46
  47     // ENCODER METHODS
  48
  49     public double getLeftEncoder() {
  50         return leftEncoder.getDistance();
  51     }
  52
  53     public double getRightEncoder() {
  54         return rightEncoder.getDistance();
  55     }
  56
  57     public double getAvgEncoderDistance() {
  58         return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
  59     }
  60
  61     public void resetEncoders() {
  62         leftEncoder.reset();
  63         rightEncoder.reset();
  64     }
  65
  66     public double getLeftSpeed() {
  67         return leftEncoder.getRate();
  68     }
  69
  70     public double getRightSpeed() {
  71         return rightEncoder.getRate();
  72     }
  73
  74     public double getSpeed() {
  75         return (getLeftSpeed() + getRightSpeed()) / 2.0;
  76     }
  77
  78     @Override
  79     protected void initDefaultCommand() {
  80     }
  81
  82 }