Commit | Line | Data |
---|---|---|
a0c3ca74 KZ |
1 | package org.usfirst.frc.team3501.robot.subsystems; |
2 | ||
3 | import org.usfirst.frc.team3501.robot.Constants; | |
06fda04b | 4 | import org.usfirst.frc.team3501.robot.Lidar; |
a0c3ca74 KZ |
5 | |
6 | import edu.wpi.first.wpilibj.CANTalon; | |
2c52cb6e | 7 | import edu.wpi.first.wpilibj.CounterBase.EncodingType; |
9b10474d | 8 | import edu.wpi.first.wpilibj.DoubleSolenoid; |
2c52cb6e | 9 | import edu.wpi.first.wpilibj.Encoder; |
0b63e0b2 | 10 | import edu.wpi.first.wpilibj.command.PIDSubsystem; |
a0c3ca74 | 11 | |
27fac8ed | 12 | /*** |
06fda04b E |
13 | * The Shooter consists of a platform and wheel, each controlled by separate |
14 | * motors. The piston controlling the platform pushes the ball onto the wheel. | |
15 | * The wheel is controlled by a motor, which is running before the ball is | |
16 | * pushed onto the wheel. The spinning wheel propels the ball. | |
27fac8ed YA |
17 | * |
18 | * @author superuser | |
19 | * | |
08623b33 CZ |
20 | * setPoint (in the context of shooter) is the target value we want the |
21 | * PID controller to adjust the system to, for example, if we want the | |
22 | * shooter wheels to run at 10in/s | |
23 | * | |
27fac8ed YA |
24 | */ |
25 | ||
0b63e0b2 | 26 | public class Shooter extends PIDSubsystem { |
08623b33 | 27 | // Shooter PID Proportional Constants P, I, and D |
fd085229 | 28 | private static double sp = 0.015, si = 0, sd = 0; |
08623b33 CZ |
29 | |
30 | // PID Controller tolerance for the error | |
0b63e0b2 | 31 | private static double encoderTolerance = 5.0; |
fd085229 | 32 | private static double pidOutput = 5.0; |
0b63e0b2 | 33 | |
a0c3ca74 | 34 | private CANTalon shooter; |
071ab315 | 35 | private DoubleSolenoid hood, punch; |
2c52cb6e | 36 | private Encoder encoder; |
06fda04b | 37 | private Lidar lidar; |
a0c3ca74 KZ |
38 | |
39 | public Shooter() { | |
fd085229 | 40 | super(sp, si, sd); |
0b63e0b2 | 41 | |
a0c3ca74 | 42 | shooter = new CANTalon(Constants.Shooter.PORT); |
071ab315 KZ |
43 | hood = new DoubleSolenoid(Constants.Shooter.HOOD_FORWARD, |
44 | Constants.Shooter.HOOD_REVERSE); | |
45 | punch = new DoubleSolenoid(Constants.Shooter.PUNCH_FORWARD, | |
46 | Constants.Shooter.PUNCH_REVERSE); | |
2c52cb6e K |
47 | |
48 | encoder = new Encoder(Constants.Shooter.ENCODER_PORT_A, | |
49 | Constants.Shooter.ENCODER_PORT_B, false, EncodingType.k4X); | |
06fda04b | 50 | |
77c791b9 | 51 | lidar = new Lidar(Constants.Shooter.LIDAR_I2C_PORT); |
a0c3ca74 KZ |
52 | } |
53 | ||
64055177 | 54 | /*** |
f08a2eef YA |
55 | * This method checks to see if the ball has successfully passed through the |
56 | * intake rollers and is inside. | |
57 | * | |
58 | * @return whether the presence of the ball is true or false and returns the | |
59 | * state of the condition (true or false). | |
60 | */ | |
61 | ||
62 | public boolean isBallInside() { | |
63 | return true; | |
64 | } | |
65 | ||
a0c3ca74 | 66 | public void setSpeed(double speed) { |
fd085229 CZ |
67 | setShooterPID(); |
68 | setSetpoint(speed); | |
a0c3ca74 KZ |
69 | } |
70 | ||
4b29730e | 71 | // This getDistance() will return the distance using the lidar from the |
005821c6 E |
72 | // desired target during match. This distance is returned in units of |
73 | // CENTIMETERS. | |
4b29730e | 74 | public double getDistance() { |
005821c6 | 75 | return lidar.getDistance(); |
4b29730e E |
76 | } |
77 | ||
a0c3ca74 KZ |
78 | public void stop() { |
79 | this.setSpeed(0.0); | |
80 | } | |
81 | ||
2c52cb6e | 82 | public double getSpeed() { |
08623b33 | 83 | return encoder.getRate(); // in/sec |
2c52cb6e K |
84 | } |
85 | ||
a0c3ca74 | 86 | // Use negative # for decrement. Positive for increment. |
27fac8ed | 87 | |
a0c3ca74 | 88 | public void changeSpeed(double change) { |
92d4c21a | 89 | double newSpeed = getSpeed() + change; |
64055177 | 90 | setSpeed(newSpeed); |
a0c3ca74 KZ |
91 | } |
92 | ||
9b10474d | 93 | // Punch Commands |
4b94da2f | 94 | public void punch() { |
9b10474d GK |
95 | punch.set(Constants.Shooter.punch); |
96 | } | |
97 | ||
c8c4e6d9 | 98 | public void retractPunch() { |
9b10474d GK |
99 | punch.set(Constants.Shooter.retract); |
100 | } | |
101 | ||
071ab315 KZ |
102 | public boolean isHoodOpen() { |
103 | return hood.get() == Constants.Shooter.open; | |
104 | } | |
105 | ||
106 | public void openHood() { | |
107 | hood.set(Constants.Shooter.open); | |
108 | } | |
109 | ||
110 | public void closeHood() { | |
111 | hood.set(Constants.Shooter.closed); | |
112 | } | |
113 | ||
a0c3ca74 KZ |
114 | @Override |
115 | protected void initDefaultCommand() { | |
116 | } | |
0b63e0b2 CZ |
117 | |
118 | @Override | |
119 | protected double returnPIDInput() { | |
fd085229 | 120 | return sensorFeedback(); |
0b63e0b2 CZ |
121 | } |
122 | ||
08623b33 CZ |
123 | /* |
124 | * Method is a required method that the PID Subsystem uses to return the | |
125 | * calculated PID value to the driver | |
126 | * | |
127 | * @param Gives the user the output from the PID algorithm that is calculated | |
128 | * internally | |
129 | */ | |
0b63e0b2 CZ |
130 | @Override |
131 | protected void usePIDOutput(double output) { | |
fd085229 CZ |
132 | shooter.set(output); |
133 | pidOutput = output; | |
134 | } | |
0b63e0b2 | 135 | |
fd085229 CZ |
136 | private double sensorFeedback() { |
137 | return this.getSpeed(); | |
0b63e0b2 CZ |
138 | } |
139 | ||
fd085229 CZ |
140 | public void setShooterPID() { |
141 | this.updatePID(); | |
0b63e0b2 CZ |
142 | this.setAbsoluteTolerance(encoderTolerance); |
143 | this.setOutputRange(-1.0, 1.0); | |
fd085229 | 144 | this.setInputRange(-500.0, 500.0); |
0b63e0b2 CZ |
145 | this.enable(); |
146 | } | |
fd085229 | 147 | |
08623b33 | 148 | // Updates the PID constants |
fd085229 CZ |
149 | public void updatePID() { |
150 | this.getPIDController().setPID(sp, si, sd); | |
151 | } | |
a0c3ca74 | 152 | } |