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 * Requests Motion Magic® to target a final velocity using a motion profile. 
022 * This allows smooth transitions between velocity set points.  Users can
023 * optionally provide a voltage feedforward.
024 * <p>
025 * Motion Magic® Velocity produces a motion profile in real-time while attempting to honor the specified
026 * Acceleration and (optional) Jerk.  This control mode does not use the CruiseVelocity, Expo_kV, or Expo_kA
027 * configs.
028 * <p>
029 * If the specified acceleration is zero, the Acceleration under Motion Magic® configuration parameter is used
030 * instead.  This allows for runtime adjustment of acceleration for advanced users.  Jerk is also specified in
031 * the Motion Magic® persistent configuration values.  If Jerk is set to zero, Motion Magic® will produce a
032 * trapezoidal acceleration profile.
033 * <p>
034 * Target velocity can also be changed on-the-fly and Motion Magic® will do its best to adjust the profile. 
035 * This control mode is voltage-based, so relevant closed-loop gains will use Volts for the numerator.
036 */
037public class MotionMagicVelocityVoltage extends ControlRequest implements Cloneable
038{
039    /**
040     * Target velocity to drive toward in rotations per second.  This can be changed
041     * on-the fly.
042     * 
043     * <ul>
044     *   <li> Units: rotations per second
045     * </ul>
046     * 
047     */
048    public double Velocity;
049    /**
050     * This is the absolute Acceleration to use generating the profile.  If this
051     * parameter is zero, the Acceleration persistent configuration parameter is
052     * used instead. Acceleration is in rotations per second squared.  If nonzero,
053     * the signage does not matter as the absolute value is used.
054     * 
055     * <ul>
056     *   <li> Units: rotations per second²
057     * </ul>
058     * 
059     */
060    public double Acceleration = 0.0;
061    /**
062     * Set to true to use FOC commutation (requires Phoenix Pro), which increases
063     * peak power by ~15% on supported devices (see {@link SupportsFOC}). Set to
064     * false to use trapezoidal commutation.
065     * <p>
066     * FOC improves motor performance by leveraging torque (current) control. 
067     * However, this may be inconvenient for applications that require specifying
068     * duty cycle or voltage.  CTR-Electronics has developed a hybrid method that
069     * combines the performances gains of FOC while still allowing applications to
070     * provide duty cycle or voltage demand.  This not to be confused with simple
071     * sinusoidal control or phase voltage control which lacks the performance
072     * gains.
073     */
074    public boolean EnableFOC = true;
075    /**
076     * Feedforward to apply in volts
077     * 
078     * <ul>
079     *   <li> Units: Volts
080     * </ul>
081     * 
082     */
083    public double FeedForward = 0.0;
084    /**
085     * Select which gains are applied by selecting the slot.  Use the configuration
086     * api to set the gain values for the selected slot before enabling this
087     * feature. Slot must be within [0,2].
088     */
089    public int Slot = 0;
090    /**
091     * Set to true to static-brake the rotor when output is zero (or within
092     * deadband).  Set to false to use the NeutralMode configuration setting
093     * (default). This flag exists to provide the fundamental behavior of this
094     * control when output is zero, which is to provide 0V to the motor.
095     */
096    public boolean OverrideBrakeDurNeutral = false;
097    /**
098     * Set to true to force forward limiting.  This allows users to use other limit
099     * switch sensors connected to robot controller.  This also allows use of active
100     * sensors that require external power.
101     */
102    public boolean LimitForwardMotion = false;
103    /**
104     * Set to true to force reverse limiting.  This allows users to use other limit
105     * switch sensors connected to robot controller.  This also allows use of active
106     * sensors that require external power.
107     */
108    public boolean LimitReverseMotion = false;
109    /**
110     * Set to true to ignore hardware limit switches and the LimitForwardMotion and
111     * LimitReverseMotion parameters, instead allowing motion.
112     * <p>
113     * This can be useful on mechanisms such as an intake/feeder, where a limit
114     * switch stops motion while intaking but should be ignored when feeding to a
115     * shooter.
116     * <p>
117     * The hardware limit faults and Forward/ReverseLimit signals will still report
118     * the values of the limit switches regardless of this parameter.
119     */
120    public boolean IgnoreHardwareLimits = false;
121    /**
122     * Set to true to delay applying this control request until a timesync boundary
123     * (requires Phoenix Pro and CANivore). This eliminates the impact of
124     * nondeterministic network delays in exchange for a larger but deterministic
125     * control latency.
126     * <p>
127     * This requires setting the ControlTimesyncFreqHz config in MotorOutputConfigs.
128     * Additionally, when this is enabled, the UpdateFreqHz of this request should
129     * be set to 0 Hz.
130     */
131    public boolean UseTimesync = false;
132
133    /**
134     * The period at which this control will update at.
135     * This is designated in Hertz, with a minimum of 20 Hz
136     * (every 50 ms) and a maximum of 1000 Hz (every 1 ms).
137     * <p>
138     * If this field is set to 0 Hz, the control request will
139     * be sent immediately as a one-shot frame. This may be useful
140     * for advanced applications that require outputs to be
141     * synchronized with data acquisition. In this case, we
142     * recommend not exceeding 50 ms between control calls.
143     */
144    public double UpdateFreqHz = 100;
145
146    /**
147     * Requests Motion Magic® to target a final velocity using a motion profile. 
148     * This allows smooth transitions between velocity set points.  Users can
149     * optionally provide a voltage feedforward.
150     * <p>
151     * Motion Magic® Velocity produces a motion profile in real-time while
152     * attempting to honor the specified Acceleration and (optional) Jerk.  This
153     * control mode does not use the CruiseVelocity, Expo_kV, or Expo_kA configs.
154     * <p>
155     * If the specified acceleration is zero, the Acceleration under Motion Magic®
156     * configuration parameter is used instead.  This allows for runtime adjustment
157     * of acceleration for advanced users.  Jerk is also specified in the Motion
158     * Magic® persistent configuration values.  If Jerk is set to zero, Motion
159     * Magic® will produce a trapezoidal acceleration profile.
160     * <p>
161     * Target velocity can also be changed on-the-fly and Motion Magic® will do its
162     * best to adjust the profile.  This control mode is voltage-based, so relevant
163     * closed-loop gains will use Volts for the numerator.
164     * 
165     * @param Velocity    Target velocity to drive toward in rotations per second. 
166     *                    This can be changed on-the fly.
167     */
168    public MotionMagicVelocityVoltage(double Velocity)
169    {
170        super("MotionMagicVelocityVoltage");
171        this.Velocity = Velocity;
172    }
173
174    /**
175     * Requests Motion Magic® to target a final velocity using a motion profile. 
176     * This allows smooth transitions between velocity set points.  Users can
177     * optionally provide a voltage feedforward.
178     * <p>
179     * Motion Magic® Velocity produces a motion profile in real-time while
180     * attempting to honor the specified Acceleration and (optional) Jerk.  This
181     * control mode does not use the CruiseVelocity, Expo_kV, or Expo_kA configs.
182     * <p>
183     * If the specified acceleration is zero, the Acceleration under Motion Magic®
184     * configuration parameter is used instead.  This allows for runtime adjustment
185     * of acceleration for advanced users.  Jerk is also specified in the Motion
186     * Magic® persistent configuration values.  If Jerk is set to zero, Motion
187     * Magic® will produce a trapezoidal acceleration profile.
188     * <p>
189     * Target velocity can also be changed on-the-fly and Motion Magic® will do its
190     * best to adjust the profile.  This control mode is voltage-based, so relevant
191     * closed-loop gains will use Volts for the numerator.
192     * 
193     * @param Velocity    Target velocity to drive toward in rotations per second. 
194     *                    This can be changed on-the fly.
195     */
196    public MotionMagicVelocityVoltage(AngularVelocity Velocity)
197    {
198        this(Velocity.in(RotationsPerSecond));
199    }
200
201    @Override
202    public String toString()
203    {
204        String ss = "Control: MotionMagicVelocityVoltage\n";
205        ss += "    Velocity: " + Velocity + " rotations per second" + "\n";
206        ss += "    Acceleration: " + Acceleration + " rotations per second²" + "\n";
207        ss += "    EnableFOC: " + EnableFOC + "\n";
208        ss += "    FeedForward: " + FeedForward + " Volts" + "\n";
209        ss += "    Slot: " + Slot + "\n";
210        ss += "    OverrideBrakeDurNeutral: " + OverrideBrakeDurNeutral + "\n";
211        ss += "    LimitForwardMotion: " + LimitForwardMotion + "\n";
212        ss += "    LimitReverseMotion: " + LimitReverseMotion + "\n";
213        ss += "    IgnoreHardwareLimits: " + IgnoreHardwareLimits + "\n";
214        ss += "    UseTimesync: " + UseTimesync + "\n";
215        return ss;
216    }
217
218    @Override
219    public StatusCode sendRequest(String network, int deviceHash)
220    {
221        return StatusCode.valueOf(ControlJNI.JNI_RequestControlMotionMagicVelocityVoltage(
222                network, deviceHash, UpdateFreqHz, Velocity, Acceleration, EnableFOC, FeedForward, Slot, OverrideBrakeDurNeutral, LimitForwardMotion, LimitReverseMotion, IgnoreHardwareLimits, UseTimesync));
223    }
224
225    /**
226     * Gets information about this control request.
227     *
228     * @return Map of control parameter names and corresponding applied values
229     */
230    @Override
231    public Map<String, String> getControlInfo()
232    {
233        var controlInfo = new HashMap<String, String>();
234        controlInfo.put("Name", getName());
235        controlInfo.put("Velocity", String.valueOf(this.Velocity));
236        controlInfo.put("Acceleration", String.valueOf(this.Acceleration));
237        controlInfo.put("EnableFOC", String.valueOf(this.EnableFOC));
238        controlInfo.put("FeedForward", String.valueOf(this.FeedForward));
239        controlInfo.put("Slot", String.valueOf(this.Slot));
240        controlInfo.put("OverrideBrakeDurNeutral", String.valueOf(this.OverrideBrakeDurNeutral));
241        controlInfo.put("LimitForwardMotion", String.valueOf(this.LimitForwardMotion));
242        controlInfo.put("LimitReverseMotion", String.valueOf(this.LimitReverseMotion));
243        controlInfo.put("IgnoreHardwareLimits", String.valueOf(this.IgnoreHardwareLimits));
244        controlInfo.put("UseTimesync", String.valueOf(this.UseTimesync));
245        return controlInfo;
246    }
247    
248    /**
249     * Modifies this Control Request's Velocity parameter and returns itself for
250     * method-chaining and easier to use request API.
251     * <p>
252     * Target velocity to drive toward in rotations per second.  This can be changed
253     * on-the fly.
254     * 
255     * <ul>
256     *   <li> Units: rotations per second
257     * </ul>
258     * 
259     *
260     * @param newVelocity Parameter to modify
261     * @return Itself
262     */
263    public MotionMagicVelocityVoltage withVelocity(double newVelocity)
264    {
265        Velocity = newVelocity;
266        return this;
267    }
268    
269    /**
270     * Modifies this Control Request's Velocity parameter and returns itself for
271     * method-chaining and easier to use request API.
272     * <p>
273     * Target velocity to drive toward in rotations per second.  This can be changed
274     * on-the fly.
275     * 
276     * <ul>
277     *   <li> Units: rotations per second
278     * </ul>
279     * 
280     *
281     * @param newVelocity Parameter to modify
282     * @return Itself
283     */
284    public MotionMagicVelocityVoltage withVelocity(AngularVelocity newVelocity)
285    {
286        Velocity = newVelocity.in(RotationsPerSecond);
287        return this;
288    }
289    
290    /**
291     * Helper method to get this Control Request's Velocity parameter converted
292     * to a unit type. If not using the Java units library, {@link #Velocity}
293     * can be accessed directly instead.
294     * <p>
295     * Target velocity to drive toward in rotations per second.  This can be changed
296     * on-the fly.
297     * 
298     * <ul>
299     *   <li> Units: rotations per second
300     * </ul>
301     * 
302     *
303     * @return Velocity
304     */
305    public AngularVelocity getVelocityMeasure()
306    {
307        return RotationsPerSecond.of(Velocity);
308    }
309    
310    /**
311     * Modifies this Control Request's Acceleration parameter and returns itself for
312     * method-chaining and easier to use request API.
313     * <p>
314     * This is the absolute Acceleration to use generating the profile.  If this
315     * parameter is zero, the Acceleration persistent configuration parameter is
316     * used instead. Acceleration is in rotations per second squared.  If nonzero,
317     * the signage does not matter as the absolute value is used.
318     * 
319     * <ul>
320     *   <li> Units: rotations per second²
321     * </ul>
322     * 
323     *
324     * @param newAcceleration Parameter to modify
325     * @return Itself
326     */
327    public MotionMagicVelocityVoltage withAcceleration(double newAcceleration)
328    {
329        Acceleration = newAcceleration;
330        return this;
331    }
332    
333    /**
334     * Modifies this Control Request's Acceleration parameter and returns itself for
335     * method-chaining and easier to use request API.
336     * <p>
337     * This is the absolute Acceleration to use generating the profile.  If this
338     * parameter is zero, the Acceleration persistent configuration parameter is
339     * used instead. Acceleration is in rotations per second squared.  If nonzero,
340     * the signage does not matter as the absolute value is used.
341     * 
342     * <ul>
343     *   <li> Units: rotations per second²
344     * </ul>
345     * 
346     *
347     * @param newAcceleration Parameter to modify
348     * @return Itself
349     */
350    public MotionMagicVelocityVoltage withAcceleration(AngularAcceleration newAcceleration)
351    {
352        Acceleration = newAcceleration.in(RotationsPerSecondPerSecond);
353        return this;
354    }
355    
356    /**
357     * Helper method to get this Control Request's Acceleration parameter converted
358     * to a unit type. If not using the Java units library, {@link #Acceleration}
359     * can be accessed directly instead.
360     * <p>
361     * This is the absolute Acceleration to use generating the profile.  If this
362     * parameter is zero, the Acceleration persistent configuration parameter is
363     * used instead. Acceleration is in rotations per second squared.  If nonzero,
364     * the signage does not matter as the absolute value is used.
365     * 
366     * <ul>
367     *   <li> Units: rotations per second²
368     * </ul>
369     * 
370     *
371     * @return Acceleration
372     */
373    public AngularAcceleration getAccelerationMeasure()
374    {
375        return RotationsPerSecondPerSecond.of(Acceleration);
376    }
377    
378    /**
379     * Modifies this Control Request's EnableFOC parameter and returns itself for
380     * method-chaining and easier to use request API.
381     * <p>
382     * Set to true to use FOC commutation (requires Phoenix Pro), which increases
383     * peak power by ~15% on supported devices (see {@link SupportsFOC}). Set to
384     * false to use trapezoidal commutation.
385     * <p>
386     * FOC improves motor performance by leveraging torque (current) control. 
387     * However, this may be inconvenient for applications that require specifying
388     * duty cycle or voltage.  CTR-Electronics has developed a hybrid method that
389     * combines the performances gains of FOC while still allowing applications to
390     * provide duty cycle or voltage demand.  This not to be confused with simple
391     * sinusoidal control or phase voltage control which lacks the performance
392     * gains.
393     *
394     * @param newEnableFOC Parameter to modify
395     * @return Itself
396     */
397    public MotionMagicVelocityVoltage withEnableFOC(boolean newEnableFOC)
398    {
399        EnableFOC = newEnableFOC;
400        return this;
401    }
402    
403    /**
404     * Modifies this Control Request's FeedForward parameter and returns itself for
405     * method-chaining and easier to use request API.
406     * <p>
407     * Feedforward to apply in volts
408     * 
409     * <ul>
410     *   <li> Units: Volts
411     * </ul>
412     * 
413     *
414     * @param newFeedForward Parameter to modify
415     * @return Itself
416     */
417    public MotionMagicVelocityVoltage withFeedForward(double newFeedForward)
418    {
419        FeedForward = newFeedForward;
420        return this;
421    }
422    
423    /**
424     * Modifies this Control Request's FeedForward parameter and returns itself for
425     * method-chaining and easier to use request API.
426     * <p>
427     * Feedforward to apply in volts
428     * 
429     * <ul>
430     *   <li> Units: Volts
431     * </ul>
432     * 
433     *
434     * @param newFeedForward Parameter to modify
435     * @return Itself
436     */
437    public MotionMagicVelocityVoltage withFeedForward(Voltage newFeedForward)
438    {
439        FeedForward = newFeedForward.in(Volts);
440        return this;
441    }
442    
443    /**
444     * Helper method to get this Control Request's FeedForward parameter converted
445     * to a unit type. If not using the Java units library, {@link #FeedForward}
446     * can be accessed directly instead.
447     * <p>
448     * Feedforward to apply in volts
449     * 
450     * <ul>
451     *   <li> Units: Volts
452     * </ul>
453     * 
454     *
455     * @return FeedForward
456     */
457    public Voltage getFeedForwardMeasure()
458    {
459        return Volts.of(FeedForward);
460    }
461    
462    /**
463     * Modifies this Control Request's Slot parameter and returns itself for
464     * method-chaining and easier to use request API.
465     * <p>
466     * Select which gains are applied by selecting the slot.  Use the configuration
467     * api to set the gain values for the selected slot before enabling this
468     * feature. Slot must be within [0,2].
469     *
470     * @param newSlot Parameter to modify
471     * @return Itself
472     */
473    public MotionMagicVelocityVoltage withSlot(int newSlot)
474    {
475        Slot = newSlot;
476        return this;
477    }
478    
479    /**
480     * Modifies this Control Request's OverrideBrakeDurNeutral parameter and returns itself for
481     * method-chaining and easier to use request API.
482     * <p>
483     * Set to true to static-brake the rotor when output is zero (or within
484     * deadband).  Set to false to use the NeutralMode configuration setting
485     * (default). This flag exists to provide the fundamental behavior of this
486     * control when output is zero, which is to provide 0V to the motor.
487     *
488     * @param newOverrideBrakeDurNeutral Parameter to modify
489     * @return Itself
490     */
491    public MotionMagicVelocityVoltage withOverrideBrakeDurNeutral(boolean newOverrideBrakeDurNeutral)
492    {
493        OverrideBrakeDurNeutral = newOverrideBrakeDurNeutral;
494        return this;
495    }
496    
497    /**
498     * Modifies this Control Request's LimitForwardMotion parameter and returns itself for
499     * method-chaining and easier to use request API.
500     * <p>
501     * Set to true to force forward limiting.  This allows users to use other limit
502     * switch sensors connected to robot controller.  This also allows use of active
503     * sensors that require external power.
504     *
505     * @param newLimitForwardMotion Parameter to modify
506     * @return Itself
507     */
508    public MotionMagicVelocityVoltage withLimitForwardMotion(boolean newLimitForwardMotion)
509    {
510        LimitForwardMotion = newLimitForwardMotion;
511        return this;
512    }
513    
514    /**
515     * Modifies this Control Request's LimitReverseMotion parameter and returns itself for
516     * method-chaining and easier to use request API.
517     * <p>
518     * Set to true to force reverse limiting.  This allows users to use other limit
519     * switch sensors connected to robot controller.  This also allows use of active
520     * sensors that require external power.
521     *
522     * @param newLimitReverseMotion Parameter to modify
523     * @return Itself
524     */
525    public MotionMagicVelocityVoltage withLimitReverseMotion(boolean newLimitReverseMotion)
526    {
527        LimitReverseMotion = newLimitReverseMotion;
528        return this;
529    }
530    
531    /**
532     * Modifies this Control Request's IgnoreHardwareLimits parameter and returns itself for
533     * method-chaining and easier to use request API.
534     * <p>
535     * Set to true to ignore hardware limit switches and the LimitForwardMotion and
536     * LimitReverseMotion parameters, instead allowing motion.
537     * <p>
538     * This can be useful on mechanisms such as an intake/feeder, where a limit
539     * switch stops motion while intaking but should be ignored when feeding to a
540     * shooter.
541     * <p>
542     * The hardware limit faults and Forward/ReverseLimit signals will still report
543     * the values of the limit switches regardless of this parameter.
544     *
545     * @param newIgnoreHardwareLimits Parameter to modify
546     * @return Itself
547     */
548    public MotionMagicVelocityVoltage withIgnoreHardwareLimits(boolean newIgnoreHardwareLimits)
549    {
550        IgnoreHardwareLimits = newIgnoreHardwareLimits;
551        return this;
552    }
553    
554    /**
555     * Modifies this Control Request's UseTimesync parameter and returns itself for
556     * method-chaining and easier to use request API.
557     * <p>
558     * Set to true to delay applying this control request until a timesync boundary
559     * (requires Phoenix Pro and CANivore). This eliminates the impact of
560     * nondeterministic network delays in exchange for a larger but deterministic
561     * control latency.
562     * <p>
563     * This requires setting the ControlTimesyncFreqHz config in MotorOutputConfigs.
564     * Additionally, when this is enabled, the UpdateFreqHz of this request should
565     * be set to 0 Hz.
566     *
567     * @param newUseTimesync Parameter to modify
568     * @return Itself
569     */
570    public MotionMagicVelocityVoltage withUseTimesync(boolean newUseTimesync)
571    {
572        UseTimesync = newUseTimesync;
573        return this;
574    }
575
576    /**
577     * Sets the period at which this control will update at.
578     * This is designated in Hertz, with a minimum of 20 Hz
579     * (every 50 ms) and a maximum of 1000 Hz (every 1 ms).
580     * <p>
581     * If this field is set to 0 Hz, the control request will
582     * be sent immediately as a one-shot frame. This may be useful
583     * for advanced applications that require outputs to be
584     * synchronized with data acquisition. In this case, we
585     * recommend not exceeding 50 ms between control calls.
586     *
587     * @param newUpdateFreqHz Parameter to modify
588     * @return Itself
589     */
590    @Override
591    public MotionMagicVelocityVoltage withUpdateFreqHz(double newUpdateFreqHz)
592    {
593        UpdateFreqHz = newUpdateFreqHz;
594        return this;
595    }
596
597    /**
598     * Sets the period at which this control will update at.
599     * This is designated in Hertz, with a minimum of 20 Hz
600     * (every 50 ms) and a maximum of 1000 Hz (every 1 ms).
601     * <p>
602     * If this field is set to 0 Hz, the control request will
603     * be sent immediately as a one-shot frame. This may be useful
604     * for advanced applications that require outputs to be
605     * synchronized with data acquisition. In this case, we
606     * recommend not exceeding 50 ms between control calls.
607     *
608     * @param newUpdateFreqHz Parameter to modify
609     * @return Itself
610     */
611    @Override
612    public MotionMagicVelocityVoltage withUpdateFreqHz(Frequency newUpdateFreqHz)
613    {
614        UpdateFreqHz = newUpdateFreqHz.in(Hertz);
615        return this;
616    }
617
618    @Override
619    public MotionMagicVelocityVoltage clone()
620    {
621        try {
622            return (MotionMagicVelocityVoltage)super.clone();
623        } catch (CloneNotSupportedException ex) {
624            /* this should never happen */
625            throw new RuntimeException(ex);
626        }
627    }
628}
629