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