001/* Copyright (C) Cross The Road Electronics 2024 */
002package com.ctre.phoenix.motorcontrol.can;
003
004import com.ctre.phoenix.CustomParamConfiguration;
005import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
006import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
007import com.ctre.phoenix.motorcontrol.can.SlotConfiguration;
008import com.ctre.phoenix.sensors.SensorVelocityMeasPeriod;
009import com.ctre.phoenix.motorcontrol.can.FilterConfiguration;
010
011/**
012 * Configurables available to base motor controllers
013 */
014public abstract class BaseMotorControllerConfiguration extends CustomParamConfiguration{
015    /**
016     * Seconds to go from 0 to full in open loop
017     */
018    public double openloopRamp;
019    /**
020     * Seconds to go from 0 to full in closed loop
021     */
022    public double closedloopRamp;
023    /**
024     * Peak output in forward direction [0,1]
025     */
026    public double peakOutputForward;
027    /**
028     * Peak output in reverse direction [-1,0]
029     */
030    public double peakOutputReverse;
031    /**
032     * Nominal/Minimum output in forward direction [0,1]
033     */
034    public double nominalOutputForward;
035    /**
036     * Nominal/Minimum output in reverse direction [-1,0]
037     */
038    public double nominalOutputReverse;
039    /**
040     * Neutral deadband [0.001, 0.25]
041     */
042    public double neutralDeadband;
043    /**
044         * This is the max voltage to apply to the hbridge when voltage
045         * compensation is enabled.  For example, if 10 (volts) is specified
046         * and a TalonSRX is commanded to 0.5 (PercentOutput, closed-loop, etc)
047         * then the TalonSRX will attempt to apply a duty-cycle to produce 5V.
048     */
049    public double voltageCompSaturation;
050    /**
051     * Number of samples in rolling average for voltage
052     */
053    public int voltageMeasurementFilter;
054    /**
055     * Desired period for velocity measurement
056     */
057    public SensorVelocityMeasPeriod velocityMeasurementPeriod;
058    /**
059     * Desired window for velocity measurement
060     */
061    public int velocityMeasurementWindow;
062    /**
063     * Threshold for soft limits in forward direction (in raw sensor units)
064     */
065    public double forwardSoftLimitThreshold;
066    /**
067     * Threshold for soft limits in reverse direction (in raw sensor units)
068     */
069    public double reverseSoftLimitThreshold;
070    /**
071     * Enable forward soft limit
072     */
073    public boolean forwardSoftLimitEnable;
074    /**
075     * Enable reverse soft limit
076     */
077    public boolean reverseSoftLimitEnable;
078    /**
079     * Configuration for slot 0
080     */
081    public SlotConfiguration slot0;
082    /**
083     * Configuration for slot 1
084     */
085    public SlotConfiguration slot1;
086    /**
087     * Configuration for slot 2
088     */
089    public SlotConfiguration slot2;
090    /**
091     * Configuration for slot 3
092     */
093    public SlotConfiguration slot3;
094    /**
095     * PID polarity inversion
096     * 
097         * Standard Polarity:
098         *    Primary Output = PID0 + PID1,
099         *    Auxiliary Output = PID0 - PID1,
100         *
101         * Inverted Polarity:
102         *    Primary Output = PID0 - PID1,
103         *    Auxiliary Output = PID0 + PID1,
104     */
105    public boolean auxPIDPolarity;
106    /**
107     * Configuration for RemoteFilter 0
108     */
109    public FilterConfiguration remoteFilter0;
110    /**
111     * Configuration for RemoteFilter 1
112     */
113    public FilterConfiguration remoteFilter1;
114    /**
115     * Motion Magic cruise velocity in raw sensor units per 100 ms.
116     */
117    public double motionCruiseVelocity;
118    /**
119     * Motion Magic acceleration in (raw sensor units per 100 ms) per second.
120     */
121    public double motionAcceleration;
122    /**
123         * Zero to use trapezoidal motion during motion magic.  [1,8] for S-Curve, higher value for greater smoothing.
124         */
125        public int motionCurveStrength;
126    /**
127     * Motion profile base trajectory period in milliseconds.
128     * 
129     * The period specified in a trajectory point will be
130     * added on to this value
131     */
132    public int motionProfileTrajectoryPeriod;
133    /**
134     * Determine whether feedback sensor is continuous or not
135     */
136    public boolean feedbackNotContinuous;
137    /**
138     * Disable neutral'ing the motor when remote sensor is lost on CAN bus
139     */
140    public boolean remoteSensorClosedLoopDisableNeutralOnLOS;
141    /**
142     * Clear the position on forward limit
143     */
144    public boolean clearPositionOnLimitF;
145    /**
146     * Clear the position on reverse limit
147     */
148    public boolean clearPositionOnLimitR;
149    /**
150     * Clear the position on index
151     */
152    public boolean clearPositionOnQuadIdx;
153    /**
154     * Disable neutral'ing the motor when remote limit switch is lost on CAN bus
155     */
156    public boolean limitSwitchDisableNeutralOnLOS;
157    /**
158     * Disable neutral'ing the motor when remote soft limit is lost on CAN bus
159     */
160    public boolean softLimitDisableNeutralOnLOS;
161    /**
162     * Number of edges per rotation for a tachometer sensor
163     */
164    public int pulseWidthPeriod_EdgesPerRot;
165    /**
166     * Desired window size for a tachometer sensor
167     */
168    public int pulseWidthPeriod_FilterWindowSz;
169    /**
170     * Enable motion profile trajectory point interpolation (defaults to true).
171     */
172    public boolean trajectoryInterpolationEnable;
173
174        public BaseMotorControllerConfiguration() { 
175        slot0 = new SlotConfiguration();
176        slot1 = new SlotConfiguration();
177        slot2 = new SlotConfiguration();
178        slot3 = new SlotConfiguration();
179
180        remoteFilter0 = new FilterConfiguration();
181        remoteFilter1 = new FilterConfiguration();
182
183        openloopRamp = 0.0;
184        closedloopRamp = 0.0;
185        peakOutputForward = 1.0;
186        peakOutputReverse = -1.0;
187        nominalOutputForward = 0.0;
188        nominalOutputReverse = 0.0;
189        neutralDeadband = 0.04;
190        voltageCompSaturation = 0.0;
191        voltageMeasurementFilter = 32;
192        velocityMeasurementPeriod = SensorVelocityMeasPeriod.Period_100Ms;
193        velocityMeasurementWindow = 64;
194        forwardSoftLimitThreshold = 0;
195        reverseSoftLimitThreshold = 0;
196        forwardSoftLimitEnable = false;
197        reverseSoftLimitEnable = false;
198        auxPIDPolarity = false;
199        motionCruiseVelocity = 0;
200        motionAcceleration = 0;
201        motionCurveStrength = 0;
202        motionProfileTrajectoryPeriod = 0;
203        feedbackNotContinuous = false;
204        remoteSensorClosedLoopDisableNeutralOnLOS = false;
205        clearPositionOnLimitF = false;
206        clearPositionOnLimitR = false;
207        clearPositionOnQuadIdx = false;
208        limitSwitchDisableNeutralOnLOS = false;
209        softLimitDisableNeutralOnLOS = false;
210        pulseWidthPeriod_EdgesPerRot = 1;
211        pulseWidthPeriod_FilterWindowSz = 1;
212        trajectoryInterpolationEnable = true;
213
214
215        }
216
217    /**
218     * @return String representation of configs
219     */
220        public String toString() {
221                return toString("");
222        }
223
224    /**
225     * @param prependString
226     *              String to prepend to configs
227     * @return String representation of configs
228     */
229    public String toString(String prependString) {
230
231        String retstr = prependString + ".openloopRamp = " + String.valueOf(openloopRamp) + ";\n";
232        retstr += prependString + ".closedloopRamp = " + String.valueOf(closedloopRamp) + ";\n";
233        retstr += prependString + ".peakOutputForward = " + String.valueOf(peakOutputForward) + ";\n";
234        retstr += prependString + ".peakOutputReverse = " + String.valueOf(peakOutputReverse) + ";\n";
235        retstr += prependString + ".nominalOutputForward = " + String.valueOf(nominalOutputForward) + ";\n";
236        retstr += prependString + ".nominalOutputReverse = " + String.valueOf(nominalOutputReverse) + ";\n";
237        retstr += prependString + ".neutralDeadband = " + String.valueOf(neutralDeadband) + ";\n";
238        retstr += prependString + ".voltageCompSaturation = " + String.valueOf(voltageCompSaturation) + ";\n";
239        retstr += prependString + ".voltageMeasurementFilter = " + String.valueOf(voltageMeasurementFilter) + ";\n";
240        retstr += prependString + ".velocityMeasurementPeriod = " + velocityMeasurementPeriod.toString() + ";\n";
241        retstr += prependString + ".velocityMeasurementWindow = " + String.valueOf(velocityMeasurementWindow) + ";\n";
242        retstr += prependString + ".forwardSoftLimitThreshold = " + String.valueOf(forwardSoftLimitThreshold) + ";\n";
243        retstr += prependString + ".reverseSoftLimitThreshold = " + String.valueOf(reverseSoftLimitThreshold) + ";\n";
244        retstr += prependString + ".forwardSoftLimitEnable = " + String.valueOf(forwardSoftLimitEnable) + ";\n";
245        retstr += prependString + ".reverseSoftLimitEnable = " + String.valueOf(reverseSoftLimitEnable) + ";\n";
246        retstr += slot0.toString(prependString + ".slot0");
247        retstr += slot1.toString(prependString + ".slot1");
248        retstr += slot2.toString(prependString + ".slot2");
249        retstr += slot3.toString(prependString + ".slot3");
250        retstr += prependString + ".auxPIDPolarity = " + String.valueOf(auxPIDPolarity) + ";\n";
251        retstr += remoteFilter0.toString(prependString + ".filter0");
252        retstr += remoteFilter1.toString(prependString + ".filter1");
253        retstr += prependString + ".motionCruiseVelocity = " + String.valueOf(motionCruiseVelocity) + ";\n";
254        retstr += prependString + ".motionAcceleration = " + String.valueOf(motionAcceleration) + ";\n";
255        retstr += prependString + ".motionCurveStrength = " + String.valueOf(motionCurveStrength) + ";\n";
256        retstr += prependString + ".motionProfileTrajectoryPeriod = " + String.valueOf(motionProfileTrajectoryPeriod) + ";\n";
257        retstr += prependString + ".feedbackNotContinuous = " + String.valueOf(feedbackNotContinuous) + ";\n";
258        retstr += prependString + ".remoteSensorClosedLoopDisableNeutralOnLOS = " + String.valueOf(remoteSensorClosedLoopDisableNeutralOnLOS) + ";\n";
259        retstr += prependString + ".clearPositionOnLimitF = " + String.valueOf(clearPositionOnLimitF) + ";\n";
260        retstr += prependString + ".clearPositionOnLimitR = " + String.valueOf(clearPositionOnLimitR) + ";\n";
261        retstr += prependString + ".clearPositionOnQuadIdx = " + String.valueOf(clearPositionOnQuadIdx) + ";\n";
262        retstr += prependString + ".limitSwitchDisableNeutralOnLOS = " + String.valueOf(limitSwitchDisableNeutralOnLOS) + ";\n";
263        retstr += prependString + ".softLimitDisableNeutralOnLOS = " + String.valueOf(softLimitDisableNeutralOnLOS) + ";\n";
264        retstr += prependString + ".pulseWidthPeriod_EdgesPerRot = " + String.valueOf(pulseWidthPeriod_EdgesPerRot) + ";\n";
265        retstr += prependString + ".pulseWidthPeriod_FilterWindowSz = " + String.valueOf(pulseWidthPeriod_FilterWindowSz) + ";\n";
266        retstr += prependString + ".trajectoryInterpolationEnable = " + String.valueOf(trajectoryInterpolationEnable) + ";\n";
267
268        retstr += super.toString(prependString);
269
270        return retstr;
271    }
272
273
274                
275}