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