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                                .withPIDConstants(Gains.Shooter.FirstShot.kP, Gains.Shooter.FirstShot.kI, Gains.Shooter.FirstShot.kD, 1)
095                                .withSupplyCurrentLimitAmps(200)
096                                .withStatorCurrentLimitAmps(200)
097                                .withNeutralMode(NeutralModeValue.Coast)
098                                .withFFConstants(Gains.Shooter.kS.get(), Gains.Shooter.kV.get(), Gains.Shooter.kA.get(), 0)
099                                .withInvertedValue(InvertedValue.CounterClockwise_Positive);
100
101                TalonFXConfig SHOOTER_MOTOR_CENTER = new TalonFXConfig()
102                                .withPIDConstants(Gains.Shooter.kP.get(), Gains.Shooter.kI.get(), Gains.Shooter.kD.get(), 0)
103                                .withPIDConstants(Gains.Shooter.FirstShot.kP, Gains.Shooter.FirstShot.kI, Gains.Shooter.FirstShot.kD, 1)
104                                .withSupplyCurrentLimitAmps(200)
105                                .withStatorCurrentLimitAmps(200)
106                                .withNeutralMode(NeutralModeValue.Coast)
107                                .withFFConstants(Gains.Shooter.kS.get(), Gains.Shooter.kV.get(), Gains.Shooter.kA.get(), 0)
108                                .withInvertedValue(InvertedValue.CounterClockwise_Positive);
109
110                TalonFXConfig SHOOTER_MOTOR_RIGHT = new TalonFXConfig()
111                                .withPIDConstants(Gains.Shooter.kP.get(), Gains.Shooter.kI.get(), Gains.Shooter.kD.get(), 0)
112                                .withPIDConstants(Gains.Shooter.FirstShot.kP, Gains.Shooter.FirstShot.kI, Gains.Shooter.FirstShot.kD, 1)
113                                .withSupplyCurrentLimitAmps(200)
114                                .withStatorCurrentLimitAmps(200)
115                                .withNeutralMode(NeutralModeValue.Coast)
116                                .withFFConstants(Gains.Shooter.kS.get(), Gains.Shooter.kV.get(), Gains.Shooter.kA.get(), 0)
117                                .withFFConstants(Gains.Shooter.kS.get(), Gains.Shooter.kV.get(), Gains.Shooter.kA.get(), 1)
118                                .withInvertedValue(InvertedValue.Clockwise_Positive);
119        }
120
121        public interface Handoff {
122
123                TalonFXConfig HANDOFF_MOTOR_CONFIG = new TalonFXConfig()
124                                .withStatorCurrentLimitAmps(80)
125                                .withNeutralMode(NeutralModeValue.Coast)
126                                .withInvertedValue(InvertedValue.CounterClockwise_Positive)
127                                .withSensorToMechanismRatio(Settings.Handoff.GEAR_RATIO);
128        }
129
130        public static class TalonFXConfig {
131
132                private final TalonFXConfiguration configuration = new TalonFXConfiguration();
133
134                private final Slot0Configs slot0Configs = new Slot0Configs();
135
136                private final Slot1Configs slot1Configs = new Slot1Configs();
137
138                private final Slot2Configs slot2Configs = new Slot2Configs();
139
140                private final MotorOutputConfigs motorOutputConfigs = new MotorOutputConfigs();
141
142                private final ClosedLoopRampsConfigs closedLoopRampsConfigs = new ClosedLoopRampsConfigs();
143
144                private final OpenLoopRampsConfigs openLoopRampsConfigs = new OpenLoopRampsConfigs();
145
146                private final CurrentLimitsConfigs currentLimitsConfigs = new CurrentLimitsConfigs();
147
148                private final FeedbackConfigs feedbackConfigs = new FeedbackConfigs();
149
150                private final MotionMagicConfigs motionMagicConfigs = new MotionMagicConfigs();
151
152                public TalonFXConfiguration getConfiguration() {
153                        return this.configuration;
154                }
155
156                public void configure(TalonFX motor) {
157                        motor.getConfigurator().apply(configuration);
158                }
159
160                // SLOT 0 CONFIGS
161                public TalonFXConfig withPIDConstants(double kP, double kI, double kD, int slot) {
162                        switch (slot) {
163                                case 0:
164                                        configuration.withSlot0(slot0Configs.withKP(kP).withKI(kI).withKD(kD));
165                                        break;
166                                case 1:
167                                        configuration.withSlot1(slot1Configs.withKP(kP).withKI(kI).withKD(kD));
168                                        break;
169                                case 2:
170                                        configuration.withSlot2(slot2Configs.withKP(kP).withKI(kI).withKD(kD));
171                                        break;
172                        }
173                        return this;
174                }
175
176                public TalonFXConfig withFFConstants(double kS, double kV, double kA, int slot) {
177                        return withFFConstants(kS, kV, kA, 0, slot);
178                }
179
180                public TalonFXConfig withFFConstants(double kS, double kV, double kA, double kG, int slot) {
181                        switch (slot) {
182                                case 0:
183                                        configuration.withSlot0(slot0Configs.withKS(kS).withKV(kV).withKA(kA).withKG(kG));
184                                        break;
185                                case 1:
186                                        configuration.withSlot1(slot1Configs.withKS(kS).withKV(kV).withKA(kA).withKG(kG));
187                                        break;
188                                case 2:
189                                        configuration.withSlot2(slot2Configs.withKS(kS).withKV(kV).withKA(kA).withKG(kG));
190                                        break;
191                        }
192                        return this;
193                }
194
195                public TalonFXConfig withGravityType(GravityTypeValue gravityType) {
196                        configuration.withSlot0(slot0Configs.withGravityType(gravityType));
197                        configuration.withSlot1(slot1Configs.withGravityType(gravityType));
198                        configuration.withSlot2(slot2Configs.withGravityType(gravityType));
199                        return this;
200                }
201
202                // MOTOR OUTPUT CONFIGS
203                public TalonFXConfig withInvertedValue(InvertedValue invertedValue) {
204                        configuration.withMotorOutput(motorOutputConfigs.withInverted(invertedValue));
205                        return this;
206                }
207
208                public TalonFXConfig withNeutralMode(NeutralModeValue neutralMode) {
209                        configuration.withMotorOutput(motorOutputConfigs.withNeutralMode(neutralMode));
210                        return this;
211                }
212
213                // RAMP RATE CONFIGS
214                public TalonFXConfig withRampRate(double rampRate) {
215                        closedLoopRampsConfigs
216                                        .withDutyCycleClosedLoopRampPeriod(rampRate)
217                                        .withTorqueClosedLoopRampPeriod(rampRate)
218                                        .withVoltageClosedLoopRampPeriod(rampRate);
219                        openLoopRampsConfigs
220                                        .withDutyCycleOpenLoopRampPeriod(rampRate)
221                                        .withTorqueOpenLoopRampPeriod(rampRate)
222                                        .withVoltageOpenLoopRampPeriod(rampRate);
223                        configuration.withClosedLoopRamps(closedLoopRampsConfigs);
224                        configuration.withOpenLoopRamps(openLoopRampsConfigs);
225                        return this;
226                }
227
228                // CURRENT LIMIT CONFIGS
229                public TalonFXConfig withStatorCurrentLimitAmps(double currentLimitAmps) {
230                        currentLimitsConfigs
231                                        .withStatorCurrentLimit(currentLimitAmps)
232                                        .withStatorCurrentLimitEnable(true);
233                        configuration.withCurrentLimits(currentLimitsConfigs);
234                        return this;
235                }
236
237                public TalonFXConfig withSupplyCurrentLimitAmps(double currentLimitAmps) {
238                        currentLimitsConfigs
239                                        .withSupplyCurrentLimit(currentLimitAmps)
240                                        .withSupplyCurrentLimitEnable(true);
241                        configuration.withCurrentLimits(currentLimitsConfigs);
242                        return this;
243                }
244
245                // MOTION MAGIC CONFIGS
246                public TalonFXConfig withMotionProfile(double maxVelocity, double maxAcceleration) {
247                        motionMagicConfigs
248                                        .withMotionMagicCruiseVelocity(maxVelocity)
249                                        .withMotionMagicAcceleration(maxAcceleration);
250                        configuration.withMotionMagic(motionMagicConfigs);
251                        return this;
252                }
253
254                // FEEDBACK CONFIGS
255                public TalonFXConfig withRemoteSensor(
256                                int ID, FeedbackSensorSourceValue source, double rotorToSensorRatio) {
257                        feedbackConfigs
258                                        .withFeedbackRemoteSensorID(ID)
259                                        .withFeedbackSensorSource(source)
260                                        .withRotorToSensorRatio(rotorToSensorRatio);
261                        configuration.withFeedback(feedbackConfigs);
262                        return this;
263                }
264
265                public TalonFXConfig withSensorToMechanismRatio(double sensorToMechanismRatio) {
266                        configuration.withFeedback(
267                                        feedbackConfigs.withSensorToMechanismRatio(sensorToMechanismRatio));
268                        return this;
269                }
270        }
271}