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.controls;
008
009import com.ctre.phoenix6.StatusCode;
010import com.ctre.phoenix6.controls.jni.ControlJNI;
011import com.ctre.phoenix6.hardware.traits.*;
012
013import edu.wpi.first.units.*;
014import edu.wpi.first.units.measure.*;
015import static edu.wpi.first.units.Units.*;
016
017import java.util.HashMap;
018import java.util.Map;
019
020/**
021 * Request PID to target position with a differential position setpoint.
022 * <p>
023 * This control mode will set the motor's position setpoint to the position specified by the user. It will
024 * also set the motor's differential position setpoint to the specified position.
025 */
026public class DifferentialPositionDutyCycle extends ControlRequest implements Cloneable
027{
028    /**
029     * Average position to drive toward in rotations.
030     * 
031     * <ul>
032     *   <li> Units: rotations
033     * </ul>
034     * 
035     */
036    public double TargetPosition;
037    /**
038     * Differential position to drive toward in rotations.
039     * 
040     * <ul>
041     *   <li> Units: rotations
042     * </ul>
043     * 
044     */
045    public double DifferentialPosition;
046    /**
047     * Set to true to use FOC commutation (requires Phoenix Pro), which increases
048     * peak power by ~15% on supported devices (see {@link SupportsFOC}). Set to
049     * false to use trapezoidal commutation.
050     * <p>
051     * FOC improves motor performance by leveraging torque (current) control. 
052     * However, this may be inconvenient for applications that require specifying
053     * duty cycle or voltage.  CTR-Electronics has developed a hybrid method that
054     * combines the performances gains of FOC while still allowing applications to
055     * provide duty cycle or voltage demand.  This not to be confused with simple
056     * sinusoidal control or phase voltage control which lacks the performance
057     * gains.
058     */
059    public boolean EnableFOC = true;
060    /**
061     * Select which gains are applied to the primary controller by selecting the
062     * slot.  Use the configuration api to set the gain values for the selected slot
063     * before enabling this feature. Slot must be within [0,2].
064     */
065    public int TargetSlot = 0;
066    /**
067     * Select which gains are applied to the differential controller by selecting
068     * the slot.  Use the configuration api to set the gain values for the selected
069     * slot before enabling this feature. Slot must be within [0,2].
070     */
071    public int DifferentialSlot = 1;
072    /**
073     * Set to true to static-brake the rotor when output is zero (or within
074     * deadband).  Set to false to use the NeutralMode configuration setting
075     * (default). This flag exists to provide the fundamental behavior of this
076     * control when output is zero, which is to provide 0V to the motor.
077     */
078    public boolean OverrideBrakeDurNeutral = false;
079    /**
080     * Set to true to force forward limiting.  This allows users to use other limit
081     * switch sensors connected to robot controller.  This also allows use of active
082     * sensors that require external power.
083     */
084    public boolean LimitForwardMotion = false;
085    /**
086     * Set to true to force reverse limiting.  This allows users to use other limit
087     * switch sensors connected to robot controller.  This also allows use of active
088     * sensors that require external power.
089     */
090    public boolean LimitReverseMotion = false;
091    /**
092     * Set to true to ignore hardware limit switches and the LimitForwardMotion and
093     * LimitReverseMotion parameters, instead allowing motion.
094     * <p>
095     * This can be useful on mechanisms such as an intake/feeder, where a limit
096     * switch stops motion while intaking but should be ignored when feeding to a
097     * shooter.
098     * <p>
099     * The hardware limit faults and Forward/ReverseLimit signals will still report
100     * the values of the limit switches regardless of this parameter.
101     */
102    public boolean IgnoreHardwareLimits = false;
103    /**
104     * Set to true to delay applying this control request until a timesync boundary
105     * (requires Phoenix Pro and CANivore). This eliminates the impact of
106     * nondeterministic network delays in exchange for a larger but deterministic
107     * control latency.
108     * <p>
109     * This requires setting the ControlTimesyncFreqHz config in MotorOutputConfigs.
110     * Additionally, when this is enabled, the UpdateFreqHz of this request should
111     * be set to 0 Hz.
112     */
113    public boolean UseTimesync = false;
114
115    /**
116     * The period at which this control will update at.
117     * This is designated in Hertz, with a minimum of 20 Hz
118     * (every 50 ms) and a maximum of 1000 Hz (every 1 ms).
119     * <p>
120     * If this field is set to 0 Hz, the control request will
121     * be sent immediately as a one-shot frame. This may be useful
122     * for advanced applications that require outputs to be
123     * synchronized with data acquisition. In this case, we
124     * recommend not exceeding 50 ms between control calls.
125     */
126    public double UpdateFreqHz = 100;
127
128    /**
129     * Request PID to target position with a differential position setpoint.
130     * <p>
131     * This control mode will set the motor's position setpoint to the position
132     * specified by the user. It will also set the motor's differential position
133     * setpoint to the specified position.
134     * 
135     * @param TargetPosition    Average position to drive toward in rotations.
136     * @param DifferentialPosition    Differential position to drive toward in
137     *                                rotations.
138     */
139    public DifferentialPositionDutyCycle(double TargetPosition, double DifferentialPosition)
140    {
141        super("DifferentialPositionDutyCycle");
142        this.TargetPosition = TargetPosition;
143        this.DifferentialPosition = DifferentialPosition;
144    }
145
146    /**
147     * Request PID to target position with a differential position setpoint.
148     * <p>
149     * This control mode will set the motor's position setpoint to the position
150     * specified by the user. It will also set the motor's differential position
151     * setpoint to the specified position.
152     * 
153     * @param TargetPosition    Average position to drive toward in rotations.
154     * @param DifferentialPosition    Differential position to drive toward in
155     *                                rotations.
156     */
157    public DifferentialPositionDutyCycle(Angle TargetPosition, Angle DifferentialPosition)
158    {
159        this(TargetPosition.in(Rotations), DifferentialPosition.in(Rotations));
160    }
161
162    @Override
163    public String toString()
164    {
165        String ss = "Control: DifferentialPositionDutyCycle\n";
166        ss += "    TargetPosition: " + TargetPosition + " rotations" + "\n";
167        ss += "    DifferentialPosition: " + DifferentialPosition + " rotations" + "\n";
168        ss += "    EnableFOC: " + EnableFOC + "\n";
169        ss += "    TargetSlot: " + TargetSlot + "\n";
170        ss += "    DifferentialSlot: " + DifferentialSlot + "\n";
171        ss += "    OverrideBrakeDurNeutral: " + OverrideBrakeDurNeutral + "\n";
172        ss += "    LimitForwardMotion: " + LimitForwardMotion + "\n";
173        ss += "    LimitReverseMotion: " + LimitReverseMotion + "\n";
174        ss += "    IgnoreHardwareLimits: " + IgnoreHardwareLimits + "\n";
175        ss += "    UseTimesync: " + UseTimesync + "\n";
176        return ss;
177    }
178
179    @Override
180    public StatusCode sendRequest(String network, int deviceHash)
181    {
182        return StatusCode.valueOf(ControlJNI.JNI_RequestControlDifferentialPositionDutyCycle(
183                network, deviceHash, UpdateFreqHz, TargetPosition, DifferentialPosition, EnableFOC, TargetSlot, DifferentialSlot, OverrideBrakeDurNeutral, LimitForwardMotion, LimitReverseMotion, IgnoreHardwareLimits, UseTimesync));
184    }
185
186    /**
187     * Gets information about this control request.
188     *
189     * @return Map of control parameter names and corresponding applied values
190     */
191    @Override
192    public Map<String, String> getControlInfo()
193    {
194        var controlInfo = new HashMap<String, String>();
195        controlInfo.put("Name", getName());
196        controlInfo.put("TargetPosition", String.valueOf(this.TargetPosition));
197        controlInfo.put("DifferentialPosition", String.valueOf(this.DifferentialPosition));
198        controlInfo.put("EnableFOC", String.valueOf(this.EnableFOC));
199        controlInfo.put("TargetSlot", String.valueOf(this.TargetSlot));
200        controlInfo.put("DifferentialSlot", String.valueOf(this.DifferentialSlot));
201        controlInfo.put("OverrideBrakeDurNeutral", String.valueOf(this.OverrideBrakeDurNeutral));
202        controlInfo.put("LimitForwardMotion", String.valueOf(this.LimitForwardMotion));
203        controlInfo.put("LimitReverseMotion", String.valueOf(this.LimitReverseMotion));
204        controlInfo.put("IgnoreHardwareLimits", String.valueOf(this.IgnoreHardwareLimits));
205        controlInfo.put("UseTimesync", String.valueOf(this.UseTimesync));
206        return controlInfo;
207    }
208    
209    /**
210     * Modifies this Control Request's TargetPosition parameter and returns itself for
211     * method-chaining and easier to use request API.
212     * <p>
213     * Average position to drive toward in rotations.
214     * 
215     * <ul>
216     *   <li> Units: rotations
217     * </ul>
218     * 
219     *
220     * @param newTargetPosition Parameter to modify
221     * @return Itself
222     */
223    public DifferentialPositionDutyCycle withTargetPosition(double newTargetPosition)
224    {
225        TargetPosition = newTargetPosition;
226        return this;
227    }
228    
229    /**
230     * Modifies this Control Request's TargetPosition parameter and returns itself for
231     * method-chaining and easier to use request API.
232     * <p>
233     * Average position to drive toward in rotations.
234     * 
235     * <ul>
236     *   <li> Units: rotations
237     * </ul>
238     * 
239     *
240     * @param newTargetPosition Parameter to modify
241     * @return Itself
242     */
243    public DifferentialPositionDutyCycle withTargetPosition(Angle newTargetPosition)
244    {
245        TargetPosition = newTargetPosition.in(Rotations);
246        return this;
247    }
248    
249    /**
250     * Helper method to get this Control Request's TargetPosition parameter converted
251     * to a unit type. If not using the Java units library, {@link #TargetPosition}
252     * can be accessed directly instead.
253     * <p>
254     * Average position to drive toward in rotations.
255     * 
256     * <ul>
257     *   <li> Units: rotations
258     * </ul>
259     * 
260     *
261     * @return TargetPosition
262     */
263    public Angle getTargetPositionMeasure()
264    {
265        return Rotations.of(TargetPosition);
266    }
267    
268    /**
269     * Modifies this Control Request's DifferentialPosition parameter and returns itself for
270     * method-chaining and easier to use request API.
271     * <p>
272     * Differential position to drive toward in rotations.
273     * 
274     * <ul>
275     *   <li> Units: rotations
276     * </ul>
277     * 
278     *
279     * @param newDifferentialPosition Parameter to modify
280     * @return Itself
281     */
282    public DifferentialPositionDutyCycle withDifferentialPosition(double newDifferentialPosition)
283    {
284        DifferentialPosition = newDifferentialPosition;
285        return this;
286    }
287    
288    /**
289     * Modifies this Control Request's DifferentialPosition parameter and returns itself for
290     * method-chaining and easier to use request API.
291     * <p>
292     * Differential position to drive toward in rotations.
293     * 
294     * <ul>
295     *   <li> Units: rotations
296     * </ul>
297     * 
298     *
299     * @param newDifferentialPosition Parameter to modify
300     * @return Itself
301     */
302    public DifferentialPositionDutyCycle withDifferentialPosition(Angle newDifferentialPosition)
303    {
304        DifferentialPosition = newDifferentialPosition.in(Rotations);
305        return this;
306    }
307    
308    /**
309     * Helper method to get this Control Request's DifferentialPosition parameter converted
310     * to a unit type. If not using the Java units library, {@link #DifferentialPosition}
311     * can be accessed directly instead.
312     * <p>
313     * Differential position to drive toward in rotations.
314     * 
315     * <ul>
316     *   <li> Units: rotations
317     * </ul>
318     * 
319     *
320     * @return DifferentialPosition
321     */
322    public Angle getDifferentialPositionMeasure()
323    {
324        return Rotations.of(DifferentialPosition);
325    }
326    
327    /**
328     * Modifies this Control Request's EnableFOC parameter and returns itself for
329     * method-chaining and easier to use request API.
330     * <p>
331     * Set to true to use FOC commutation (requires Phoenix Pro), which increases
332     * peak power by ~15% on supported devices (see {@link SupportsFOC}). Set to
333     * false to use trapezoidal commutation.
334     * <p>
335     * FOC improves motor performance by leveraging torque (current) control. 
336     * However, this may be inconvenient for applications that require specifying
337     * duty cycle or voltage.  CTR-Electronics has developed a hybrid method that
338     * combines the performances gains of FOC while still allowing applications to
339     * provide duty cycle or voltage demand.  This not to be confused with simple
340     * sinusoidal control or phase voltage control which lacks the performance
341     * gains.
342     *
343     * @param newEnableFOC Parameter to modify
344     * @return Itself
345     */
346    public DifferentialPositionDutyCycle withEnableFOC(boolean newEnableFOC)
347    {
348        EnableFOC = newEnableFOC;
349        return this;
350    }
351    
352    /**
353     * Modifies this Control Request's TargetSlot parameter and returns itself for
354     * method-chaining and easier to use request API.
355     * <p>
356     * Select which gains are applied to the primary controller by selecting the
357     * slot.  Use the configuration api to set the gain values for the selected slot
358     * before enabling this feature. Slot must be within [0,2].
359     *
360     * @param newTargetSlot Parameter to modify
361     * @return Itself
362     */
363    public DifferentialPositionDutyCycle withTargetSlot(int newTargetSlot)
364    {
365        TargetSlot = newTargetSlot;
366        return this;
367    }
368    
369    /**
370     * Modifies this Control Request's DifferentialSlot parameter and returns itself for
371     * method-chaining and easier to use request API.
372     * <p>
373     * Select which gains are applied to the differential controller by selecting
374     * the slot.  Use the configuration api to set the gain values for the selected
375     * slot before enabling this feature. Slot must be within [0,2].
376     *
377     * @param newDifferentialSlot Parameter to modify
378     * @return Itself
379     */
380    public DifferentialPositionDutyCycle withDifferentialSlot(int newDifferentialSlot)
381    {
382        DifferentialSlot = newDifferentialSlot;
383        return this;
384    }
385    
386    /**
387     * Modifies this Control Request's OverrideBrakeDurNeutral parameter and returns itself for
388     * method-chaining and easier to use request API.
389     * <p>
390     * Set to true to static-brake the rotor when output is zero (or within
391     * deadband).  Set to false to use the NeutralMode configuration setting
392     * (default). This flag exists to provide the fundamental behavior of this
393     * control when output is zero, which is to provide 0V to the motor.
394     *
395     * @param newOverrideBrakeDurNeutral Parameter to modify
396     * @return Itself
397     */
398    public DifferentialPositionDutyCycle withOverrideBrakeDurNeutral(boolean newOverrideBrakeDurNeutral)
399    {
400        OverrideBrakeDurNeutral = newOverrideBrakeDurNeutral;
401        return this;
402    }
403    
404    /**
405     * Modifies this Control Request's LimitForwardMotion parameter and returns itself for
406     * method-chaining and easier to use request API.
407     * <p>
408     * Set to true to force forward limiting.  This allows users to use other limit
409     * switch sensors connected to robot controller.  This also allows use of active
410     * sensors that require external power.
411     *
412     * @param newLimitForwardMotion Parameter to modify
413     * @return Itself
414     */
415    public DifferentialPositionDutyCycle withLimitForwardMotion(boolean newLimitForwardMotion)
416    {
417        LimitForwardMotion = newLimitForwardMotion;
418        return this;
419    }
420    
421    /**
422     * Modifies this Control Request's LimitReverseMotion parameter and returns itself for
423     * method-chaining and easier to use request API.
424     * <p>
425     * Set to true to force reverse limiting.  This allows users to use other limit
426     * switch sensors connected to robot controller.  This also allows use of active
427     * sensors that require external power.
428     *
429     * @param newLimitReverseMotion Parameter to modify
430     * @return Itself
431     */
432    public DifferentialPositionDutyCycle withLimitReverseMotion(boolean newLimitReverseMotion)
433    {
434        LimitReverseMotion = newLimitReverseMotion;
435        return this;
436    }
437    
438    /**
439     * Modifies this Control Request's IgnoreHardwareLimits parameter and returns itself for
440     * method-chaining and easier to use request API.
441     * <p>
442     * Set to true to ignore hardware limit switches and the LimitForwardMotion and
443     * LimitReverseMotion parameters, instead allowing motion.
444     * <p>
445     * This can be useful on mechanisms such as an intake/feeder, where a limit
446     * switch stops motion while intaking but should be ignored when feeding to a
447     * shooter.
448     * <p>
449     * The hardware limit faults and Forward/ReverseLimit signals will still report
450     * the values of the limit switches regardless of this parameter.
451     *
452     * @param newIgnoreHardwareLimits Parameter to modify
453     * @return Itself
454     */
455    public DifferentialPositionDutyCycle withIgnoreHardwareLimits(boolean newIgnoreHardwareLimits)
456    {
457        IgnoreHardwareLimits = newIgnoreHardwareLimits;
458        return this;
459    }
460    
461    /**
462     * Modifies this Control Request's UseTimesync parameter and returns itself for
463     * method-chaining and easier to use request API.
464     * <p>
465     * Set to true to delay applying this control request until a timesync boundary
466     * (requires Phoenix Pro and CANivore). This eliminates the impact of
467     * nondeterministic network delays in exchange for a larger but deterministic
468     * control latency.
469     * <p>
470     * This requires setting the ControlTimesyncFreqHz config in MotorOutputConfigs.
471     * Additionally, when this is enabled, the UpdateFreqHz of this request should
472     * be set to 0 Hz.
473     *
474     * @param newUseTimesync Parameter to modify
475     * @return Itself
476     */
477    public DifferentialPositionDutyCycle withUseTimesync(boolean newUseTimesync)
478    {
479        UseTimesync = newUseTimesync;
480        return this;
481    }
482
483    /**
484     * Sets the period at which this control will update at.
485     * This is designated in Hertz, with a minimum of 20 Hz
486     * (every 50 ms) and a maximum of 1000 Hz (every 1 ms).
487     * <p>
488     * If this field is set to 0 Hz, the control request will
489     * be sent immediately as a one-shot frame. This may be useful
490     * for advanced applications that require outputs to be
491     * synchronized with data acquisition. In this case, we
492     * recommend not exceeding 50 ms between control calls.
493     *
494     * @param newUpdateFreqHz Parameter to modify
495     * @return Itself
496     */
497    @Override
498    public DifferentialPositionDutyCycle withUpdateFreqHz(double newUpdateFreqHz)
499    {
500        UpdateFreqHz = newUpdateFreqHz;
501        return this;
502    }
503
504    /**
505     * Sets the period at which this control will update at.
506     * This is designated in Hertz, with a minimum of 20 Hz
507     * (every 50 ms) and a maximum of 1000 Hz (every 1 ms).
508     * <p>
509     * If this field is set to 0 Hz, the control request will
510     * be sent immediately as a one-shot frame. This may be useful
511     * for advanced applications that require outputs to be
512     * synchronized with data acquisition. In this case, we
513     * recommend not exceeding 50 ms between control calls.
514     *
515     * @param newUpdateFreqHz Parameter to modify
516     * @return Itself
517     */
518    @Override
519    public DifferentialPositionDutyCycle withUpdateFreqHz(Frequency newUpdateFreqHz)
520    {
521        UpdateFreqHz = newUpdateFreqHz.in(Hertz);
522        return this;
523    }
524
525    @Override
526    public DifferentialPositionDutyCycle clone()
527    {
528        try {
529            return (DifferentialPositionDutyCycle)super.clone();
530        } catch (CloneNotSupportedException ex) {
531            /* this should never happen */
532            throw new RuntimeException(ex);
533        }
534    }
535}
536