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     frontLeft.set(left);
  48     rearLeft.set(left);
  49
  50     frontRight.set(-right);
  51     rearRight.set(-right);
  52   }
  53
  54   public void joystickDrive(final double thrust, final double twist) {
  55     robotDrive.arcadeDrive(thrust, twist, true);
  56   }
  57
  58   public void stop() {
  59     setMotorValues(0, 0);
  60   }
  61
  62   public double getFrontLeftMotorVal() {
  63     return frontLeft.get();
  64   }
  65
  66   public double getFrontRightMotorVal() {
  67     return frontRight.get();
  68   }
  69
  70   public double getRearLeftMotorVal() {
  71     return frontLeft.get();
  72   }
  73
  74   public double getRearRightMotorVal() {
  75     return frontLeft.get();
  76   }
  77
  78   public CANTalon getFrontLeft() {
  79     return frontLeft;
  80   }
  81
  82   public CANTalon getFrontRight() {
  83     return frontRight;
  84   }
  85
  86   public CANTalon getRearLeft() {
  87     return rearLeft;
  88   }
  89
  90   public CANTalon getRearRight() {
  91     return rearRight;
  92   }
  93
  94   // ENCODER METHODS
  95
  96   public double getLeftEncoder() {
  97     return leftEncoder.getDistance();
  98   }
  99
 100   public double getRightEncoder() {
 101     return rightEncoder.getDistance();
 102   }
 103
 104   public double getAvgEncoderDistance() {
 105     return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
 106   }
 107
 108   public void resetEncoders() {
 109     leftEncoder.reset();
 110     rightEncoder.reset();
 111   }
 112
 113   public double getLeftSpeed() {
 114     return leftEncoder.getRate();
 115   }
 116
 117   public double getRightSpeed() {
 118     return rightEncoder.getRate();
 119   }
 120
 121   public double getSpeed() {
 122     return (getLeftSpeed() + getRightSpeed()) / 2.0;
 123   }
 124
 125   @Override
 126   protected void initDefaultCommand() {
 127     setDefaultCommand(new JoystickDrive());
 128   }
 129
 130 }