001/************************* PROJECT RON *************************/
002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */
003/* Use of this source code is governed by an MIT-style license */
004/* that can be found in the repository LICENSE file.           */
005/***************************************************************/
006package com.stuypulse.robot.constants;
007
008import static edu.wpi.first.units.Units.Amps;
009
010import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
011import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
012import com.ctre.phoenix6.configs.FeedbackConfigs;
013import com.ctre.phoenix6.configs.MotionMagicConfigs;
014import com.ctre.phoenix6.configs.MotorOutputConfigs;
015import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
016import com.ctre.phoenix6.configs.Slot0Configs;
017import com.ctre.phoenix6.configs.Slot1Configs;
018import com.ctre.phoenix6.configs.Slot2Configs;
019import com.ctre.phoenix6.configs.TalonFXConfiguration;
020import com.ctre.phoenix6.hardware.TalonFX;
021import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
022import com.ctre.phoenix6.signals.GravityTypeValue;
023import com.ctre.phoenix6.signals.InvertedValue;
024import com.ctre.phoenix6.signals.NeutralModeValue;
025import edu.wpi.first.networktables.DoubleSubscriber;
026
027/*-
028 * File containing all of the configurations that different motors require.
029 *
030 * Such configurations include:
031 *  - If it is Inverted
032 *  - The Idle Mode of the Motor
033 *  - The Current Limit
034 *  - The Open Loop Ramp Rate
035 */
036public interface Motors {
037
038        /** Classes to store all of the values a motor needs */
039        public interface Intake {
040
041                TalonFXConfig PIVOT_CONFIG = new TalonFXConfig()
042                                .withSupplyCurrentLimitAmps(30)
043                                .withStatorCurrentLimitAmps(40)
044                                .withInvertedValue( // not necessarily true, get inverted val
045                                                InvertedValue.Clockwise_Positive)
046                                .withNeutralMode(NeutralModeValue.Brake)
047                                .withSensorToMechanismRatio(Settings.Intake.Pivot.GEAR_RATIO)
048                                .withGravityType(GravityTypeValue.Arm_Cosine)
049                                .withPIDConstants(Gains.Intake.kP, Gains.Intake.kI, Gains.Intake.kD, 0)
050                                .withFFConstants(
051                                                Gains.Intake.kS.in(Amps),
052                                                Gains.Intake.kA.in(Amps),
053                                                Gains.Intake.kV.in(Amps),
054                                                Gains.Intake.kG.in(Amps), // regular constants
055                                                0)
056                                .withPIDConstants(
057                                                Gains.Intake.Digestion.kP, Gains.Intake.Digestion.kI, Gains.Intake.Digestion.kD, 1)
058                                .withFFConstants(
059                                                Gains.Intake.kS.in(Amps),
060                                                Gains.Intake.kA.in(Amps),
061                                                Gains.Intake.kV.in(Amps),
062                                                Gains.Intake.kG.in(Amps), // digestion constants
063                                                1);
064
065                TalonFXConfig LEFT_ROLLER_CONFIG = // TODO: apply later
066                                new TalonFXConfig()
067                                                .withStatorCurrentLimitAmps(50)
068                                                .withInvertedValue( // not necessarily true, get inverted val
069                                                                InvertedValue.CounterClockwise_Positive)
070                                                .withNeutralMode(NeutralModeValue.Coast)
071                                                .withSensorToMechanismRatio(Settings.Intake.Roller.GEAR_RATIO);
072
073                TalonFXConfig RIGHT_ROLLER_CONFIG = // TODO: apply later
074                                new TalonFXConfig()
075                                                .withStatorCurrentLimitAmps(50)
076                                                .withInvertedValue(InvertedValue.Clockwise_Positive)
077                                                .withNeutralMode(NeutralModeValue.Coast)
078                                                .withSensorToMechanismRatio(Settings.Intake.Roller.GEAR_RATIO);
079        }
080
081        public interface Feeder {
082
083                // TODO: get values after motor pinion swap
084                TalonFXConfig LEADER_CONFIG = new TalonFXConfig()
085                                .withStatorCurrentLimitAmps(80)
086                                .withNeutralMode(NeutralModeValue.Coast)
087                                .withInvertedValue(InvertedValue.Clockwise_Positive)
088                                .withSensorToMechanismRatio(Settings.Feeder.GEAR_RATIO);
089        }
090
091        public interface Shooter {
092                TalonFXConfig SHOOTER_MOTOR_LEFT = new TalonFXConfig()
093                                .withPIDConstants(Gains.Shooter.kP.get(), Gains.Shooter.kI.get(), Gains.Shooter.kD.get(), 0)
094                                .withSupplyCurrentLimitAmps(200)
095                                .withStatorCurrentLimitAmps(200)
096                                .withNeutralMode(NeutralModeValue.Coast)
097                                .withFFConstants(Gains.Shooter.kS.get(), Gains.Shooter.kV.get(), Gains.Shooter.kA.get(), 0)
098                                .withInvertedValue(InvertedValue.CounterClockwise_Positive);
099
100                TalonFXConfig SHOOTER_MOTOR_CENTER = new TalonFXConfig()
101                                .withPIDConstants(Gains.Shooter.kP.get(), Gains.Shooter.kI.get(), Gains.Shooter.kD.get(), 0)
102                                .withSupplyCurrentLimitAmps(200)
103                                .withStatorCurrentLimitAmps(200)
104                                .withNeutralMode(NeutralModeValue.Coast)
105                                .withFFConstants(Gains.Shooter.kS.get(), Gains.Shooter.kV.get(), Gains.Shooter.kA.get(), 0)
106                                .withInvertedValue(InvertedValue.CounterClockwise_Positive);
107
108                TalonFXConfig SHOOTER_MOTOR_RIGHT = new TalonFXConfig()
109                                .withPIDConstants(Gains.Shooter.kP.get(), Gains.Shooter.kI.get(), Gains.Shooter.kD.get(), 0)
110                                .withSupplyCurrentLimitAmps(200)
111                                .withStatorCurrentLimitAmps(200)
112                                .withNeutralMode(NeutralModeValue.Coast)
113                                .withFFConstants(Gains.Shooter.kS.get(), Gains.Shooter.kV.get(), Gains.Shooter.kA.get(), 0)
114                                .withInvertedValue(InvertedValue.Clockwise_Positive);
115        }
116
117        public interface Handoff {
118
119                TalonFXConfig HANDOFF_MOTOR_CONFIG = new TalonFXConfig()
120                                .withStatorCurrentLimitAmps(80)
121                                .withNeutralMode(NeutralModeValue.Coast)
122                                .withInvertedValue(InvertedValue.CounterClockwise_Positive)
123                                .withSensorToMechanismRatio(Settings.Handoff.GEAR_RATIO);
124        }
125
126        public static class TalonFXConfig {
127
128                private final TalonFXConfiguration configuration = new TalonFXConfiguration();
129
130                private final Slot0Configs slot0Configs = new Slot0Configs();
131
132                private final Slot1Configs slot1Configs = new Slot1Configs();
133
134                private final Slot2Configs slot2Configs = new Slot2Configs();
135
136                private final MotorOutputConfigs motorOutputConfigs = new MotorOutputConfigs();
137
138                private final ClosedLoopRampsConfigs closedLoopRampsConfigs = new ClosedLoopRampsConfigs();
139
140                private final OpenLoopRampsConfigs openLoopRampsConfigs = new OpenLoopRampsConfigs();
141
142                private final CurrentLimitsConfigs currentLimitsConfigs = new CurrentLimitsConfigs();
143
144                private final FeedbackConfigs feedbackConfigs = new FeedbackConfigs();
145
146                private final MotionMagicConfigs motionMagicConfigs = new MotionMagicConfigs();
147
148                public TalonFXConfiguration getConfiguration() {
149                        return this.configuration;
150                }
151
152                public void configure(TalonFX motor) {
153                        motor.getConfigurator().apply(configuration);
154                }
155
156                // SLOT 0 CONFIGS
157                public TalonFXConfig withPIDConstants(double kP, double kI, double kD, int slot) {
158                        switch (slot) {
159                                case 0:
160                                        configuration.withSlot0(slot0Configs.withKP(kP).withKI(kI).withKD(kD));
161                                        break;
162                                case 1:
163                                        configuration.withSlot1(slot1Configs.withKP(kP).withKI(kI).withKD(kD));
164                                        break;
165                                case 2:
166                                        configuration.withSlot2(slot2Configs.withKP(kP).withKI(kI).withKD(kD));
167                                        break;
168                        }
169                        return this;
170                }
171
172                public TalonFXConfig withFFConstants(double kS, double kV, double kA, int slot) {
173                        return withFFConstants(kS, kV, kA, 0, slot);
174                }
175
176                public TalonFXConfig withFFConstants(double kS, double kV, double kA, double kG, int slot) {
177                        switch (slot) {
178                                case 0:
179                                        configuration.withSlot0(slot0Configs.withKS(kS).withKV(kV).withKA(kA).withKG(kG));
180                                        break;
181                                case 1:
182                                        configuration.withSlot1(slot1Configs.withKS(kS).withKV(kV).withKA(kA).withKG(kG));
183                                        break;
184                                case 2:
185                                        configuration.withSlot2(slot2Configs.withKS(kS).withKV(kV).withKA(kA).withKG(kG));
186                                        break;
187                        }
188                        return this;
189                }
190
191                public TalonFXConfig withGravityType(GravityTypeValue gravityType) {
192                        configuration.withSlot0(slot0Configs.withGravityType(gravityType));
193                        configuration.withSlot1(slot1Configs.withGravityType(gravityType));
194                        configuration.withSlot2(slot2Configs.withGravityType(gravityType));
195                        return this;
196                }
197
198                // MOTOR OUTPUT CONFIGS
199                public TalonFXConfig withInvertedValue(InvertedValue invertedValue) {
200                        configuration.withMotorOutput(motorOutputConfigs.withInverted(invertedValue));
201                        return this;
202                }
203
204                public TalonFXConfig withNeutralMode(NeutralModeValue neutralMode) {
205                        configuration.withMotorOutput(motorOutputConfigs.withNeutralMode(neutralMode));
206                        return this;
207                }
208
209                // RAMP RATE CONFIGS
210                public TalonFXConfig withRampRate(double rampRate) {
211                        closedLoopRampsConfigs
212                                        .withDutyCycleClosedLoopRampPeriod(rampRate)
213                                        .withTorqueClosedLoopRampPeriod(rampRate)
214                                        .withVoltageClosedLoopRampPeriod(rampRate);
215                        openLoopRampsConfigs
216                                        .withDutyCycleOpenLoopRampPeriod(rampRate)
217                                        .withTorqueOpenLoopRampPeriod(rampRate)
218                                        .withVoltageOpenLoopRampPeriod(rampRate);
219                        configuration.withClosedLoopRamps(closedLoopRampsConfigs);
220                        configuration.withOpenLoopRamps(openLoopRampsConfigs);
221                        return this;
222                }
223
224                // CURRENT LIMIT CONFIGS
225                public TalonFXConfig withStatorCurrentLimitAmps(double currentLimitAmps) {
226                        currentLimitsConfigs
227                                        .withStatorCurrentLimit(currentLimitAmps)
228                                        .withStatorCurrentLimitEnable(true);
229                        configuration.withCurrentLimits(currentLimitsConfigs);
230                        return this;
231                }
232
233                public TalonFXConfig withSupplyCurrentLimitAmps(double currentLimitAmps) {
234                        currentLimitsConfigs
235                                        .withSupplyCurrentLimit(currentLimitAmps)
236                                        .withSupplyCurrentLimitEnable(true);
237                        configuration.withCurrentLimits(currentLimitsConfigs);
238                        return this;
239                }
240
241                // MOTION MAGIC CONFIGS
242                public TalonFXConfig withMotionProfile(double maxVelocity, double maxAcceleration) {
243                        motionMagicConfigs
244                                        .withMotionMagicCruiseVelocity(maxVelocity)
245                                        .withMotionMagicAcceleration(maxAcceleration);
246                        configuration.withMotionMagic(motionMagicConfigs);
247                        return this;
248                }
249
250                // FEEDBACK CONFIGS
251                public TalonFXConfig withRemoteSensor(
252                                int ID, FeedbackSensorSourceValue source, double rotorToSensorRatio) {
253                        feedbackConfigs
254                                        .withFeedbackRemoteSensorID(ID)
255                                        .withFeedbackSensorSource(source)
256                                        .withRotorToSensorRatio(rotorToSensorRatio);
257                        configuration.withFeedback(feedbackConfigs);
258                        return this;
259                }
260
261                public TalonFXConfig withSensorToMechanismRatio(double sensorToMechanismRatio) {
262                        configuration.withFeedback(
263                                        feedbackConfigs.withSensorToMechanismRatio(sensorToMechanismRatio));
264                        return this;
265                }
266        }
267}