Source code

001/**
002 * ********************** PROJECT RON ************************
003 */
004/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */
005/* Use of this source code is governed by an MIT-style license */
006/* that can be found in the repository LICENSE file.           */
007/**
008 * ***********************************************************
009 */
010package com.stuypulse.robot.constants;
011
012import static edu.wpi.first.units.Units.*;
013import edu.wpi.first.math.VecBuilder;
014import edu.wpi.first.math.Vector;
015import edu.wpi.first.math.geometry.Pose2d;
016import edu.wpi.first.math.geometry.Rotation2d;
017import edu.wpi.first.math.numbers.N3;
018import edu.wpi.first.math.util.Units;
019import edu.wpi.first.networktables.BooleanSubscriber;
020import edu.wpi.first.networktables.DoubleSubscriber;
021import edu.wpi.first.units.*;
022import edu.wpi.first.units.measure.*;
023import edu.wpi.first.wpilibj.LEDPattern;
024import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
025import edu.wpi.first.wpilibj.util.Color;
026import com.ctre.phoenix6.CANBus;
027import dev.doglog.DogLog;
028
029import com.pathplanner.lib.path.PathConstraints;
030
031/*-
032 * File containing tunable settings for every subsystem on the robot.
033 *
034 * We use DogLog's tunables in order to have tunable
035 * values that we can edit on whatever dashboard we
036 * are using.
037 */
038public interface Settings {
039
040    Time DT = Seconds.of(0.020);
041
042    boolean DEBUG_MODE = true;
043
044    CANBus CANBUS = new CANBus("rio");
045
046    public interface EnabledSubsystems {
047
048        BooleanSubscriber FEEDER = DogLog.tunable("Enabled Subsystems/Feeder", true);
049
050        BooleanSubscriber INTAKE = DogLog.tunable("Enabled Subsystems/Intake", true);
051
052        // BooleanSubscriber INTAKE_ROLLERS = DogLog.tunable("Enabled Subsystems/Intake/Rollers", true);
053
054        // BooleanSubscriber INTAKE_PIVOT = DogLog.tunable("Enabled Subsystems/Intake/Pivot", true);
055
056        BooleanSubscriber LED = DogLog.tunable("Enabled Subsystems/LED", false);
057
058        BooleanSubscriber HANDOFF = DogLog.tunable("Enabled Subsystems/Handoff", true);
059
060        BooleanSubscriber SHOOTER = DogLog.tunable("Enabled Subsystems/Shooter", true);
061
062        BooleanSubscriber VISION = DogLog.tunable("Enabled Subsystems/Vision", true);
063
064        BooleanSubscriber SWERVE = DogLog.tunable("Enabled Subsystems/Swerve", true);
065    }
066
067    public interface Vision {
068
069        // TODO: These numbers are temporary, may need testing
070        public final Vector<N3> MT1_STDEVS = VecBuilder.fill(0.5, 0.5, 1.0);
071
072        public final Vector<N3> MT2_STDEVS = VecBuilder.fill(0.7, 0.7, 694694.0);
073
074        public final Pose2d INVALID_POSITION = Pose2d.kZero;
075
076        public final double MAX_ANGULAR_VELOCITY_RAD_SEC = 2 * Math.PI;
077    }
078
079    public interface Intake {
080
081        public interface Pivot {
082
083            // state angles
084            // TODO:Get new pivot angles
085            Angle INITIAL_ANGLE = Degrees.of(-102);
086
087            Angle STOW_ANGLE = Degrees.of(-102);
088
089            Angle DEPLOY_ANGLE = Degrees.of(-22);
090
091            Angle AGITATE_UP_ANGLE = Degrees.of(-62);
092
093            Angle DIGEST_ANGLE = Degrees.of(-92);
094
095            Angle AGITATE_DOWN_ANGLE = Degrees.of(-22);
096
097            // misc
098            Angle ANGLE_TOLERANCE = Degrees.of(0.5);
099
100            Angle PUSHDOWN_THRESHOLD = Degrees.of(-30);
101
102            DoubleSubscriber PUSHDOWN_CURRENT = DogLog.tunable("Intake/Pivot/Pushdown Current Tuning Amps", 13.0);
103
104            // amps
105            Current STALL_CURRENT = Amps.of(25);
106
107            // TODO: set this up?
108            Time STALL_DEBOUNCE_SEC = Seconds.of(0.0);
109
110            Voltage HOMING_DOWN_VOLTAGE = Volts.of(3);
111
112            // sysid
113            Velocity<VoltageUnit> RAMP_RATE = Volts.of(1).per(Second);
114
115            Voltage STEP_VOLTAGE = Volts.of(1);
116
117            // sim
118            Angle MIN_ANGLE = Degrees.of(0);
119
120            Angle MAX_ANGLE = Degrees.of(-102.0);
121
122            double GEAR_RATIO = 60.0;
123
124            Distance PIVOT_ARM_LENGTH = Meters.of(0.1439822);
125
126            // mass in kg
127            MomentOfInertia MOI = KilogramSquareMeters.of(SingleJointedArmSim.estimateMOI(PIVOT_ARM_LENGTH.in(Meters), 1));
128        }
129
130        public interface Roller {
131
132            Current STALL_CURRENT = Amps.of(50);
133
134            Time STALL_DEBOUNCE_SEC = Seconds.of(0.1);
135
136            double GEAR_RATIO = 16.0 / 27.0;
137
138            MomentOfInertia J = KilogramSquareMeters.of(0.001);
139
140            double IDLE_DUTY_CYCLE = 0;
141
142            double INTAKE_DUTY_CYCLE = 1;
143
144            double OUTTAKE_DUTY_CYCLE = -1;
145        }
146    }
147
148    public interface Feeder {
149
150        Voltage REVERSE_VOLTAGE = Volts.of(-10.0); // TODO: get
151        DoubleSubscriber REVERSE_TIME_BEFORE_SHOOT = DogLog.tunable("Feeder/Seconds To Reverse Before Shooting", 0.75);
152
153        Voltage FORWARD_VOLTAGE = Volts.of(10.0);
154
155        // TODO: get from mec
156        double GEAR_RATIO = 34/14; // (34/14) : 1
157
158        MomentOfInertia J = KilogramSquareMeters.of(0.001);
159    }
160
161    public interface LED {
162
163        // TODO: Get actual length of led, along with length of individual sections
164        int LED_LENGTH = 80;
165
166        // Buffer Views {Starting Index, Ending Index}
167        int[] SHOOTER_BUFFER = { 0, 19 };
168
169        int[] FEEDER_BUFFER = { 20, 39 };
170
171        int[] INTAKE_BUFFER = { 40, 59 };
172
173        int[] HANDOFF_BUFFER = { 60, 79 };
174
175        // shooter
176        LEDPattern SHOOTING = LEDPattern.solid(Color.kOrange);
177
178        LEDPattern FERRYING = LEDPattern.solid(Color.kPurple);
179
180        LEDPattern MANUAL = LEDPattern.solid(Color.kPeru);
181
182        // feeder
183        LEDPattern FEEDER_FORWARD = LEDPattern.solid(Color.kBlue);
184
185        LEDPattern FEEDER_REVERSE = LEDPattern.solid(Color.kRed);
186
187        // intake
188        LEDPattern INTAKING = LEDPattern.solid(Color.kYellow);
189
190        LEDPattern OUTTAKING = LEDPattern.solid(Color.kGreen);
191
192        LEDPattern HOMING_DOWN = LEDPattern.solid(Color.kGainsboro);
193
194        LEDPattern AGITATING = LEDPattern.solid(Color.kCyan);
195
196        // handoff
197        LEDPattern HANDOFF_FORWARD = LEDPattern.solid(Color.kDarkOrange);
198
199        // mmm papaya whip
200        LEDPattern HANDOFF_REVERSE = LEDPattern.solid(Color.kPapayaWhip);
201
202        // states
203        LEDPattern DISABLED = LEDPattern.solid(Color.kGray);
204    }
205
206    public interface Handoff {
207        Voltage IDLE_VOLTAGE = Volts.of(0.0);
208
209        Voltage FORWARD_VOLTAGE = Volts.of(12.0);
210
211        Voltage REVERSE_VOLTAGE = Volts.of(-10.0);
212
213        double STALL_CURRENT = 67;
214
215        // TODO: get and maybe convert to wpilib units
216        double STALL_DEBOUNCE = 67;
217
218        double J_KG_METERS_SQUARED = 1;
219
220        double GEAR_RATIO = 1.0 / 3.0; // 1:3
221    }
222
223    public interface Shooter {
224        DoubleSubscriber FIRST_SHOT_BONUS = DogLog.tunable("Shooter/First_shot_bonus_RPM", 250.0);
225        Time FIRST_SHOT_DEBOUNCE = Seconds.of(6.7);
226
227        Time SHOOT_TIME_AUTO = Seconds.of(1.5);
228
229        Velocity<VoltageUnit> RAMP_RATE = Volts.of(1).per(Second);
230
231        Voltage STEP_VOLTAGE = Volts.of(7);
232
233        Distance WHEEL_RADIUS = Inches.of(4);
234
235        // Sim
236        MomentOfInertia J = KilogramSquareMeters.of(0.1);
237
238        double GEAR_RATIO = 0.1;
239
240        // TODO: get
241        Distance FLYWHEEL_RADIUS = Inches.of(3);
242
243        // TODO: Test for manual shooting RPM
244        DoubleSubscriber MANUAL_HUB_RPM = DogLog.tunable("Shooter/Manual Shot Tuning RPM", 3650.0);
245
246        AngularVelocity MIN_SHOOTER_VELOCITY = RPM.of(1740);
247
248        DoubleSubscriber SHOOT_TUNING_RPM = DogLog.tunable("Shooter/Shoot Tuning RPM", 0.0);
249        DoubleSubscriber FERRY_TUNING_RPM = DogLog.tunable("Shooter/Ferry Tuning RPM", 0.0);
250
251        AngularVelocity SHOOTER_SPUN_UP_TOLERANCE = RPM.of(100);
252        public interface RPMInterpolation {
253
254            double[][] distanceRPMInterpolationValues = {
255                {1.46, 2600},
256                {2.07, 3150},
257                {3.13, 3700},
258                {3.45, 3933},
259                {4.13, 4200}
260                //TODO: These numbers don't make sense
261                // { 4.895367348608047, 3250.0 },
262                // { 6.1322461808798705, 3487.0 } 
263            };
264        }
265
266        // These values are placeholders and should be replaced with actual data from testing
267        public interface TOFInterpolation {
268
269            double[][] distanceTOFInterpolationValues = {
270                { 1.0, 0.5 },
271                { 2.0, 0.75 },
272                { 3.0, 1.0 },
273                { 4.0, 1.25 },
274                { 5.0, 1.5 } };
275        }
276
277        // These values are placeholders and should be replaced with actual data from testing
278        public interface FerryRPMInterpolation {
279
280            double[][] ferryDistanceRPMInterpolation = {
281                { 1.0, 2300.0 },
282                { 2.0, 2800.0 },
283                { 3.0, 3300.0 },
284                { 4.0, 3800.0 },
285                { 5.0, 5500.0 } };
286        }
287
288        // These values are placeholders and should be replaced with actual data from testing
289        public interface FerryTOFInterpolation {
290
291            double[][] FerryTOFInterpolationInterpolation = {
292                { 1.0, 0.5 },
293                { 2.0, 0.75 },
294                { 3.0, 1.0 },
295                { 4.0, 1.25 },
296                { 5.0, 1.5 } };
297        }
298        // These values are placeholders and should be replaced with actual data from testing
299    }
300
301    public interface Swerve {
302
303        double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.1;
304
305        double ROTATIONAL_DEADBAND_RAD_PER_S = 0.1;
306
307        public interface Constraints {
308
309            double MAX_VELOCITY_M_PER_S = 4.3;
310
311            // TODO: revert to 15.0
312            double MAX_ACCEL_M_PER_S_SQUARED = 20.0;
313
314            double MAX_ANGULAR_VEL_RAD_PER_S = Units.degreesToRadians(400.0);
315
316            // TODO: revert to 900
317            double MAX_ANGULAR_ACCEL_RAD_PER_S = Units.degreesToRadians(300.0);
318
319            PathConstraints DEFAULT_CONSTRAINTS = new PathConstraints(MAX_VELOCITY_M_PER_S, MAX_ACCEL_M_PER_S_SQUARED, MAX_ANGULAR_VEL_RAD_PER_S, MAX_ANGULAR_ACCEL_RAD_PER_S);
320        }
321
322        public interface Alignment {
323
324            public interface Constraints {
325
326                double DEFAULT_MAX_VELOCITY = 4.3;
327
328                double DEFAULT_MAX_ACCELERATION = 15.0;
329
330                double DEFAULT_MAX_ANGULAR_VELOCITY = Units.degreesToRadians(400.0);
331
332                double DEFAULT_MAX_ANGULAR_ACCELERATION = Units.degreesToRadians(900.0);
333            }
334
335            public interface Tolerances {
336
337                Distance X_TOLERANCE = Inches.of(2.0);
338
339                Distance Y_TOLERANCE = Inches.of(2.0);
340
341                Rotation2d THETA_TOLERANCE = Rotation2d.fromDegrees(8);
342
343                Pose2d POSE_TOLERANCE = new Pose2d(X_TOLERANCE.in(Meters), Y_TOLERANCE.in(Meters), THETA_TOLERANCE);
344
345                LinearVelocity MAX_VELOCITY_WHEN_ALIGNED = MetersPerSecond.of(0.15);
346
347                Time ALIGNMENT_DEBOUNCE = Seconds.of(0.15);
348            }
349
350            public interface Targets {
351
352                // TODO: Get actual angle
353                Rotation2d HUB_LEFT_CORNER = Rotation2d.fromDegrees(45);
354
355                Rotation2d HUB_RIGHT_CORNER = Rotation2d.fromDegrees(-45);
356            }
357        }
358    }
359
360    public interface Driver {
361
362        double BUZZ_TIME = 1.0;
363
364        double BUZZ_INTENSITY = 1.0;
365
366        public interface Drive {
367
368            double DEADBAND = 0.05;
369
370            double RC = 0.05;
371
372            double POWER = 2.0;
373        }
374
375        public interface Turn {
376
377            double DEADBAND = 0.07;
378
379            double RC = 0.05;
380
381            double POWER = 2.0;
382        }
383    }
384}