001package com.ctre.phoenix.motorcontrol.can;
002import com.ctre.phoenix.ErrorCode;
003import com.ctre.phoenix.motorcontrol.IMotorControllerEnhanced;
004import com.ctre.phoenix.motorcontrol.DemandType;
005import com.ctre.phoenix.motorcontrol.FeedbackDevice;
006import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
007import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
008import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
009import com.ctre.phoenix.motorcontrol.SensorCollection;
010import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
011import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
012import com.ctre.phoenix.motorcontrol.TalonSRXControlMode;
013import com.ctre.phoenix.motorcontrol.TalonSRXFeedbackDevice;
014import com.ctre.phoenix.motorcontrol.TalonSRXSimCollection;
015import com.ctre.phoenix.motorcontrol.can.MotControllerJNI;
016import com.ctre.phoenix.ErrorCollection;
017import com.ctre.phoenix.ParamEnum;
018import com.ctre.phoenix.motorcontrol.SensorTerm;
019
020/**
021 * CTRE Talon SRX Motor Controller when used on CAN Bus.
022 *
023 * <pre>
024 * {@code
025 * // Example usage of a TalonSRX motor controller
026 * TalonSRX motor = new TalonSRX(0); // creates a new TalonSRX with ID 0
027 *
028 * TalonSRXConfiguration config = new TalonSRXConfiguration();
029 * config.peakCurrentLimit = 40; // the peak current, in amps
030 * config.peakCurrentDuration = 1500; // the time at the peak current before the limit triggers, in ms
031 * config.continuousCurrentLimit = 30; // the current to maintain if the peak limit is triggered
032 * motor.configAllSettings(config); // apply the config settings; this selects the quadrature encoder
033 *
034 * motor.set(TalonSRXControlMode.PercentOutput, 0.5); // runs the motor at 50% power
035 *
036 * System.out.println(motor.getSelectedSensorPosition()); // prints the position of the selected sensor
037 * System.out.println(motor.getSelectedSensorVelocity()); // prints the velocity recorded by the selected sensor
038 * System.out.println(motor.getMotorOutputPercent()); // prints the percent output of the motor (0.5)
039 * System.out.println(motor.getStatorCurrent()); // prints the output current of the motor
040 *
041 * ErrorCode error = motor.getLastError(); // gets the last error generated by the motor controller
042 * Faults faults = new Faults();
043 * ErrorCode faultsError = motor.getFaults(faults); // fills faults with the current motor controller faults; returns the last error generated
044 *
045 * motor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 10); // changes the period of the Status 2 frame (getSelectedSensor*()) to 10ms
046 * }
047 * </pre>
048 */
049public class TalonSRX extends com.ctre.phoenix.motorcontrol.can.BaseTalon {
050
051
052        /**
053         * Constructor for TalonSRX object
054         * @param deviceNumber CAN Device ID of Device
055         */
056        public TalonSRX(int deviceNumber) {
057                super(deviceNumber, "Talon SRX");
058        }
059
060        // ------ Set output routines. ----------//
061    /**
062     * Sets the appropriate output on the talon, depending on the mode.
063     * @param mode The output mode to apply.
064     * In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped.
065     * In Current mode, output value is in amperes.
066     * In Velocity mode, output value is in position change / 100ms.
067     * In Position mode, output value is in encoder ticks or an analog value,
068     *   depending on the sensor.
069     * In Follower mode, the output value is the integer device ID of the talon to
070     * duplicate.
071     *
072     * @param value The setpoint value, as described above.
073     *
074     *
075     *  Standard Driving Example:
076    *   _talonLeft.set(ControlMode.PercentOutput, leftJoy);
077    *   _talonRght.set(ControlMode.PercentOutput, rghtJoy);
078    */
079    public void set(TalonSRXControlMode mode, double value) {
080        super.set(mode.toControlMode(), value);
081    }
082    /**
083     * @param mode Sets the appropriate output on the talon, depending on the mode.
084     * @param demand0 The output value to apply.
085     *  such as advanced feed forward and/or auxiliary close-looping in firmware.
086     * In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped.
087     * In Current mode, output value is in amperes.
088     * In Velocity mode, output value is in position change / 100ms.
089     * In Position mode, output value is in encoder ticks or an analog value,
090     *   depending on the sensor. See
091     * In Follower mode, the output value is the integer device ID of the talon to
092     * duplicate.
093     *
094     * @param demand1Type The demand type for demand1.
095     * Neutral: Ignore demand1 and apply no change to the demand0 output.
096     * AuxPID: Use demand1 to set the target for the auxiliary PID 1.  Auxiliary
097     *   PID is always executed as standard Position PID control.
098     * ArbitraryFeedForward: Use demand1 as an arbitrary additive value to the
099     *   demand0 output.  In PercentOutput the demand0 output is the motor output,
100    *   and in closed-loop modes the demand0 output is the output of PID0.
101    * @param demand1 Supplmental output value.
102    * AuxPID: Target position in Sensor Units
103    * ArbitraryFeedForward: Percent Output between -1.0 and 1.0
104    *
105    *
106    *  Arcade Drive Example:
107    *           _talonLeft.set(ControlMode.PercentOutput, joyForward, DemandType.ArbitraryFeedForward, +joyTurn);
108    *           _talonRght.set(ControlMode.PercentOutput, joyForward, DemandType.ArbitraryFeedForward, -joyTurn);
109    *
110    *   Drive Straight Example:
111    *   Note: Selected Sensor Configuration is necessary for both PID0 and PID1.
112    *           _talonLeft.follow(_talonRght, FollwerType.AuxOutput1);
113    *           _talonRght.set(ControlMode.PercentOutput, joyForward, DemandType.AuxPID, desiredRobotHeading);
114    *
115    *   Drive Straight to a Distance Example:
116    *   Note: Other configurations (sensor selection, PID gains, etc.) need to be set.
117    *           _talonLeft.follow(_talonRght, FollwerType.AuxOutput1);
118    *           _talonRght.set(ControlMode.MotionMagic, targetDistance, DemandType.AuxPID, desiredRobotHeading);
119    */
120    public void set(TalonSRXControlMode mode, double demand0, DemandType demand1Type, double demand1) {
121        super.set(mode.toControlMode(), demand0, demand1Type, demand1);
122    }
123
124        /**
125         * @return object that can get/set individual raw sensor values.
126         */
127        public SensorCollection getSensorCollection() {
128                return super.getTalonSRXSensorCollection();
129        }
130
131        /**
132         * @return object that can get/set simulation inputs.
133         */
134        public TalonSRXSimCollection getSimCollection() {
135                return super.getTalonSRXSimCollection();
136        }
137
138        /**
139         * Select the feedback device for the motor controller.
140         *
141         * @param feedbackDevice
142         *            Talon SRX Feedback Device to select.
143         * @param pidIdx
144         *            0 for Primary closed-loop. 1 for auxiliary closed-loop.
145         * @param timeoutMs
146         *            Timeout value in ms. If nonzero, function will wait for
147         *            config success and report an error if it times out.
148         *            If zero, no blocking or checking is performed.
149         * @return Error Code generated by function. 0 indicates no error.
150         */
151        public ErrorCode configSelectedFeedbackSensor(TalonSRXFeedbackDevice feedbackDevice, int pidIdx, int timeoutMs)
152        {
153                return super.configSelectedFeedbackSensor(FeedbackDevice.valueOf(feedbackDevice.value), pidIdx, timeoutMs);
154        }
155
156
157        // ------ Current Lim ----------//
158        /**
159         * Configures the supply (input) current limit.
160         * @param currLimitConfigs  Current limit configuration
161         * @param timeoutMs
162         *            Timeout value in ms. If nonzero, function will wait for
163         *            config success and report an error if it times out.
164         *            If zero, no blocking or checking is performed.
165         * @return Error Code generated by function. 0 indicates no error.
166         */
167        public ErrorCode configSupplyCurrentLimit(SupplyCurrentLimitConfiguration currLimitConfigs, int timeoutMs){
168                ErrorCollection retval = new ErrorCollection();
169                retval.NewError(configPeakCurrentLimit((int)currLimitConfigs.triggerThresholdCurrent, timeoutMs));
170                retval.NewError(configPeakCurrentDuration((int)(1000.0 * currLimitConfigs.triggerThresholdTime), timeoutMs));
171                retval.NewError(configContinuousCurrentLimit((int)currLimitConfigs.currentLimit, timeoutMs));
172                enableCurrentLimit(currLimitConfigs.enable);
173                return retval._worstError;
174        }
175        /**
176         * Configures the supply (input) current limit.
177         * @param currLimitConfigs  Current limit configuration
178         * @return Error Code generated by function. 0 indicates no error.
179         */
180        public ErrorCode configSupplyCurrentLimit(SupplyCurrentLimitConfiguration currLimitConfigs){
181                int timeoutMs = 50;
182                return configSupplyCurrentLimit(currLimitConfigs, timeoutMs);
183        }
184
185        /**
186         * Configure the peak allowable current (when current limit is enabled).
187         *
188         * Supply current limiting is also available via configSupplyCurrentLimit(),
189         * which is a common routine with Talon FX.
190         *
191         * Current limit is activated when current exceeds the peak limit for longer
192         * than the peak duration. Then software will limit to the continuous limit.
193         * This ensures current limiting while allowing for momentary excess current
194         * events.
195         *
196         * For simpler current-limiting (single threshold) use
197         * ConfigContinuousCurrentLimit() and set the peak to zero:
198         * ConfigPeakCurrentLimit(0).
199         *
200         * @param amps
201         *            Amperes to limit.
202         * @param timeoutMs
203         *            Timeout value in ms. If nonzero, function will wait for config
204         *            success and report an error if it times out. If zero, no
205         *            blocking or checking is performed.
206     * @return Error Code generated by function. 0 indicates no error.
207         */
208        public ErrorCode configPeakCurrentLimit(int amps, int timeoutMs) {
209                int retval =  MotControllerJNI.ConfigPeakCurrentLimit(m_handle, amps, timeoutMs);
210                return ErrorCode.valueOf(retval);
211        }
212        /**
213         * Configure the peak allowable current (when current limit is enabled).
214         *
215         * Supply current limiting is also available via configSupplyCurrentLimit(),
216         * which is a common routine with Talon FX.
217         *
218         * Current limit is activated when current exceeds the peak limit for longer
219         * than the peak duration. Then software will limit to the continuous limit.
220         * This ensures current limiting while allowing for momentary excess current
221         * events.
222         *
223         * For simpler current-limiting (single threshold) use
224         * ConfigContinuousCurrentLimit() and set the peak to zero:
225         * ConfigPeakCurrentLimit(0).
226         *
227         * @param amps
228         *            Amperes to limit.
229     * @return Error Code generated by function. 0 indicates no error.
230         */
231        public ErrorCode configPeakCurrentLimit(int amps) {
232                int timeoutMs = 0;
233                return configPeakCurrentLimit( amps,  timeoutMs);
234        }
235
236        /**
237         * Configure the peak allowable duration (when current limit is enabled).
238         *
239         * Supply current limiting is also available via configSupplyCurrentLimit(),
240         * which is a common routine with Talon FX.
241         *
242         * Current limit is activated when current exceeds the peak limit for longer
243         * than the peak duration. Then software will limit to the continuous limit.
244         * This ensures current limiting while allowing for momentary excess current
245         * events.
246         *
247         * For simpler current-limiting (single threshold) use
248         * ConfigContinuousCurrentLimit() and set the peak to zero:
249         * ConfigPeakCurrentLimit(0).
250         *
251         * @param milliseconds
252         *            How long to allow current-draw past peak limit.
253         * @param timeoutMs
254         *            Timeout value in ms. If nonzero, function will wait for config
255         *            success and report an error if it times out. If zero, no
256         *            blocking or checking is performed.
257     * @return Error Code generated by function. 0 indicates no error.
258         */
259        public ErrorCode configPeakCurrentDuration(int milliseconds, int timeoutMs) {
260                int retval = MotControllerJNI.ConfigPeakCurrentDuration(m_handle, milliseconds, timeoutMs);
261                return ErrorCode.valueOf(retval);
262        }
263        /**
264         * Configure the peak allowable duration (when current limit is enabled).
265         *
266         * Supply current limiting is also available via configSupplyCurrentLimit(),
267         * which is a common routine with Talon FX.
268         *
269         * Current limit is activated when current exceeds the peak limit for longer
270         * than the peak duration. Then software will limit to the continuous limit.
271         * This ensures current limiting while allowing for momentary excess current
272         * events.
273         *
274         * For simpler current-limiting (single threshold) use
275         * ConfigContinuousCurrentLimit() and set the peak to zero:
276         * ConfigPeakCurrentLimit(0).
277         *
278         * @param milliseconds
279         *            How long to allow current-draw past peak limit.
280     * @return Error Code generated by function. 0 indicates no error.
281         */
282        public ErrorCode configPeakCurrentDuration(int milliseconds) {
283                int timeoutMs = 0;
284                return configPeakCurrentDuration( milliseconds,  timeoutMs);
285        }
286
287        /**
288         * Configure the continuous allowable current-draw (when current limit is
289         * enabled).
290         *
291         * Supply current limiting is also available via configSupplyCurrentLimit(),
292         * which is a common routine with Talon FX.
293         *
294         * Current limit is activated when current exceeds the peak limit for longer
295         * than the peak duration. Then software will limit to the continuous limit.
296         * This ensures current limiting while allowing for momentary excess current
297         * events.
298         *
299         * For simpler current-limiting (single threshold) use
300         * ConfigContinuousCurrentLimit() and set the peak to zero:
301         * ConfigPeakCurrentLimit(0).
302         *
303         * @param amps
304         *            Amperes to limit.
305         * @param timeoutMs
306         *            Timeout value in ms. If nonzero, function will wait for config
307         *            success and report an error if it times out. If zero, no
308         *            blocking or checking is performed.
309     * @return Error Code generated by function. 0 indicates no error.
310         */
311        public ErrorCode configContinuousCurrentLimit(int amps, int timeoutMs) {
312                int retval =  MotControllerJNI.ConfigContinuousCurrentLimit(m_handle, amps, timeoutMs);
313                return ErrorCode.valueOf(retval);
314        }
315        /**
316         * Configure the continuous allowable current-draw (when current limit is
317         * enabled).
318         *
319         * Supply current limiting is also available via configSupplyCurrentLimit(),
320         * which is a common routine with Talon FX.
321         *
322         * Current limit is activated when current exceeds the peak limit for longer
323         * than the peak duration. Then software will limit to the continuous limit.
324         * This ensures current limiting while allowing for momentary excess current
325         * events.
326         *
327         * For simpler current-limiting (single threshold) use
328         * ConfigContinuousCurrentLimit() and set the peak to zero:
329         * ConfigPeakCurrentLimit(0).
330         *
331         * @param amps
332         *            Amperes to limit.
333     * @return Error Code generated by function. 0 indicates no error.
334         */
335        public ErrorCode configContinuousCurrentLimit(int amps) {
336                int timeoutMs = 0;
337                return configContinuousCurrentLimit( amps,  timeoutMs);
338        }
339
340        /**
341         * Enable or disable Current Limit.
342         *
343         * Supply current limiting is also available via configSupplyCurrentLimit(),
344         * which is a common routine with Talon FX.
345         *
346         * @param enable
347         *            Enable state of current limit.
348         * @see #configPeakCurrentLimit(int,int)
349         * @see #configPeakCurrentDuration(int,int)
350         * @see #configContinuousCurrentLimit(int,int)
351         */
352        public void enableCurrentLimit(boolean enable) {
353                MotControllerJNI.EnableCurrentLimit(m_handle, enable);
354        }
355
356    /**
357     * Configures all PID set persistent settings (overloaded so timeoutMs is 50 ms
358     * and pidIdx is 0).
359     *
360         * @param pid               Object with all of the PID set persistant settings
361         * @param pidIdx            0 for Primary closed-loop. 1 for auxiliary closed-loop.
362     * @param timeoutMs
363     *              Timeout value in ms. If nonzero, function will wait for
364     *              config success and report an error if it times out.
365     *              If zero, no blocking or checking is performed.
366     *
367     * @return Error Code generated by function. 0 indicates no error.
368     */
369        protected ErrorCode configurePID(TalonSRXPIDSetConfiguration pid, int pidIdx, int timeoutMs) {
370        return super.configurePID(pid, pidIdx, timeoutMs, false);
371
372
373        }
374    /**
375     * Configures all PID set persistent settings (overloaded so timeoutMs is 50 ms
376     * and pidIdx is 0).
377     *
378         * @param pid               Object with all of the PID set persistant settings
379     *
380     * @return Error Code generated by function. 0 indicates no error.
381     */
382        protected ErrorCode configurePID(TalonSRXPIDSetConfiguration pid) {
383        return super.configurePID(pid);
384    }
385
386    /**
387     * Gets all PID set persistant settings.
388     *
389         * @param pid               Object with all of the PID set persistant settings
390         * @param pidIdx            0 for Primary closed-loop. 1 for auxiliary closed-loop.
391     * @param timeoutMs
392     *              Timeout value in ms. If nonzero, function will wait for
393     *              config success and report an error if it times out.
394     *              If zero, no blocking or checking is performed.
395     */
396    public void getPIDConfigs(TalonSRXPIDSetConfiguration pid, int pidIdx, int timeoutMs){
397        super.getPIDConfigs(pid, pidIdx, timeoutMs);
398    }
399    /**
400     * Gets all PID set persistant settings (overloaded so timeoutMs is 50 ms
401     * and pidIdx is 0).
402     *
403         * @param pid               Object with all of the PID set persistant settings
404     */
405        public void getPIDConfigs(TalonSRXPIDSetConfiguration pid) {
406        int pidIdx = 0;
407        int timeoutMs = 50;
408        getPIDConfigs(pid, pidIdx, timeoutMs);
409    }
410
411
412
413    /**
414     * Configures all persistent settings.
415     *
416         * @param allConfigs        Object with all of the persistant settings
417     * @param timeoutMs
418     *              Timeout value in ms. If nonzero, function will wait for
419     *              config success and report an error if it times out.
420     *              If zero, no blocking or checking is performed.
421     *
422     * @return Error Code generated by function. 0 indicates no error.
423     */
424        public ErrorCode configAllSettings(TalonSRXConfiguration allConfigs, int timeoutMs) {
425        ErrorCollection errorCollection = new ErrorCollection();
426
427        errorCollection.NewError(super.configAllSettings(allConfigs, timeoutMs));
428
429                if(TalonSRXConfigUtil.peakCurrentLimitDifferent(allConfigs)) errorCollection.NewError(configPeakCurrentLimit(allConfigs.peakCurrentLimit, timeoutMs));
430                if(TalonSRXConfigUtil.peakCurrentDurationDifferent(allConfigs)) errorCollection.NewError(configPeakCurrentDuration(allConfigs.peakCurrentDuration, timeoutMs));
431                if(TalonSRXConfigUtil.continuousCurrentLimitDifferent(allConfigs)) errorCollection.NewError(configContinuousCurrentLimit(allConfigs.continuousCurrentLimit, timeoutMs));
432
433        return errorCollection._worstError;
434
435        }
436
437    /**
438     * Configures all persistent settings (overloaded so timeoutMs is 50 ms).
439     *
440         * @param allConfigs        Object with all of the persistant settings
441     *
442     * @return Error Code generated by function. 0 indicates no error.
443     */
444        public ErrorCode configAllSettings(TalonSRXConfiguration allConfigs) {
445                int timeoutMs = 50;
446                return configAllSettings(allConfigs, timeoutMs);
447        }
448
449    /**
450     * Gets all persistant settings.
451     *
452         * @param allConfigs        Object with all of the persistant settings
453     * @param timeoutMs
454     *              Timeout value in ms. If nonzero, function will wait for
455     *              config success and report an error if it times out.
456     *              If zero, no blocking or checking is performed.
457     */
458    public void getAllConfigs(TalonSRXConfiguration allConfigs, int timeoutMs) {
459
460        super.getAllConfigs(allConfigs, timeoutMs);
461
462        allConfigs.peakCurrentLimit        = (int) configGetParameter(ParamEnum.ePeakCurrentLimitAmps, 0, timeoutMs);
463        allConfigs.peakCurrentDuration     = (int) configGetParameter(ParamEnum.ePeakCurrentLimitMs, 0, timeoutMs);
464        allConfigs.continuousCurrentLimit  = (int) configGetParameter(ParamEnum.eContinuousCurrentLimitAmps, 0, timeoutMs);
465
466    }
467    /**
468     * Gets all persistant settings (overloaded so timeoutMs is 50 ms).
469     *
470         * @param allConfigs        Object with all of the persistant settings
471     */
472    public void getAllConfigs(TalonSRXConfiguration allConfigs) {
473        int timeoutMs = 50;
474        getAllConfigs(allConfigs, timeoutMs);
475    }
476
477
478
479}