001/**
002 * Copyright 2024 The Space Cookies : Girl Scout Troop #62868 and FRC Team #1868
003 * Open Source Software; you may modify and/or share it under the terms of
004 * the 3-Clause BSD License found in the root directory of this project.
005 */
006
007package tagalong.subsystems.micro.confs;
008
009import com.ctre.phoenix6.configs.CANcoderConfiguration;
010import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
011import com.ctre.phoenix6.configs.MagnetSensorConfigs;
012import com.ctre.phoenix6.signals.InvertedValue;
013import com.ctre.phoenix6.signals.NeutralModeValue;
014import com.ctre.phoenix6.signals.SensorDirectionValue;
015import edu.wpi.first.math.controller.ArmFeedforward;
016import edu.wpi.first.wpilibj.IterativeRobotBase;
017import tagalong.controls.FeedforwardConstants;
018import tagalong.controls.PIDSGVAConstants;
019import tagalong.devices.Encoders;
020import tagalong.devices.Motors;
021import tagalong.units.AccelerationUnits;
022import tagalong.units.DistanceUnits;
023import tagalong.units.VelocityUnits;
024
025/**
026 * Configuration for the pivot microsystem
027 */
028public class PivotConf extends MicrosystemConf {
029  /**
030   * Unit for rotational limit (default: rotations)
031   */
032  public final DistanceUnits rotationalLimitUnit = DistanceUnits.ROTATION;
033  /**
034   * Minimum and maximum rotation
035   */
036  public final double rotationalMin, rotationalMax;
037  /**
038   * Feedforward model for the arm
039   */
040  public final ArmFeedforward feedForward;
041
042  /**
043   * Encoder type used for the pivot
044   */
045  public final Encoders encoderType;
046  /**
047   * Device ID of the encoder
048   */
049  public final int encoderDeviceID;
050  /**
051   * CAN bus of the encoder
052   */
053  public final String encoderCanBus;
054  /**
055   * Ratio between the motor and encoder
056   */
057  public double motorToEncoderRatio;
058  /**
059   * Gear ratio between encoder and pivot
060   */
061  public double encoderToPivotRatio;
062  /**
063   * Encoder configuration
064   */
065  public final CANcoderConfiguration encoderConfig;
066
067  /**
068   * Whether the encoder configuration operates in a zero to one range
069   */
070  public final boolean encoderConfigZeroToOne;
071  /**
072   * Whether the encoder is configured as clockwise positive
073   */
074  public final boolean encoderConfigClockwisePositive;
075  /**
076   * Unit of the encoder magnet offset
077   */
078  public final DistanceUnits encoderConfigMagnetOffsetUnit = DistanceUnits.ROTATION;
079  /**
080   * Value of the encoder magnet offset
081   */
082  public final double encoderConfigMagnetOffsetValue;
083
084  /**
085   * Whether there is continuous wrapping in closed loop configurations
086   */
087  public final boolean closedLoopConfigsContinuousWrap;
088
089  /**
090   * Unit of the feedforward offset
091   */
092  public final DistanceUnits ffOffsetUnit = DistanceUnits.RADIAN;
093  /**
094   * Value of the feedforward offset
095   */
096  public final double ffOffsetValue;
097
098  /**
099   * Unit of the profile offset
100   */
101  public final DistanceUnits profileOffsetUnit = DistanceUnits.ROTATION;
102  /**
103   * Value of the profile offset
104   */
105  public final double profileOffsetValue;
106
107  /* -------- Simulation Specific Control -------- */
108  /**
109   * Dimension of the mechanical system
110   */
111  public final double mech2dDim;
112  /**
113   * Sim root name
114   */
115  public final String rootName;
116  /**
117   * Sim root x coordinate
118   */
119  public final double rootX;
120  /**
121   * Sim root y coordinate
122   */
123  public final double rootY;
124  /**
125   * Moment of inertia for the pivot
126   */
127  public final double pivotMOI;
128  /**
129   * Length of the pivot in meters
130   */
131  public final double pivotLengthM;
132  /**
133   * Control constants
134   */
135  // public static final FeedforwardConstants simFeedForward =
136  // new FeedforwardConstants(0.0, 0.0, 0.0, 0.0);
137  // /* -------- rotational -------- */
138  // public static final PIDSGVAConstants simSlot0 =
139  // new PIDSGVAConstants(1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
140  // /* -------- Velocity -------- */
141  // public static final PIDSGVAConstants simSlot1 =
142  // new PIDSGVAConstants(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
143  // /* -------- Current -------- */
144  // public static final PIDSGVAConstants simSlot2 =
145  // new PIDSGVAConstants(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
146
147  /**
148   *
149   * @param name                            pivot name
150   * @param motorTypes                      motor types
151   * @param motorDeviceIDs                  motor device ids
152   * @param motorCanBus                     motor can bus
153   * @param motorDirection                  motor directions
154   * @param encoderType                     encoder type used for the pivot
155   * @param encoderDeviceID                 device id of the encoder
156   * @param encoderCanBus                   can bus of the encoder
157   * @param encoderConfigZeroToOne          whether the encoder config operates in a 0 to 1 range
158   * @param encoderConfigClockwisePositive  whether the encoder is configured as clockwise positive
159   * @param encoderConfigMagnetOffsetUnit   unit of the encoder magnet offset
160   * @param encoderConfigMagnetOffsetValue  value of the encoder magnet offset
161   * @param motorEnabledBrakeMode           brake mode when motors are enabled
162   * @param motorDisabledBrakeMode          brake mode when motors are disabled
163   * @param motorToPivotRatio               ratio between the motor and encoder
164   * @param encoderToPivotRatio             gear ratio between encoder and pivot
165   * @param rotationalLimitsUnit            unit for rotational limit (default: rotations)
166   * @param rotationalMin                   minimum rotation
167   * @param rotationalMax                   maximum rotation
168   * @param trapezoidalLengthUnit           unit of trapezoidal motion length
169   * @param trapezoidalVelocityUnit         unit of trapezoidal velocity
170   * @param trapezoidalLimitsVelocity       trapezoidal motion velocity limits
171   * @param trapezoidalAccelerationUnit     unit of trapezoidal acceleration
172   * @param trapezoidalLimitsAcceleration   trapezoidal motion acceleration limits
173   * @param defaultTolerancesUnit           unit of default tolerances
174   * @param defaultLowerTolerance           default lower tolerance
175   * @param defaultUpperTolerance           default upper tolerance
176   * @param feedForward                     feedforward model for the arm
177   * @param simFeedForward                  feedforward model for simulation
178   * @param currentLimitsConfigs            current limit configurations
179   * @param slot0                           slot 0 configuration
180   * @param slot1                           slot 1 configuration
181   * @param slot2                           slot 2 configuration
182   * @param simSlot0                        slot 0 configuration for simulation
183   * @param simSlot1                        slot 1 configuration for simulation
184   * @param simSlot2                        slot 2 configuration for simulation
185   * @param closedLoopConfigsContinuousWrap whether continuous wrapping in closed loop configs
186   * @param ffOffsetUnit                    unit of the feedforward offset
187   * @param ffOffsetValue                   value of the feedforward offset
188   * @param profileOffsetUnit               unit of the profile offset
189   * @param profileOffsetValue              value of the profile offset
190   * @param mech2dDim                       dimensions of mechanical system
191   * @param rootName                        sim root name
192   * @param rootX                           sim root x coordinate
193   * @param rootY                           sim root y coordinate
194   * @param pivotMOI                        moment of inertia for the pivot
195   * @param pivotLengthM                    length of the pivot in meters
196   */
197  public PivotConf(
198      String name,
199      Motors[] motorTypes,
200      int[] motorDeviceIDs,
201      String[] motorCanBus,
202      InvertedValue[] motorDirection,
203      Encoders encoderType,
204      int encoderDeviceID,
205      String encoderCanBus,
206      boolean encoderConfigZeroToOne,
207      boolean encoderConfigClockwisePositive,
208      DistanceUnits encoderConfigMagnetOffsetUnit,
209      double encoderConfigMagnetOffsetValue,
210      NeutralModeValue[] motorEnabledBrakeMode,
211      NeutralModeValue[] motorDisabledBrakeMode,
212      int[][] motorToPivotRatio,
213      int[][] encoderToPivotRatio,
214      DistanceUnits rotationalLimitsUnit,
215      double rotationalMin,
216      double rotationalMax,
217      DistanceUnits trapezoidalLengthUnit,
218      VelocityUnits trapezoidalVelocityUnit,
219      double trapezoidalLimitsVelocity,
220      AccelerationUnits trapezoidalAccelerationUnit,
221      double trapezoidalLimitsAcceleration,
222      DistanceUnits defaultTolerancesUnit,
223      double defaultLowerTolerance,
224      double defaultUpperTolerance,
225      FeedforwardConstants feedForward,
226      FeedforwardConstants simFeedForward,
227      CurrentLimitsConfigs currentLimitsConfigs,
228      PIDSGVAConstants slot0,
229      PIDSGVAConstants slot1,
230      PIDSGVAConstants slot2,
231      PIDSGVAConstants simSlot0,
232      PIDSGVAConstants simSlot1,
233      PIDSGVAConstants simSlot2,
234      boolean closedLoopConfigsContinuousWrap, // remove
235      DistanceUnits ffOffsetUnit,
236      double ffOffsetValue,
237      DistanceUnits profileOffsetUnit,
238      double profileOffsetValue,
239      double mech2dDim,
240      String rootName,
241      double rootX,
242      double rootY,
243      double pivotMOI,
244      double pivotLengthM
245  ) {
246    super(
247        name,
248        motorTypes,
249        motorDeviceIDs,
250        motorCanBus,
251        motorDirection,
252        motorEnabledBrakeMode,
253        motorDisabledBrakeMode,
254        motorToPivotRatio,
255        trapezoidalLengthUnit,
256        VelocityUnits.ROTATIONS_PER_SECOND,
257        trapezoidalVelocityUnit.convertX(
258            trapezoidalLimitsVelocity, VelocityUnits.ROTATIONS_PER_SECOND
259        ),
260        AccelerationUnits.ROTATIONS_PER_SECOND2,
261        trapezoidalAccelerationUnit.convertX(
262            trapezoidalLimitsAcceleration, AccelerationUnits.ROTATIONS_PER_SECOND2
263        ),
264        DistanceUnits.ROTATION,
265        defaultTolerancesUnit.convertX(defaultLowerTolerance, DistanceUnits.ROTATION),
266        defaultTolerancesUnit.convertX(defaultUpperTolerance, DistanceUnits.ROTATION),
267        currentLimitsConfigs,
268        slot0,
269        slot1,
270        slot2,
271        simSlot0,
272        simSlot1,
273        simSlot2
274    );
275    this.encoderType = encoderType;
276    this.encoderDeviceID = encoderDeviceID;
277    this.encoderCanBus = encoderCanBus;
278    this.encoderToPivotRatio = super.calculateGearRatio(encoderToPivotRatio);
279    this.motorToEncoderRatio =
280        super.calculateGearRatio(motorToPivotRatio) / this.encoderToPivotRatio;
281    this.encoderConfigZeroToOne = encoderConfigZeroToOne;
282    this.encoderConfigClockwisePositive = encoderConfigClockwisePositive;
283    this.encoderConfigMagnetOffsetValue = encoderConfigMagnetOffsetUnit.convertX(
284        encoderConfigMagnetOffsetValue, this.encoderConfigMagnetOffsetUnit
285    );
286
287    MagnetSensorConfigs magnetSensorConfigs =
288        new MagnetSensorConfigs()
289            .withMagnetOffset(encoderConfigMagnetOffsetValue)
290            .withSensorDirection(
291                encoderConfigClockwisePositive ? SensorDirectionValue.Clockwise_Positive
292                                               : SensorDirectionValue.CounterClockwise_Positive
293            );
294    magnetSensorConfigs.AbsoluteSensorDiscontinuityPoint = encoderConfigZeroToOne ? 1 : 0;
295    this.encoderConfig = new CANcoderConfiguration().withMagnetSensor(magnetSensorConfigs);
296
297    this.rotationalMin = rotationalLimitsUnit.convertX(rotationalMin, this.rotationalLimitUnit);
298    this.rotationalMax = rotationalLimitsUnit.convertX(rotationalMax, this.rotationalLimitUnit);
299    this.feedForward = IterativeRobotBase.isReal() ? feedForward.getArmFeedforward()
300                                                   : simFeedForward.getArmFeedforward();
301    this.closedLoopConfigsContinuousWrap = closedLoopConfigsContinuousWrap;
302    this.ffOffsetValue = ffOffsetUnit.convertX(ffOffsetValue, this.ffOffsetUnit);
303    this.profileOffsetValue =
304        profileOffsetUnit.convertX(profileOffsetValue, this.profileOffsetUnit);
305
306    this.mech2dDim = mech2dDim;
307    this.rootName = rootName;
308    this.rootX = rootX;
309    this.rootY = rootY;
310    this.pivotMOI = pivotMOI;
311    this.pivotLengthM = pivotLengthM;
312  }
313}