001package com.ctre.phoenix.motorcontrol.can;
002
003import com.ctre.phoenix.motorcontrol.IMotorController;
004import com.ctre.phoenix.motorcontrol.DemandType;
005import com.ctre.phoenix.motorcontrol.FeedbackDevice;
006import com.ctre.phoenix.ErrorCode;
007import com.ctre.phoenix.ErrorCollection;
008import com.ctre.phoenix.ParamEnum;
009import com.ctre.phoenix.motorcontrol.SensorTerm;
010import com.ctre.phoenix.motorcontrol.VictorSPXControlMode;
011import com.ctre.phoenix.motorcontrol.VictorSPXSimCollection;
012import com.ctre.phoenix.motorcontrol.RemoteFeedbackDevice;
013import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
014import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource;
015
016//import edu.wpi.first.wpilibj.hal.HAL;
017/**
018 * VEX Victor SPX Motor Controller when used on CAN Bus.
019 *
020 * <pre>
021 * {@code
022 * // Example usage of a VictorSPX motor controller
023 * VictorSPX motor = new VictorSPX(0); // creates a new VictorSPX with ID 0
024 *
025 * motor.set(VictorSPXControlMode.PercentOutput, 0.5); // runs the motor at 50% power
026 *
027 * System.out.println(motor.getMotorOutputPercent()); // prints the percent output of the motor (0.5)
028 * System.out.println(motor.getBusVoltage()); // prints the bus voltage seen by the motor controller
029 *
030 * ErrorCode error = motor.getLastError(); // gets the last error generated by the motor controller
031 * Faults faults = new Faults();
032 * ErrorCode faultsError = motor.getFaults(faults); // fills faults with the current motor controller faults; returns the last error generated
033 * }
034 * </pre>
035 */
036public class VictorSPX extends com.ctre.phoenix.motorcontrol.can.BaseMotorController
037    implements IMotorController {
038                
039        /**
040         * Constructor
041         * 
042         * @param deviceNumber
043         *            [0,62]
044         */
045        public VictorSPX(int deviceNumber) {
046                super(deviceNumber, "Victor SPX");
047    }
048    
049    // ------ Set output routines. ----------//
050    /**
051     * Sets the appropriate output on the motor controller, depending on the mode.
052     * @param mode The output mode to apply.
053     * In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped.
054     * In Velocity mode, output value is in position change / 100ms.
055     * In Position mode, output value is in encoder ticks or an analog value,
056     *   depending on the sensor.
057     * In Follower mode, the output value is the integer device ID of the motor controller to duplicate.
058     *
059     * @param value The setpoint value, as described above.
060     *
061     *
062     *  Standard Driving Example:
063    *   _victorLeft.set(ControlMode.PercentOutput, leftJoy);
064    *   _victorRght.set(ControlMode.PercentOutput, rghtJoy);
065    */
066    public void set(VictorSPXControlMode mode, double value) {
067        super.set(mode.toControlMode(), value);
068    }
069    /**
070     * @param mode Sets the appropriate output on the motor controller, depending on the mode.
071     * @param demand0 The output value to apply.
072     *  such as advanced feed forward and/or auxiliary close-looping in firmware.
073     * In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped.
074     * In Velocity mode, output value is in position change / 100ms.
075     * In Position mode, output value is in encoder ticks or an analog value,
076     *   depending on the sensor. See
077     * In Follower mode, the output value is the integer device ID of the motor controller to
078     * duplicate.
079     *
080     * @param demand1Type The demand type for demand1.
081     * Neutral: Ignore demand1 and apply no change to the demand0 output.
082     * AuxPID: Use demand1 to set the target for the auxiliary PID 1.  Auxiliary
083     *   PID is always executed as standard Position PID control.
084     * ArbitraryFeedForward: Use demand1 as an arbitrary additive value to the
085     *   demand0 output.  In PercentOutput the demand0 output is the motor output,
086    *   and in closed-loop modes the demand0 output is the output of PID0.
087    * @param demand1 Supplmental output value.
088    * AuxPID: Target position in Sensor Units
089    * ArbitraryFeedForward: Percent Output between -1.0 and 1.0
090    *
091    *
092    *  Arcade Drive Example:
093    *           _victorLeft.set(ControlMode.PercentOutput, joyForward, DemandType.ArbitraryFeedForward, +joyTurn);
094    *           _victorRght.set(ControlMode.PercentOutput, joyForward, DemandType.ArbitraryFeedForward, -joyTurn);
095    *
096    *   Drive Straight Example:
097    *   Note: Selected Sensor Configuration is necessary for both PID0 and PID1.
098    *           _victorLeft.follow(_victorRght, FollwerType.AuxOutput1);
099    *           _victorRght.set(ControlMode.PercentOutput, joyForward, DemandType.AuxPID, desiredRobotHeading);
100    *
101    *   Drive Straight to a Distance Example:
102    *   Note: Other configurations (sensor selection, PID gains, etc.) need to be set.
103    *           _victorLeft.follow(_victorRght, FollwerType.AuxOutput1);
104    *           _victorRght.set(ControlMode.MotionMagic, targetDistance, DemandType.AuxPID, desiredRobotHeading);
105    */
106    public void set(VictorSPXControlMode mode, double demand0, DemandType demand1Type, double demand1) {
107        super.set(mode.toControlMode(), demand0, demand1Type, demand1);
108    }
109
110        /**
111         * @return object that can get/set simulation inputs.
112         */
113        public VictorSPXSimCollection getSimCollection() {
114                return super.getVictorSPXSimCollection();
115        }
116        
117    /**
118     * Configures all PID set persistent settings (overloaded so timeoutMs is 50 ms
119     * and pidIdx is 0).
120     *
121         * @param pid               Object with all of the PID set persistant settings
122         * @param pidIdx            0 for Primary closed-loop. 1 for auxiliary closed-loop.
123     * @param timeoutMs
124     *              Timeout value in ms. If nonzero, function will wait for
125     *              config success and report an error if it times out.
126     *              If zero, no blocking or checking is performed.
127     *
128     * @return Error Code generated by function. 0 indicates no error. 
129     */
130        ErrorCode configurePID(VictorSPXPIDSetConfiguration pid, int pidIdx, int timeoutMs, boolean enableOptimizations) {
131        ErrorCollection errorCollection = new ErrorCollection();
132        
133        //------ sensor selection ----------//      
134                
135                if(VictorSPXPIDSetConfigUtil.selectedFeedbackCoefficientDifferent(pid) || !enableOptimizations)
136            errorCollection.NewError(configSelectedFeedbackCoefficient(pid.selectedFeedbackCoefficient, pidIdx, timeoutMs));
137        
138        /* This is ignored because Victor's firmware default value is impossible to set in API */
139                //if(VictorSPXPIDSetConfigUtil.selectedFeedbackSensorDifferent(pid) || !enableOptimizations)
140        errorCollection.NewError(configSelectedFeedbackSensor(pid.selectedFeedbackSensor, pidIdx, timeoutMs));
141        
142
143        return errorCollection._worstError;
144
145        }
146
147    
148    /**
149     * Configures all PID set persistent settings (overloaded so timeoutMs is 50 ms
150     * and pidIdx is 0).
151     *
152         * @param pid               Object with all of the PID set persistant settings
153     *
154     * @return Error Code generated by function. 0 indicates no error. 
155     */
156        ErrorCode configurePID(VictorSPXPIDSetConfiguration pid) {
157        int pidIdx = 0;
158        int timeoutMs = 50;
159        return configurePID(pid, pidIdx, timeoutMs, true);
160    }
161
162
163    /**
164     * Gets all PID set persistant settings.
165     *
166         * @param pid               Object with all of the PID set persistant settings
167         * @param pidIdx            0 for Primary closed-loop. 1 for auxiliary closed-loop.
168     * @param timeoutMs
169     *              Timeout value in ms. If nonzero, function will wait for
170     *              config success and report an error if it times out.
171     *              If zero, no blocking or checking is performed.
172     */
173    public void getPIDConfigs(VictorSPXPIDSetConfiguration pid, int pidIdx, int timeoutMs)
174    {
175        baseGetPIDConfigs(pid, pidIdx, timeoutMs);
176        pid.selectedFeedbackSensor = RemoteFeedbackDevice.valueOf(configGetParameter(ParamEnum.eFeedbackSensorType, pidIdx, timeoutMs));
177    
178    }
179    /**
180     * Gets all PID set persistant settings (overloaded so timeoutMs is 50 ms
181     * and pidIdx is 0).
182     *
183         * @param pid               Object with all of the PID set persistant settings
184     */
185        public void getPIDConfigs(VictorSPXPIDSetConfiguration pid) {
186        int pidIdx = 0;
187        int timeoutMs = 50;
188        getPIDConfigs(pid, pidIdx, timeoutMs);
189    }
190
191    /**
192     * Configures all persistent settings.
193     *
194         * @param allConfigs        Object with all of the persistant settings
195     * @param timeoutMs
196     *              Timeout value in ms. If nonzero, function will wait for
197     *              config success and report an error if it times out.
198     *              If zero, no blocking or checking is performed.
199     *
200     * @return Error Code generated by function. 0 indicates no error. 
201     */
202        public ErrorCode configAllSettings(VictorSPXConfiguration allConfigs, int timeoutMs) {
203        
204        ErrorCollection errorCollection = new ErrorCollection();
205        
206        errorCollection.NewError(baseConfigAllSettings(allConfigs, timeoutMs));
207        
208
209        //--------PIDs---------------//
210
211                errorCollection.NewError(configurePID(allConfigs.primaryPID, 0, timeoutMs, allConfigs.enableOptimizations));
212                errorCollection.NewError(configurePID(allConfigs.auxiliaryPID, 1, timeoutMs, allConfigs.enableOptimizations));
213                
214                if(VictorConfigUtil.forwardLimitSwitchDifferent(allConfigs)) 
215                        errorCollection.NewError(MotControllerJNI.ConfigForwardLimitSwitchSource(m_handle, allConfigs.forwardLimitSwitchSource.value,
216                                allConfigs.forwardLimitSwitchNormal.value, allConfigs.forwardLimitSwitchDeviceID, timeoutMs));
217                if(VictorConfigUtil.reverseLimitSwitchDifferent(allConfigs)) 
218                        errorCollection.NewError(MotControllerJNI.ConfigReverseLimitSwitchSource(m_handle, allConfigs.reverseLimitSwitchSource.value,
219                                allConfigs.reverseLimitSwitchNormal.value, allConfigs.reverseLimitSwitchDeviceID, timeoutMs));
220                
221                if(VictorConfigUtil.sum0TermDifferent(allConfigs)) errorCollection.NewError(configSensorTerm(SensorTerm.Sum0, allConfigs.sum0Term, timeoutMs));
222                if(VictorConfigUtil.sum1TermDifferent(allConfigs)) errorCollection.NewError(configSensorTerm(SensorTerm.Sum1, allConfigs.sum1Term, timeoutMs));
223                if(VictorConfigUtil.diff0TermDifferent(allConfigs)) errorCollection.NewError(configSensorTerm(SensorTerm.Diff0, allConfigs.diff0Term, timeoutMs));
224                if(VictorConfigUtil.diff1TermDifferent(allConfigs)) errorCollection.NewError(configSensorTerm(SensorTerm.Diff1, allConfigs.diff1Term, timeoutMs));
225
226        return errorCollection._worstError;
227
228        }
229    /**
230     * Configures all persistent settings (overloaded so timeoutMs is 50 ms).
231     *
232         * @param allConfigs        Object with all of the persistant settings
233     *
234     * @return Error Code generated by function. 0 indicates no error. 
235     */
236        public ErrorCode configAllSettings(VictorSPXConfiguration allConfigs) {
237                int timeoutMs = 50;
238                return configAllSettings(allConfigs, timeoutMs);
239        }       
240    /**
241     * Gets all persistant settings.
242     *
243         * @param allConfigs        Object with all of the persistant settings
244     * @param timeoutMs
245     *              Timeout value in ms. If nonzero, function will wait for
246     *              config success and report an error if it times out.
247     *              If zero, no blocking or checking is performed.
248     */
249    public void getAllConfigs(VictorSPXConfiguration allConfigs, int timeoutMs) {
250    
251        baseGetAllConfigs(allConfigs, timeoutMs);
252    
253        getPIDConfigs(allConfigs.primaryPID, 0, timeoutMs);
254        getPIDConfigs(allConfigs.auxiliaryPID, 1, timeoutMs);
255        allConfigs.sum0Term =  RemoteFeedbackDevice.valueOf(configGetParameter(ParamEnum.eSensorTerm, 0, timeoutMs));
256        allConfigs.sum1Term =  RemoteFeedbackDevice.valueOf(configGetParameter(ParamEnum.eSensorTerm, 1, timeoutMs));
257        allConfigs.diff0Term = RemoteFeedbackDevice.valueOf(configGetParameter(ParamEnum.eSensorTerm, 2, timeoutMs));
258        allConfigs.diff1Term = RemoteFeedbackDevice.valueOf(configGetParameter(ParamEnum.eSensorTerm, 3, timeoutMs));
259    
260        allConfigs.forwardLimitSwitchSource = RemoteLimitSwitchSource.valueOf(configGetParameter(ParamEnum.eLimitSwitchSource, 0, timeoutMs));
261        allConfigs.reverseLimitSwitchSource = RemoteLimitSwitchSource.valueOf(configGetParameter(ParamEnum.eLimitSwitchSource, 1, timeoutMs));
262        allConfigs.forwardLimitSwitchDeviceID = (int) configGetParameter(ParamEnum.eLimitSwitchRemoteDevID, 0, timeoutMs);
263        allConfigs.reverseLimitSwitchDeviceID = (int) configGetParameter(ParamEnum.eLimitSwitchRemoteDevID, 1, timeoutMs);
264        allConfigs.forwardLimitSwitchNormal = LimitSwitchNormal.valueOf(configGetParameter(ParamEnum.eLimitSwitchNormClosedAndDis, 0, timeoutMs));
265        allConfigs.reverseLimitSwitchNormal = LimitSwitchNormal.valueOf(configGetParameter(ParamEnum.eLimitSwitchNormClosedAndDis, 1, timeoutMs));
266    
267    }
268    /**
269     * Gets all persistant settings (overloaded so timeoutMs is 50 ms).
270     *
271         * @param allConfigs        Object with all of the persistant settings
272     */
273    public void getAllConfigs(VictorSPXConfiguration allConfigs) {
274        int timeoutMs = 0;
275        getAllConfigs(allConfigs, timeoutMs); 
276    }
277
278
279}