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 import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
   5
   6 import com.ctre.CANTalon;
   7
   8 import edu.wpi.first.wpilibj.Encoder;
   9 import edu.wpi.first.wpilibj.RobotDrive;
  10 import edu.wpi.first.wpilibj.command.Subsystem;
  11
  12 public class DriveTrain extends Subsystem {
  13   private static DriveTrain driveTrain;
  14   private final CANTalon frontLeft, frontRight, rearLeft, rearRight;
  15   private final RobotDrive robotDrive;
  16   private final Encoder leftEncoder, rightEncoder;
  17
  18   private DriveTrain() {
  19     // MOTOR CONTROLLERS
  20     frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
  21     frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
  22     rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT);
  23     rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
  24
  25     // ENCODERS
  26     leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
  27         Constants.DriveTrain.ENCODER_LEFT_B);
  28     rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
  29         Constants.DriveTrain.ENCODER_RIGHT_B);
  30
  31     leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  32     rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  33
  34     // ROBOT DRIVE
  35     robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
  36   }
  37
  38   public static DriveTrain getDriveTrain() {
  39     if (driveTrain == null) {
  40       driveTrain = new DriveTrain();
  41     }
  42     return driveTrain;
  43   }
  44
  45   // DRIVE METHODS
  46   public void setMotorValues(final double left, final double right) {
  47     robotDrive.tankDrive(left, right);
  48   }
  49
  50   public void joystickDrive(final double thrust, final double twist) {
  51     robotDrive.arcadeDrive(thrust, twist);
  52   }
  53
  54   public void stop() {
  55     setMotorValues(0, 0);
  56   }
  57
  58   public double getFrontLeftMotorVal() {
  59     return frontLeft.get();
  60   }
  61
  62   public double getFrontRightMotorVal() {
  63     return frontRight.get();
  64   }
  65
  66   public double getRearLeftMotorVal() {
  67     return frontLeft.get();
  68   }
  69
  70   public double getRearRightMotorVal() {
  71     return frontLeft.get();
  72   }
  73
  74   public CANTalon getFrontLeft() {
  75     return frontLeft;
  76   }
  77
  78   public CANTalon getFrontRight() {
  79     return frontRight;
  80   }
  81
  82   public CANTalon getRearLeft() {
  83     return rearLeft;
  84   }
  85
  86   public CANTalon getRearRight() {
  87     return rearRight;
  88   }
  89
  90   // ENCODER METHODS
  91
  92   public double getLeftEncoder() {
  93     return leftEncoder.getDistance();
  94   }
  95
  96   public double getRightEncoder() {
  97     return rightEncoder.getDistance();
  98   }
  99
 100   public double getAvgEncoderDistance() {
 101     return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
 102   }
 103
 104   public void resetEncoders() {
 105     leftEncoder.reset();
 106     rightEncoder.reset();
 107   }
 108
 109   public double getLeftSpeed() {
 110     return leftEncoder.getRate();
 111   }
 112
 113   public double getRightSpeed() {
 114     return rightEncoder.getRate();
 115   }
 116
 117   public double getSpeed() {
 118     return (getLeftSpeed() + getRightSpeed()) / 2.0;
 119   }
 120
 121   @Override
 122   protected void initDefaultCommand() {
 123     setDefaultCommand(new JoystickDrive());
 124   }
 125
 126 }