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}