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