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
152        Voltage FORWARD_VOLTAGE = Volts.of(10.0);
153
154        // TODO: get from mec
155        double GEAR_RATIO = 34/14; // (34/14) : 1
156
157        MomentOfInertia J = KilogramSquareMeters.of(0.001);
158    }
159
160    public interface LED {
161
162        // TODO: Get actual length of led, along with length of individual sections
163        int LED_LENGTH = 80;
164
165        // Buffer Views {Starting Index, Ending Index}
166        int[] SHOOTER_BUFFER = { 0, 19 };
167
168        int[] FEEDER_BUFFER = { 20, 39 };
169
170        int[] INTAKE_BUFFER = { 40, 59 };
171
172        int[] HANDOFF_BUFFER = { 60, 79 };
173
174        // shooter
175        LEDPattern SHOOTING = LEDPattern.solid(Color.kOrange);
176
177        LEDPattern FERRYING = LEDPattern.solid(Color.kPurple);
178
179        LEDPattern MANUAL = LEDPattern.solid(Color.kPeru);
180
181        // feeder
182        LEDPattern FEEDER_FORWARD = LEDPattern.solid(Color.kBlue);
183
184        LEDPattern FEEDER_REVERSE = LEDPattern.solid(Color.kRed);
185
186        // intake
187        LEDPattern INTAKING = LEDPattern.solid(Color.kYellow);
188
189        LEDPattern OUTTAKING = LEDPattern.solid(Color.kGreen);
190
191        LEDPattern HOMING_DOWN = LEDPattern.solid(Color.kGainsboro);
192
193        LEDPattern AGITATING = LEDPattern.solid(Color.kCyan);
194
195        // handoff
196        LEDPattern HANDOFF_FORWARD = LEDPattern.solid(Color.kDarkOrange);
197
198        // mmm papaya whip
199        LEDPattern HANDOFF_REVERSE = LEDPattern.solid(Color.kPapayaWhip);
200
201        // states
202        LEDPattern DISABLED = LEDPattern.solid(Color.kGray);
203    }
204
205    public interface Handoff {
206        Voltage IDLE_VOLTAGE = Volts.of(0.0);
207
208        Voltage FORWARD_VOLTAGE = Volts.of(12.0);
209
210        Voltage REVERSE_VOLTAGE = Volts.of(-10.0);
211
212        double STALL_CURRENT = 67;
213
214        // TODO: get and maybe convert to wpilib units
215        double STALL_DEBOUNCE = 67;
216
217        double J_KG_METERS_SQUARED = 1;
218
219        double GEAR_RATIO = 1.0 / 3.0; // 1:3
220    }
221
222    public interface Shooter {
223
224        Time SHOOT_TIME_AUTO = Seconds.of(1.5);
225
226        Velocity<VoltageUnit> RAMP_RATE = Volts.of(1).per(Second);
227
228        Voltage STEP_VOLTAGE = Volts.of(7);
229
230        Distance WHEEL_RADIUS = Inches.of(4);
231
232        // Sim
233        MomentOfInertia J = KilogramSquareMeters.of(0.1);
234
235        double GEAR_RATIO = 0.1;
236
237        // TODO: get
238        Distance FLYWHEEL_RADIUS = Inches.of(3);
239
240        // TODO: Test for manual shooting RPM
241        DoubleSubscriber MANUAL_HUB_RPM = DogLog.tunable("Shooter/Manual Shot Tuning RPM", 3650.0);
242
243        AngularVelocity MIN_SHOOTER_VELOCITY = RPM.of(1740);
244
245        DoubleSubscriber SHOOT_TUNING_RPM = DogLog.tunable("Shooter/Shoot Tuning RPM", 0.0);
246        DoubleSubscriber FERRY_TUNING_RPM = DogLog.tunable("Shooter/Ferry Tuning RPM", 0.0);
247
248        AngularVelocity SHOOTER_SPUN_UP_TOLERANCE = RPM.of(100);
249        public interface RPMInterpolation {
250
251            double[][] distanceRPMInterpolationValues = {
252                {1.46, 2600},
253                {2.07, 3150},
254                {3.0, 3633.0},
255                {3.13, 3700},
256                {3.45, 3933},
257                //TODO: These numbers don't make sense
258                // { 4.895367348608047, 3250.0 },
259                // { 6.1322461808798705, 3487.0 } 
260            };
261        }
262
263        // These values are placeholders and should be replaced with actual data from testing
264        public interface TOFInterpolation {
265
266            double[][] distanceTOFInterpolationValues = {
267                { 1.0, 0.5 },
268                { 2.0, 0.75 },
269                { 3.0, 1.0 },
270                { 4.0, 1.25 },
271                { 5.0, 1.5 } };
272        }
273
274        // These values are placeholders and should be replaced with actual data from testing
275        public interface FerryRPMInterpolation {
276
277            double[][] ferryDistanceRPMInterpolation = {
278                { 1.0, 2300.0 },
279                { 2.0, 2800.0 },
280                { 3.0, 3300.0 },
281                { 4.0, 3800.0 },
282                { 5.0, 5500.0 } };
283        }
284
285        // These values are placeholders and should be replaced with actual data from testing
286        public interface FerryTOFInterpolation {
287
288            double[][] FerryTOFInterpolationInterpolation = {
289                { 1.0, 0.5 },
290                { 2.0, 0.75 },
291                { 3.0, 1.0 },
292                { 4.0, 1.25 },
293                { 5.0, 1.5 } };
294        }
295        // These values are placeholders and should be replaced with actual data from testing
296    }
297
298    public interface Swerve {
299
300        double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.1;
301
302        double ROTATIONAL_DEADBAND_RAD_PER_S = 0.1;
303
304        public interface Constraints {
305
306            double MAX_VELOCITY_M_PER_S = 4.3;
307
308            // TODO: revert to 15.0
309            double MAX_ACCEL_M_PER_S_SQUARED = 20.0;
310
311            double MAX_ANGULAR_VEL_RAD_PER_S = Units.degreesToRadians(400.0);
312
313            // TODO: revert to 900
314            double MAX_ANGULAR_ACCEL_RAD_PER_S = Units.degreesToRadians(300.0);
315
316            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);
317        }
318
319        public interface Alignment {
320
321            public interface Constraints {
322
323                double DEFAULT_MAX_VELOCITY = 4.3;
324
325                double DEFAULT_MAX_ACCELERATION = 15.0;
326
327                double DEFAULT_MAX_ANGULAR_VELOCITY = Units.degreesToRadians(400.0);
328
329                double DEFAULT_MAX_ANGULAR_ACCELERATION = Units.degreesToRadians(900.0);
330            }
331
332            public interface Tolerances {
333
334                Distance X_TOLERANCE = Inches.of(2.0);
335
336                Distance Y_TOLERANCE = Inches.of(2.0);
337
338                Rotation2d THETA_TOLERANCE = Rotation2d.fromDegrees(8);
339
340                Pose2d POSE_TOLERANCE = new Pose2d(X_TOLERANCE.in(Meters), Y_TOLERANCE.in(Meters), THETA_TOLERANCE);
341
342                LinearVelocity MAX_VELOCITY_WHEN_ALIGNED = MetersPerSecond.of(0.15);
343
344                Time ALIGNMENT_DEBOUNCE = Seconds.of(0.15);
345            }
346
347            public interface Targets {
348
349                // TODO: Get actual angle
350                Rotation2d HUB_LEFT_CORNER = Rotation2d.fromDegrees(45);
351
352                Rotation2d HUB_RIGHT_CORNER = Rotation2d.fromDegrees(-45);
353            }
354        }
355    }
356
357    public interface Driver {
358
359        double BUZZ_TIME = 1.0;
360
361        double BUZZ_INTENSITY = 1.0;
362
363        public interface Drive {
364
365            double DEADBAND = 0.05;
366
367            double RC = 0.05;
368
369            double POWER = 2.0;
370        }
371
372        public interface Turn {
373
374            double DEADBAND = 0.05;
375
376            double RC = 0.05;
377
378            double POWER = 2.0;
379        }
380    }
381}