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