af4aa14ed7e2ed6e454c2c5eb5269e8f78481d96
[3501/stronghold-2016] / 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.FirebotGyro;
5 import org.usfirst.frc.team3501.robot.Lidar;
6
7 import edu.wpi.first.wpilibj.AnalogInput;
8 import edu.wpi.first.wpilibj.CANTalon;
9 import edu.wpi.first.wpilibj.CounterBase.EncodingType;
10 import edu.wpi.first.wpilibj.Encoder;
11 import edu.wpi.first.wpilibj.I2C;
12 import edu.wpi.first.wpilibj.PIDController;
13 import edu.wpi.first.wpilibj.command.Subsystem;
14
15 public class DriveTrain extends Subsystem {
16 // Drivetrain related objects
17 private Encoder leftEncoder, rightEncoder;
18 public static Lidar leftLidar;
19 public static Lidar rightLidar;
20 private CANTalon frontLeft, frontRight, rearLeft, rearRight;
21 private PIDController frontLeftC, frontRightC, rearLeftC, rearRightC;
22 // Drivetrain specific constants that relate to the inches per pulse value for
23 // the encoders
24 private final static double WHEEL_DIAMETER = 6.0; // in inches
25 private final static double PULSES_PER_ROTATION = 256; // in pulses
26 private final static double OUTPUT_SPROCKET_DIAMETER = 2.0; // in inches
27 private final static double WHEEL_SPROCKET_DIAMETER = 3.5; // in inches
28 public final static double INCHES_PER_PULSE = (((Math.PI)
29 * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION)
30 / WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;
31
32 // Drivetrain specific constants that relate to the PID controllers
33 private final static double Kp = 1.0, Ki = 0.0,
34 Kd = 0.0 * (OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION)
35 / (WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;
36
37 public AnalogInput channel;
38
39 // Gyro stuff
40 private final static double NANOSECONDS_PER_SECOND = 1000000000;
41 short rawValue;
42 public FirebotGyro gyro;
43
44 double initialSpeedNanoseconds;
45 double finalSpeedNanoseconds;
46 double initialSpeedSeconds;
47 double finalSpeedSeconds;
48 double deltaSpeed;
49 double degrees;
50
51 public DriveTrain() {
52 frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
53 frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
54 rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT);
55 rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
56
57 leftLidar = new Lidar(I2C.Port.kOnboard);
58 rightLidar = new Lidar(I2C.Port.kOnboard); // TODO: find port for second
59 // lidar
60 leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
61 Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
62 rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
63 Constants.DriveTrain.ENCODER_RIGHT_B, false, EncodingType.k4X);
64 leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
65 rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
66 leftEncoder.setDistancePerPulse(INCHES_PER_PULSE);
67 rightEncoder.setDistancePerPulse(INCHES_PER_PULSE);
68
69 gyro = new FirebotGyro(I2C.Port.kOnboard, (byte) 0x68);
70 gyro.initialize();
71 }
72
73 @Override
74 protected void initDefaultCommand() {
75 }
76
77 public void resetEncoders() {
78 leftEncoder.reset();
79 rightEncoder.reset();
80 }
81
82 public double getLeftLidarDistance() {
83 return leftLidar.pidGet();
84 }
85
86 public double getRightLidarDistance() {
87 return rightLidar.pidGet();
88 }
89
90 public double getRightSpeed() {
91 return rightEncoder.getRate(); // in inches per second
92 }
93
94 public double getLeftSpeed() {
95 return leftEncoder.getRate(); // in inches per second
96 }
97
98 public double getSpeed() {
99 return (getLeftSpeed() + getRightSpeed()) / 2.0; // in inches per second
100 }
101
102 public double getRightDistance() {
103 return rightEncoder.getDistance(); // in inches
104 }
105
106 public double getLeftDistance() {
107 return leftEncoder.getDistance(); // in inches
108 }
109
110 public double getDistance() {
111 return (getRightDistance() + getLeftDistance()) / 2.0; // in inches
112 }
113
114 public void stop() {
115 setMotorSpeeds(0, 0);
116 }
117
118 public void setMotorSpeeds(double leftSpeed, double rightSpeed) {
119 // speed passed to right motor is negative because right motor rotates in
120 // opposite direction
121 this.frontLeft.set(leftSpeed);
122 this.frontRight.set(-rightSpeed);
123 this.rearLeft.set(leftSpeed);
124 this.rearRight.set(-rightSpeed);
125 }
126 }