001/*
002 * Copyright (C) Cross The Road Electronics.  All rights reserved.
003 * License information can be found in CTRE_LICENSE.txt
004 * For support and suggestions contact support@ctr-electronics.com or file
005 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
006 */
007package com.ctre.phoenix6.configs;
008
009import com.ctre.phoenix6.StatusCode;
010import com.ctre.phoenix6.configs.jni.ConfigJNI;
011import java.util.HashMap;
012import java.util.Map;
013
014import com.ctre.phoenix6.spns.*;
015import com.ctre.phoenix6.signals.*;
016
017/**
018 * Gains for the specified slot.
019 * <p>
020 * If this slot is selected, these gains are used in closed loop
021 * control requests.
022 */
023public class SlotConfigs implements ParentConfiguration
024{
025    /**
026     * Proportional Gain.
027     * <p>
028     * The units for this gain is dependent on the control mode. Since
029     * this gain is multiplied by error in the input, the units should be
030     * defined as units of output per unit of input error. For example,
031     * when controlling velocity using a duty cycle closed loop, the units
032     * for the proportional gain will be duty cycle per rps of error, or
033     * 1/rps.
034     * 
035     * <ul>
036     *   <li> <b>Minimum Value:</b> 0
037     *   <li> <b>Maximum Value:</b> 3.4e+38
038     *   <li> <b>Default Value:</b> 0
039     *   <li> <b>Units:</b> 
040     * </ul>
041     */
042    public double kP = 0;
043    /**
044     * Integral Gain.
045     * <p>
046     * The units for this gain is dependent on the control mode. Since
047     * this gain is multiplied by error in the input integrated over time
048     * (in units of seconds), the units should be defined as units of
049     * output per unit of integrated input error. For example, when
050     * controlling velocity using a duty cycle closed loop, integrating
051     * velocity over time results in rps * s = rotations. Therefore, the
052     * units for the integral gain will be duty cycle per rotation of
053     * accumulated error, or 1/rot.
054     * 
055     * <ul>
056     *   <li> <b>Minimum Value:</b> 0
057     *   <li> <b>Maximum Value:</b> 3.4e+38
058     *   <li> <b>Default Value:</b> 0
059     *   <li> <b>Units:</b> 
060     * </ul>
061     */
062    public double kI = 0;
063    /**
064     * Derivative Gain.
065     * <p>
066     * The units for this gain is dependent on the control mode. Since
067     * this gain is multiplied by the derivative of error in the input
068     * with respect to time (in units of seconds), the units should be
069     * defined as units of output per unit of the differentiated input
070     * error. For example, when controlling velocity using a duty cycle
071     * closed loop, the derivative of velocity with respect to time is rot
072     * per sec², which is acceleration. Therefore, the units for the
073     * derivative gain will be duty cycle per unit of acceleration error,
074     * or 1/(rot per sec²).
075     * 
076     * <ul>
077     *   <li> <b>Minimum Value:</b> 0
078     *   <li> <b>Maximum Value:</b> 3.4e+38
079     *   <li> <b>Default Value:</b> 0
080     *   <li> <b>Units:</b> 
081     * </ul>
082     */
083    public double kD = 0;
084    /**
085     * Static Feedforward Gain.
086     * <p>
087     * This is added to the closed loop output. The unit for this constant
088     * is dependent on the control mode, typically fractional duty cycle,
089     * voltage, or torque current.
090     * <p>
091     * The sign is typically determined by reference velocity when using
092     * position, velocity, and Motion Magic® closed loop modes. However,
093     * when using position closed loop with zero velocity reference (no
094     * motion profiling), the application can instead use the position
095     * closed loop error by setting the Static Feedforward Sign
096     * configuration parameter.  When doing so, we recommend the minimal
097     * amount of kS, otherwise the motor output may dither when closed
098     * loop error is near zero.
099     * 
100     * <ul>
101     *   <li> <b>Minimum Value:</b> -512
102     *   <li> <b>Maximum Value:</b> 511
103     *   <li> <b>Default Value:</b> 0
104     *   <li> <b>Units:</b> 
105     * </ul>
106     */
107    public double kS = 0;
108    /**
109     * Velocity Feedforward Gain.
110     * <p>
111     * The units for this gain is dependent on the control mode. Since
112     * this gain is multiplied by the requested velocity, the units should
113     * be defined as units of output per unit of requested input velocity.
114     * For example, when controlling velocity using a duty cycle closed
115     * loop, the units for the velocity feedfoward gain will be duty cycle
116     * per requested rps, or 1/rps.
117     * 
118     * <ul>
119     *   <li> <b>Minimum Value:</b> 0
120     *   <li> <b>Maximum Value:</b> 3.4e+38
121     *   <li> <b>Default Value:</b> 0
122     *   <li> <b>Units:</b> 
123     * </ul>
124     */
125    public double kV = 0;
126    /**
127     * Acceleration Feedforward Gain.
128     * <p>
129     * The units for this gain is dependent on the control mode. Since
130     * this gain is multiplied by the requested acceleration, the units
131     * should be defined as units of output per unit of requested input
132     * acceleration. For example, when controlling velocity using a duty
133     * cycle closed loop, the units for the acceleration feedfoward gain
134     * will be duty cycle per requested rot per sec², or 1/(rot per sec²).
135     * 
136     * <ul>
137     *   <li> <b>Minimum Value:</b> 0
138     *   <li> <b>Maximum Value:</b> 3.4e+38
139     *   <li> <b>Default Value:</b> 0
140     *   <li> <b>Units:</b> 
141     * </ul>
142     */
143    public double kA = 0;
144    /**
145     * Gravity Feedforward/Feedback Gain.
146     * <p>
147     * This is added to the closed loop output. The sign is determined by
148     * GravityType. The unit for this constant is dependent on the control
149     * mode, typically fractional duty cycle, voltage, or torque current.
150     * 
151     * <ul>
152     *   <li> <b>Minimum Value:</b> -512
153     *   <li> <b>Maximum Value:</b> 511
154     *   <li> <b>Default Value:</b> 0
155     *   <li> <b>Units:</b> 
156     * </ul>
157     */
158    public double kG = 0;
159    /**
160     * Gravity Feedforward/Feedback Type.
161     * <p>
162     * This determines the type of the gravity feedforward/feedback.
163     * <p>
164     * Choose Elevator_Static for systems where the gravity feedforward is
165     * constant, such as an elevator. The gravity feedforward output will
166     * always have the same sign.
167     * <p>
168     * Choose Arm_Cosine for systems where the gravity feedback is
169     * dependent on the angular position of the mechanism, such as an arm.
170     * The gravity feedback output will vary depending on the mechanism
171     * angular position. Note that the sensor offset and ratios must be
172     * configured so that the sensor reports a position of 0 when the
173     * mechanism is horizonal (parallel to the ground), and the reported
174     * sensor position is 1:1 with the mechanism.
175     * 
176     */
177    public GravityTypeValue GravityType = GravityTypeValue.Elevator_Static;
178    /**
179     * Static Feedforward Sign during position closed loop.
180     * <p>
181     * This determines the sign of the applied kS during position
182     * closed-loop modes. The default behavior uses the velocity reference
183     * sign. This works well with velocity closed loop, Motion Magic®
184     * controls, and position closed loop when velocity reference is
185     * specified (motion profiling).
186     * <p>
187     * However, when using position closed loop with zero velocity
188     * reference (no motion profiling), the application may want to apply
189     * static feedforward based on the sign of closed loop error instead.
190     * When doing so, we recommend using the minimal amount of kS,
191     * otherwise the motor output may dither when closed loop error is
192     * near zero.
193     * 
194     */
195    public StaticFeedforwardSignValue StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign;
196    
197    /**
198     * Modifies this configuration's kP parameter and returns itself for
199     * method-chaining and easier to use config API.
200     * <p>
201     * Proportional Gain.
202     * <p>
203     * The units for this gain is dependent on the control mode. Since
204     * this gain is multiplied by error in the input, the units should be
205     * defined as units of output per unit of input error. For example,
206     * when controlling velocity using a duty cycle closed loop, the units
207     * for the proportional gain will be duty cycle per rps of error, or
208     * 1/rps.
209     * 
210     * <ul>
211     *   <li> <b>Minimum Value:</b> 0
212     *   <li> <b>Maximum Value:</b> 3.4e+38
213     *   <li> <b>Default Value:</b> 0
214     *   <li> <b>Units:</b> 
215     * </ul>
216     *
217     * @param newKP Parameter to modify
218     * @return Itself
219     */
220    public SlotConfigs withKP(double newKP)
221    {
222        kP = newKP;
223        return this;
224    }
225    
226    /**
227     * Modifies this configuration's kI parameter and returns itself for
228     * method-chaining and easier to use config API.
229     * <p>
230     * Integral Gain.
231     * <p>
232     * The units for this gain is dependent on the control mode. Since
233     * this gain is multiplied by error in the input integrated over time
234     * (in units of seconds), the units should be defined as units of
235     * output per unit of integrated input error. For example, when
236     * controlling velocity using a duty cycle closed loop, integrating
237     * velocity over time results in rps * s = rotations. Therefore, the
238     * units for the integral gain will be duty cycle per rotation of
239     * accumulated error, or 1/rot.
240     * 
241     * <ul>
242     *   <li> <b>Minimum Value:</b> 0
243     *   <li> <b>Maximum Value:</b> 3.4e+38
244     *   <li> <b>Default Value:</b> 0
245     *   <li> <b>Units:</b> 
246     * </ul>
247     *
248     * @param newKI Parameter to modify
249     * @return Itself
250     */
251    public SlotConfigs withKI(double newKI)
252    {
253        kI = newKI;
254        return this;
255    }
256    
257    /**
258     * Modifies this configuration's kD parameter and returns itself for
259     * method-chaining and easier to use config API.
260     * <p>
261     * Derivative Gain.
262     * <p>
263     * The units for this gain is dependent on the control mode. Since
264     * this gain is multiplied by the derivative of error in the input
265     * with respect to time (in units of seconds), the units should be
266     * defined as units of output per unit of the differentiated input
267     * error. For example, when controlling velocity using a duty cycle
268     * closed loop, the derivative of velocity with respect to time is rot
269     * per sec², which is acceleration. Therefore, the units for the
270     * derivative gain will be duty cycle per unit of acceleration error,
271     * or 1/(rot per sec²).
272     * 
273     * <ul>
274     *   <li> <b>Minimum Value:</b> 0
275     *   <li> <b>Maximum Value:</b> 3.4e+38
276     *   <li> <b>Default Value:</b> 0
277     *   <li> <b>Units:</b> 
278     * </ul>
279     *
280     * @param newKD Parameter to modify
281     * @return Itself
282     */
283    public SlotConfigs withKD(double newKD)
284    {
285        kD = newKD;
286        return this;
287    }
288    
289    /**
290     * Modifies this configuration's kS parameter and returns itself for
291     * method-chaining and easier to use config API.
292     * <p>
293     * Static Feedforward Gain.
294     * <p>
295     * This is added to the closed loop output. The unit for this constant
296     * is dependent on the control mode, typically fractional duty cycle,
297     * voltage, or torque current.
298     * <p>
299     * The sign is typically determined by reference velocity when using
300     * position, velocity, and Motion Magic® closed loop modes. However,
301     * when using position closed loop with zero velocity reference (no
302     * motion profiling), the application can instead use the position
303     * closed loop error by setting the Static Feedforward Sign
304     * configuration parameter.  When doing so, we recommend the minimal
305     * amount of kS, otherwise the motor output may dither when closed
306     * loop error is near zero.
307     * 
308     * <ul>
309     *   <li> <b>Minimum Value:</b> -512
310     *   <li> <b>Maximum Value:</b> 511
311     *   <li> <b>Default Value:</b> 0
312     *   <li> <b>Units:</b> 
313     * </ul>
314     *
315     * @param newKS Parameter to modify
316     * @return Itself
317     */
318    public SlotConfigs withKS(double newKS)
319    {
320        kS = newKS;
321        return this;
322    }
323    
324    /**
325     * Modifies this configuration's kV parameter and returns itself for
326     * method-chaining and easier to use config API.
327     * <p>
328     * Velocity Feedforward Gain.
329     * <p>
330     * The units for this gain is dependent on the control mode. Since
331     * this gain is multiplied by the requested velocity, the units should
332     * be defined as units of output per unit of requested input velocity.
333     * For example, when controlling velocity using a duty cycle closed
334     * loop, the units for the velocity feedfoward gain will be duty cycle
335     * per requested rps, or 1/rps.
336     * 
337     * <ul>
338     *   <li> <b>Minimum Value:</b> 0
339     *   <li> <b>Maximum Value:</b> 3.4e+38
340     *   <li> <b>Default Value:</b> 0
341     *   <li> <b>Units:</b> 
342     * </ul>
343     *
344     * @param newKV Parameter to modify
345     * @return Itself
346     */
347    public SlotConfigs withKV(double newKV)
348    {
349        kV = newKV;
350        return this;
351    }
352    
353    /**
354     * Modifies this configuration's kA parameter and returns itself for
355     * method-chaining and easier to use config API.
356     * <p>
357     * Acceleration Feedforward Gain.
358     * <p>
359     * The units for this gain is dependent on the control mode. Since
360     * this gain is multiplied by the requested acceleration, the units
361     * should be defined as units of output per unit of requested input
362     * acceleration. For example, when controlling velocity using a duty
363     * cycle closed loop, the units for the acceleration feedfoward gain
364     * will be duty cycle per requested rot per sec², or 1/(rot per sec²).
365     * 
366     * <ul>
367     *   <li> <b>Minimum Value:</b> 0
368     *   <li> <b>Maximum Value:</b> 3.4e+38
369     *   <li> <b>Default Value:</b> 0
370     *   <li> <b>Units:</b> 
371     * </ul>
372     *
373     * @param newKA Parameter to modify
374     * @return Itself
375     */
376    public SlotConfigs withKA(double newKA)
377    {
378        kA = newKA;
379        return this;
380    }
381    
382    /**
383     * Modifies this configuration's kG parameter and returns itself for
384     * method-chaining and easier to use config API.
385     * <p>
386     * Gravity Feedforward/Feedback Gain.
387     * <p>
388     * This is added to the closed loop output. The sign is determined by
389     * GravityType. The unit for this constant is dependent on the control
390     * mode, typically fractional duty cycle, voltage, or torque current.
391     * 
392     * <ul>
393     *   <li> <b>Minimum Value:</b> -512
394     *   <li> <b>Maximum Value:</b> 511
395     *   <li> <b>Default Value:</b> 0
396     *   <li> <b>Units:</b> 
397     * </ul>
398     *
399     * @param newKG Parameter to modify
400     * @return Itself
401     */
402    public SlotConfigs withKG(double newKG)
403    {
404        kG = newKG;
405        return this;
406    }
407    
408    /**
409     * Modifies this configuration's GravityType parameter and returns itself for
410     * method-chaining and easier to use config API.
411     * <p>
412     * Gravity Feedforward/Feedback Type.
413     * <p>
414     * This determines the type of the gravity feedforward/feedback.
415     * <p>
416     * Choose Elevator_Static for systems where the gravity feedforward is
417     * constant, such as an elevator. The gravity feedforward output will
418     * always have the same sign.
419     * <p>
420     * Choose Arm_Cosine for systems where the gravity feedback is
421     * dependent on the angular position of the mechanism, such as an arm.
422     * The gravity feedback output will vary depending on the mechanism
423     * angular position. Note that the sensor offset and ratios must be
424     * configured so that the sensor reports a position of 0 when the
425     * mechanism is horizonal (parallel to the ground), and the reported
426     * sensor position is 1:1 with the mechanism.
427     * 
428     *
429     * @param newGravityType Parameter to modify
430     * @return Itself
431     */
432    public SlotConfigs withGravityType(GravityTypeValue newGravityType)
433    {
434        GravityType = newGravityType;
435        return this;
436    }
437    
438    /**
439     * Modifies this configuration's StaticFeedforwardSign parameter and returns itself for
440     * method-chaining and easier to use config API.
441     * <p>
442     * Static Feedforward Sign during position closed loop.
443     * <p>
444     * This determines the sign of the applied kS during position
445     * closed-loop modes. The default behavior uses the velocity reference
446     * sign. This works well with velocity closed loop, Motion Magic®
447     * controls, and position closed loop when velocity reference is
448     * specified (motion profiling).
449     * <p>
450     * However, when using position closed loop with zero velocity
451     * reference (no motion profiling), the application may want to apply
452     * static feedforward based on the sign of closed loop error instead.
453     * When doing so, we recommend using the minimal amount of kS,
454     * otherwise the motor output may dither when closed loop error is
455     * near zero.
456     * 
457     *
458     * @param newStaticFeedforwardSign Parameter to modify
459     * @return Itself
460     */
461    public SlotConfigs withStaticFeedforwardSign(StaticFeedforwardSignValue newStaticFeedforwardSign)
462    {
463        StaticFeedforwardSign = newStaticFeedforwardSign;
464        return this;
465    }
466
467
468    /**
469     * Chooses which slot these configs are for.
470     */
471    public int SlotNumber = 0;
472
473    private class SlotSpns
474    {
475        int kPSpn;
476        int kISpn;
477        int kDSpn;
478        int kSSpn;
479        int kVSpn;
480        int kASpn;
481        int kGSpn;
482        int GravityTypeSpn;
483        int StaticFeedforwardSignSpn;
484    }
485
486    private Map<Integer, SlotSpns> genericMap = new HashMap<Integer, SlotSpns>() {{
487        put(0, new SlotSpns() {{
488            kPSpn = SpnValue.Slot0_kP.value;
489            kISpn = SpnValue.Slot0_kI.value;
490            kDSpn = SpnValue.Slot0_kD.value;
491            kSSpn = SpnValue.Slot0_kS.value;
492            kVSpn = SpnValue.Slot0_kV.value;
493            kASpn = SpnValue.Slot0_kA.value;
494            kGSpn = SpnValue.Slot0_kG.value;
495            GravityTypeSpn = SpnValue.Slot0_kG_Type.value;
496            StaticFeedforwardSignSpn = SpnValue.Slot0_kS_Sign.value;
497        }});
498        put(1, new SlotSpns() {{
499            kPSpn = SpnValue.Slot1_kP.value;
500            kISpn = SpnValue.Slot1_kI.value;
501            kDSpn = SpnValue.Slot1_kD.value;
502            kSSpn = SpnValue.Slot1_kS.value;
503            kVSpn = SpnValue.Slot1_kV.value;
504            kASpn = SpnValue.Slot1_kA.value;
505            kGSpn = SpnValue.Slot1_kG.value;
506            GravityTypeSpn = SpnValue.Slot1_kG_Type.value;
507            StaticFeedforwardSignSpn = SpnValue.Slot1_kS_Sign.value;
508        }});
509        put(2, new SlotSpns() {{
510            kPSpn = SpnValue.Slot2_kP.value;
511            kISpn = SpnValue.Slot2_kI.value;
512            kDSpn = SpnValue.Slot2_kD.value;
513            kSSpn = SpnValue.Slot2_kS.value;
514            kVSpn = SpnValue.Slot2_kV.value;
515            kASpn = SpnValue.Slot2_kA.value;
516            kGSpn = SpnValue.Slot2_kG.value;
517            GravityTypeSpn = SpnValue.Slot2_kG_Type.value;
518            StaticFeedforwardSignSpn = SpnValue.Slot2_kS_Sign.value;
519        }});
520    }};
521
522    public static SlotConfigs from(Slot0Configs value)
523    {
524        return new SlotConfigs() {{
525            kP = value.kP;
526            kI = value.kI;
527            kD = value.kD;
528            kS = value.kS;
529            kV = value.kV;
530            kA = value.kA;
531            kG = value.kG;
532            GravityType = value.GravityType;
533            StaticFeedforwardSign = value.StaticFeedforwardSign;
534            SlotNumber = 0;
535        }};
536    }
537    public static SlotConfigs from(Slot1Configs value)
538    {
539        return new SlotConfigs() {{
540            kP = value.kP;
541            kI = value.kI;
542            kD = value.kD;
543            kS = value.kS;
544            kV = value.kV;
545            kA = value.kA;
546            kG = value.kG;
547            GravityType = value.GravityType;
548            StaticFeedforwardSign = value.StaticFeedforwardSign;
549            SlotNumber = 1;
550        }};
551    }
552    public static SlotConfigs from(Slot2Configs value)
553    {
554        return new SlotConfigs() {{
555            kP = value.kP;
556            kI = value.kI;
557            kD = value.kD;
558            kS = value.kS;
559            kV = value.kV;
560            kA = value.kA;
561            kG = value.kG;
562            GravityType = value.GravityType;
563            StaticFeedforwardSign = value.StaticFeedforwardSign;
564            SlotNumber = 2;
565        }};
566    }
567
568    @Override
569    public String toString()
570    {
571        String ss = "Config Group: Slot\n";
572        ss += "    kP: " + kP + "\n";
573        ss += "    kI: " + kI + "\n";
574        ss += "    kD: " + kD + "\n";
575        ss += "    kS: " + kS + "\n";
576        ss += "    kV: " + kV + "\n";
577        ss += "    kA: " + kA + "\n";
578        ss += "    kG: " + kG + "\n";
579        ss += "    GravityType: " + GravityType + "\n";
580        ss += "    StaticFeedforwardSign: " + StaticFeedforwardSign + "\n";
581        return ss;
582    }
583
584    /**
585     *
586     */
587    public StatusCode deserialize(String to_deserialize)
588    {
589        SlotSpns currentSpns = genericMap.get(SlotNumber);
590        kP = ConfigJNI.Deserializedouble(currentSpns.kPSpn, to_deserialize);
591        kI = ConfigJNI.Deserializedouble(currentSpns.kISpn, to_deserialize);
592        kD = ConfigJNI.Deserializedouble(currentSpns.kDSpn, to_deserialize);
593        kS = ConfigJNI.Deserializedouble(currentSpns.kSSpn, to_deserialize);
594        kV = ConfigJNI.Deserializedouble(currentSpns.kVSpn, to_deserialize);
595        kA = ConfigJNI.Deserializedouble(currentSpns.kASpn, to_deserialize);
596        kG = ConfigJNI.Deserializedouble(currentSpns.kGSpn, to_deserialize);
597        GravityType = GravityTypeValue.valueOf(ConfigJNI.Deserializeint(currentSpns.GravityTypeSpn, to_deserialize));
598        StaticFeedforwardSign = StaticFeedforwardSignValue.valueOf(ConfigJNI.Deserializeint(currentSpns.StaticFeedforwardSignSpn, to_deserialize));
599        return  StatusCode.OK;
600    }
601
602    /**
603     *
604     */
605    public String serialize()
606    {
607        String ss = "";
608        SlotSpns currentSpns = genericMap.get(SlotNumber);
609        ss += ConfigJNI.Serializedouble(currentSpns.kPSpn, kP);
610        ss += ConfigJNI.Serializedouble(currentSpns.kISpn, kI);
611        ss += ConfigJNI.Serializedouble(currentSpns.kDSpn, kD);
612        ss += ConfigJNI.Serializedouble(currentSpns.kSSpn, kS);
613        ss += ConfigJNI.Serializedouble(currentSpns.kVSpn, kV);
614        ss += ConfigJNI.Serializedouble(currentSpns.kASpn, kA);
615        ss += ConfigJNI.Serializedouble(currentSpns.kGSpn, kG);
616        ss += ConfigJNI.Serializeint(currentSpns.GravityTypeSpn, GravityType.value);
617        ss += ConfigJNI.Serializeint(currentSpns.StaticFeedforwardSignSpn, StaticFeedforwardSign.value);
618        return ss;
619    }
620}
621