001/* 002 * Copyright (C) Cross The Road Electronics. All rights reserved. 003 * License information can be found in CTRE_LICENSE.txt 004 * For support and suggestions contact support@ctr-electronics.com or file 005 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases 006 */ 007package com.ctre.phoenix6.swerve; 008 009import static edu.wpi.first.units.Units.*; 010 011import java.util.HashMap; 012 013import com.ctre.phoenix6.StatusCode; 014import com.ctre.phoenix6.controls.CoastOut; 015import com.ctre.phoenix6.controls.PositionVoltage; 016import com.ctre.phoenix6.controls.VoltageOut; 017import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveControlParameters; 018import com.ctre.phoenix6.swerve.jni.SwerveJNI; 019import com.ctre.phoenix6.swerve.utility.PhoenixPIDController; 020 021import edu.wpi.first.math.geometry.Rotation2d; 022import edu.wpi.first.math.geometry.Translation2d; 023import edu.wpi.first.math.kinematics.ChassisSpeeds; 024import edu.wpi.first.math.kinematics.SwerveDriveKinematics; 025import edu.wpi.first.units.measure.*; 026 027/** 028 * Container for all the Swerve Requests. Use this to find all applicable swerve 029 * drive requests. 030 * <p> 031 * This is also an interface common to all swerve drive control requests that 032 * allow the request to calculate the state to apply to the modules. 033 */ 034public interface SwerveRequest { 035 /** 036 * In field-centric control, the direction of "forward" is sometimes different 037 * depending on perspective. This addresses which forward to use. 038 */ 039 public enum ForwardPerspectiveValue { 040 /** 041 * "Forward" (positive X) is determined from the operator's perspective. This 042 * is important for most teleop driven field-centric requests, where positive 043 * X means to drive away from the operator. 044 * <p> 045 * Important: Users must specify the OperatorPerspective in the SwerveDrivetrain object 046 */ 047 OperatorPerspective(0), 048 /** 049 * "Forward" (positive X) is always from the perspective of the blue alliance (i.e. towards 050 * the red alliance). This is important in situations such as path following where positive 051 * X is always from the blue alliance perspective, regardless of where the operator is 052 * physically located. 053 */ 054 BlueAlliance(1); 055 056 public final int value; 057 058 ForwardPerspectiveValue(int initValue) { 059 this.value = initValue; 060 } 061 062 private static HashMap<Integer, ForwardPerspectiveValue> _map = null; 063 static { 064 _map = new HashMap<Integer, ForwardPerspectiveValue>(); 065 for (ForwardPerspectiveValue type : ForwardPerspectiveValue.values()) { 066 _map.put(type.value, type); 067 } 068 } 069 070 /** 071 * Gets ForwardPerspectiveValue from specified value 072 * 073 * @param value Value of ForwardPerspectiveValue 074 * @return ForwardPerspectiveValue of specified value 075 */ 076 public static ForwardPerspectiveValue valueOf(int value) { 077 ForwardPerspectiveValue retval = _map.get(value); 078 if (retval != null) 079 return retval; 080 return ForwardPerspectiveValue.values()[0]; 081 } 082 } 083 084 /** 085 * Applies this swerve request to the given modules. This is 086 * typically called by the SwerveDrivetrain odometry thread. 087 * <p> 088 * For native swerve requests, this API can be called from a 089 * non-native request's {@code apply} to compose the two together. 090 * 091 * @param parameters Parameters the control request needs to calculate the module state 092 * @param modulesToApply Modules to which the control request is applied 093 * @return Status code of sending the request 094 */ 095 public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply); 096 097 /** Swerve requests implemented in native code. */ 098 public interface NativeSwerveRequest extends SwerveRequest { 099 /** 100 * Applies a native swerve request to the native drivetrain with the provided ID. 101 * <p> 102 * When this is implemented, the regular {@link #apply} function should do nothing 103 * (return OK). Additionally, this cannot be called from another swerve request's 104 * apply method, as this overrides the native swerve request of the drivetrain. 105 * <p> 106 * Unlike {@link #apply}, this function is called every time {@link SwerveDrivetrain#setControl} 107 * is run, not every loop of the odometry thread. Instead, the underlying native 108 * request is run at the full update frequency of the odometry thread. 109 * 110 * @param id ID of the native swerve drivetrain 111 */ 112 public void applyNative(int id); 113 } 114 115 116 /** 117 * Does nothing to the swerve module state. This is the default state of a newly 118 * created swerve drive mechanism. 119 */ 120 public static class Idle implements NativeSwerveRequest { 121 122 123 124 public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) { 125 return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_Idle(parameters.drivetrainId)); 126 } 127 128 public void applyNative(int id) { 129 SwerveJNI.JNI_SetControl_Idle(id); 130 } 131 } 132 133 /** 134 * Sets the swerve drive module states to point inward on the robot in an "X" 135 * fashion, creating a natural brake which will oppose any motion. 136 */ 137 public static class SwerveDriveBrake implements NativeSwerveRequest { 138 /** 139 * The type of control request to use for the drive motor. 140 */ 141 public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage; 142 /** 143 * The type of control request to use for the drive motor. 144 */ 145 public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position; 146 147 /** 148 * Modifies the DriveRequestType parameter and returns itself. 149 * <p> 150 * The type of control request to use for the drive motor. 151 * 152 * @param newDriveRequestType Parameter to modify 153 * @return this object 154 */ 155 public SwerveDriveBrake withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) { 156 this.DriveRequestType = newDriveRequestType; 157 return this; 158 } 159 160 /** 161 * Modifies the SteerRequestType parameter and returns itself. 162 * <p> 163 * The type of control request to use for the drive motor. 164 * 165 * @param newSteerRequestType Parameter to modify 166 * @return this object 167 */ 168 public SwerveDriveBrake withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) { 169 this.SteerRequestType = newSteerRequestType; 170 return this; 171 } 172 173 public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) { 174 return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_SwerveDriveBrake(parameters.drivetrainId, 175 DriveRequestType.value, 176 SteerRequestType.value)); 177 } 178 179 public void applyNative(int id) { 180 SwerveJNI.JNI_SetControl_SwerveDriveBrake(id, 181 DriveRequestType.value, 182 SteerRequestType.value); 183 } 184 } 185 186 /** 187 * Drives the swerve drivetrain in a field-centric manner. 188 * <p> 189 * When users use this request, they specify the direction the robot should 190 * travel oriented against the field, and the rate at which their robot should 191 * rotate about the center of the robot. 192 * <p> 193 * An example scenario is that the robot is oriented to the east, the VelocityX 194 * is +5 m/s, VelocityY is 0 m/s, and RotationRate is 0.5 rad/s. In this 195 * scenario, the robot would drive northward at 5 m/s and turn counterclockwise 196 * at 0.5 rad/s. 197 */ 198 public static class FieldCentric implements NativeSwerveRequest { 199 /** 200 * The velocity in the X direction, in m/s. X is defined as forward according to 201 * WPILib convention, so this determines how fast to travel forward. 202 */ 203 public double VelocityX = 0; 204 /** 205 * The velocity in the Y direction, in m/s. Y is defined as to the left 206 * according to WPILib convention, so this determines how fast to travel to the 207 * left. 208 */ 209 public double VelocityY = 0; 210 /** 211 * The angular rate to rotate at, in radians per second. Angular rate is defined 212 * as counterclockwise positive, so this determines how fast to turn 213 * counterclockwise. 214 */ 215 public double RotationalRate = 0; 216 /** 217 * The allowable deadband of the request, in m/s. 218 */ 219 public double Deadband = 0; 220 /** 221 * The rotational deadband of the request, in radians per second. 222 */ 223 public double RotationalDeadband = 0; 224 /** 225 * The center of rotation the robot should rotate around. This is (0,0) by 226 * default, which will rotate around the center of the robot. 227 */ 228 public Translation2d CenterOfRotation = new Translation2d(); 229 /** 230 * The type of control request to use for the drive motor. 231 */ 232 public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage; 233 /** 234 * The type of control request to use for the drive motor. 235 */ 236 public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position; 237 /** 238 * Whether to desaturate wheel speeds before applying. For more information, see 239 * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}. 240 */ 241 public boolean DesaturateWheelSpeeds = true; 242 /** 243 * The perspective to use when determining which direction is forward. 244 */ 245 public ForwardPerspectiveValue ForwardPerspective = ForwardPerspectiveValue.OperatorPerspective; 246 247 /** 248 * Modifies the VelocityX parameter and returns itself. 249 * <p> 250 * The velocity in the X direction, in m/s. X is defined as forward according to 251 * WPILib convention, so this determines how fast to travel forward. 252 * 253 * @param newVelocityX Parameter to modify 254 * @return this object 255 */ 256 public FieldCentric withVelocityX(double newVelocityX) { 257 this.VelocityX = newVelocityX; 258 return this; 259 } 260 261 /** 262 * Modifies the VelocityX parameter and returns itself. 263 * <p> 264 * The velocity in the X direction, in m/s. X is defined as forward according to 265 * WPILib convention, so this determines how fast to travel forward. 266 * 267 * @param newVelocityX Parameter to modify 268 * @return this object 269 */ 270 public FieldCentric withVelocityX(LinearVelocity newVelocityX) { 271 this.VelocityX = newVelocityX.in(MetersPerSecond); 272 return this; 273 } 274 275 /** 276 * Modifies the VelocityY parameter and returns itself. 277 * <p> 278 * The velocity in the Y direction, in m/s. Y is defined as to the left 279 * according to WPILib convention, so this determines how fast to travel to the 280 * left. 281 * 282 * @param newVelocityY Parameter to modify 283 * @return this object 284 */ 285 public FieldCentric withVelocityY(double newVelocityY) { 286 this.VelocityY = newVelocityY; 287 return this; 288 } 289 290 /** 291 * Modifies the VelocityY parameter and returns itself. 292 * <p> 293 * The velocity in the Y direction, in m/s. Y is defined as to the left 294 * according to WPILib convention, so this determines how fast to travel to the 295 * left. 296 * 297 * @param newVelocityY Parameter to modify 298 * @return this object 299 */ 300 public FieldCentric withVelocityY(LinearVelocity newVelocityY) { 301 this.VelocityY = newVelocityY.in(MetersPerSecond); 302 return this; 303 } 304 305 /** 306 * Modifies the RotationalRate parameter and returns itself. 307 * <p> 308 * The angular rate to rotate at, in radians per second. Angular rate is defined 309 * as counterclockwise positive, so this determines how fast to turn 310 * counterclockwise. 311 * 312 * @param newRotationalRate Parameter to modify 313 * @return this object 314 */ 315 public FieldCentric withRotationalRate(double newRotationalRate) { 316 this.RotationalRate = newRotationalRate; 317 return this; 318 } 319 320 /** 321 * Modifies the RotationalRate parameter and returns itself. 322 * <p> 323 * The angular rate to rotate at, in radians per second. Angular rate is defined 324 * as counterclockwise positive, so this determines how fast to turn 325 * counterclockwise. 326 * 327 * @param newRotationalRate Parameter to modify 328 * @return this object 329 */ 330 public FieldCentric withRotationalRate(AngularVelocity newRotationalRate) { 331 this.RotationalRate = newRotationalRate.in(RadiansPerSecond); 332 return this; 333 } 334 335 /** 336 * Modifies the Deadband parameter and returns itself. 337 * <p> 338 * The allowable deadband of the request, in m/s. 339 * 340 * @param newDeadband Parameter to modify 341 * @return this object 342 */ 343 public FieldCentric withDeadband(double newDeadband) { 344 this.Deadband = newDeadband; 345 return this; 346 } 347 348 /** 349 * Modifies the Deadband parameter and returns itself. 350 * <p> 351 * The allowable deadband of the request, in m/s. 352 * 353 * @param newDeadband Parameter to modify 354 * @return this object 355 */ 356 public FieldCentric withDeadband(LinearVelocity newDeadband) { 357 this.Deadband = newDeadband.in(MetersPerSecond); 358 return this; 359 } 360 361 /** 362 * Modifies the RotationalDeadband parameter and returns itself. 363 * <p> 364 * The rotational deadband of the request, in radians per second. 365 * 366 * @param newRotationalDeadband Parameter to modify 367 * @return this object 368 */ 369 public FieldCentric withRotationalDeadband(double newRotationalDeadband) { 370 this.RotationalDeadband = newRotationalDeadband; 371 return this; 372 } 373 374 /** 375 * Modifies the RotationalDeadband parameter and returns itself. 376 * <p> 377 * The rotational deadband of the request, in radians per second. 378 * 379 * @param newRotationalDeadband Parameter to modify 380 * @return this object 381 */ 382 public FieldCentric withRotationalDeadband(AngularVelocity newRotationalDeadband) { 383 this.RotationalDeadband = newRotationalDeadband.in(RadiansPerSecond); 384 return this; 385 } 386 387 /** 388 * Modifies the CenterOfRotation parameter and returns itself. 389 * <p> 390 * The center of rotation the robot should rotate around. This is (0,0) by 391 * default, which will rotate around the center of the robot. 392 * 393 * @param newCenterOfRotation Parameter to modify 394 * @return this object 395 */ 396 public FieldCentric withCenterOfRotation(Translation2d newCenterOfRotation) { 397 this.CenterOfRotation = newCenterOfRotation; 398 return this; 399 } 400 401 /** 402 * Modifies the DriveRequestType parameter and returns itself. 403 * <p> 404 * The type of control request to use for the drive motor. 405 * 406 * @param newDriveRequestType Parameter to modify 407 * @return this object 408 */ 409 public FieldCentric withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) { 410 this.DriveRequestType = newDriveRequestType; 411 return this; 412 } 413 414 /** 415 * Modifies the SteerRequestType parameter and returns itself. 416 * <p> 417 * The type of control request to use for the drive motor. 418 * 419 * @param newSteerRequestType Parameter to modify 420 * @return this object 421 */ 422 public FieldCentric withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) { 423 this.SteerRequestType = newSteerRequestType; 424 return this; 425 } 426 427 /** 428 * Modifies the DesaturateWheelSpeeds parameter and returns itself. 429 * <p> 430 * Whether to desaturate wheel speeds before applying. For more information, see 431 * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}. 432 * 433 * @param newDesaturateWheelSpeeds Parameter to modify 434 * @return this object 435 */ 436 public FieldCentric withDesaturateWheelSpeeds(boolean newDesaturateWheelSpeeds) { 437 this.DesaturateWheelSpeeds = newDesaturateWheelSpeeds; 438 return this; 439 } 440 441 /** 442 * Modifies the ForwardPerspective parameter and returns itself. 443 * <p> 444 * The perspective to use when determining which direction is forward. 445 * 446 * @param newForwardPerspective Parameter to modify 447 * @return this object 448 */ 449 public FieldCentric withForwardPerspective(ForwardPerspectiveValue newForwardPerspective) { 450 this.ForwardPerspective = newForwardPerspective; 451 return this; 452 } 453 454 public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) { 455 return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_FieldCentric(parameters.drivetrainId, 456 VelocityX, 457 VelocityY, 458 RotationalRate, 459 Deadband, 460 RotationalDeadband, 461 CenterOfRotation.getX(), 462 CenterOfRotation.getY(), 463 DriveRequestType.value, 464 SteerRequestType.value, 465 DesaturateWheelSpeeds, 466 ForwardPerspective.value)); 467 } 468 469 public void applyNative(int id) { 470 SwerveJNI.JNI_SetControl_FieldCentric(id, 471 VelocityX, 472 VelocityY, 473 RotationalRate, 474 Deadband, 475 RotationalDeadband, 476 CenterOfRotation.getX(), 477 CenterOfRotation.getY(), 478 DriveRequestType.value, 479 SteerRequestType.value, 480 DesaturateWheelSpeeds, 481 ForwardPerspective.value); 482 } 483 } 484 485 /** 486 * Drives the swerve drivetrain in a robot-centric manner. 487 * <p> 488 * When users use this request, they specify the direction the robot should 489 * travel oriented against the robot itself, and the rate at which their robot 490 * should rotate about the center of the robot. 491 * <p> 492 * An example scenario is that the robot is oriented to the east, the VelocityX 493 * is +5 m/s, VelocityY is 0 m/s, and RotationRate is 0.5 rad/s. In this 494 * scenario, the robot would drive eastward at 5 m/s and turn counterclockwise 495 * at 0.5 rad/s. 496 */ 497 public static class RobotCentric implements NativeSwerveRequest { 498 /** 499 * The velocity in the X direction, in m/s. X is defined as forward according to 500 * WPILib convention, so this determines how fast to travel forward. 501 */ 502 public double VelocityX = 0; 503 /** 504 * The velocity in the Y direction, in m/s. Y is defined as to the left 505 * according to WPILib convention, so this determines how fast to travel to the 506 * left. 507 */ 508 public double VelocityY = 0; 509 /** 510 * The angular rate to rotate at, in radians per second. Angular rate is defined 511 * as counterclockwise positive, so this determines how fast to turn 512 * counterclockwise. 513 */ 514 public double RotationalRate = 0; 515 /** 516 * The allowable deadband of the request, in m/s. 517 */ 518 public double Deadband = 0; 519 /** 520 * The rotational deadband of the request, in radians per second. 521 */ 522 public double RotationalDeadband = 0; 523 /** 524 * The center of rotation the robot should rotate around. This is (0,0) by 525 * default, which will rotate around the center of the robot. 526 */ 527 public Translation2d CenterOfRotation = new Translation2d(); 528 /** 529 * The type of control request to use for the drive motor. 530 */ 531 public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage; 532 /** 533 * The type of control request to use for the drive motor. 534 */ 535 public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position; 536 /** 537 * Whether to desaturate wheel speeds before applying. For more information, see 538 * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}. 539 */ 540 public boolean DesaturateWheelSpeeds = true; 541 542 /** 543 * Modifies the VelocityX parameter and returns itself. 544 * <p> 545 * The velocity in the X direction, in m/s. X is defined as forward according to 546 * WPILib convention, so this determines how fast to travel forward. 547 * 548 * @param newVelocityX Parameter to modify 549 * @return this object 550 */ 551 public RobotCentric withVelocityX(double newVelocityX) { 552 this.VelocityX = newVelocityX; 553 return this; 554 } 555 556 /** 557 * Modifies the VelocityX parameter and returns itself. 558 * <p> 559 * The velocity in the X direction, in m/s. X is defined as forward according to 560 * WPILib convention, so this determines how fast to travel forward. 561 * 562 * @param newVelocityX Parameter to modify 563 * @return this object 564 */ 565 public RobotCentric withVelocityX(LinearVelocity newVelocityX) { 566 this.VelocityX = newVelocityX.in(MetersPerSecond); 567 return this; 568 } 569 570 /** 571 * Modifies the VelocityY parameter and returns itself. 572 * <p> 573 * The velocity in the Y direction, in m/s. Y is defined as to the left 574 * according to WPILib convention, so this determines how fast to travel to the 575 * left. 576 * 577 * @param newVelocityY Parameter to modify 578 * @return this object 579 */ 580 public RobotCentric withVelocityY(double newVelocityY) { 581 this.VelocityY = newVelocityY; 582 return this; 583 } 584 585 /** 586 * Modifies the VelocityY parameter and returns itself. 587 * <p> 588 * The velocity in the Y direction, in m/s. Y is defined as to the left 589 * according to WPILib convention, so this determines how fast to travel to the 590 * left. 591 * 592 * @param newVelocityY Parameter to modify 593 * @return this object 594 */ 595 public RobotCentric withVelocityY(LinearVelocity newVelocityY) { 596 this.VelocityY = newVelocityY.in(MetersPerSecond); 597 return this; 598 } 599 600 /** 601 * Modifies the RotationalRate parameter and returns itself. 602 * <p> 603 * The angular rate to rotate at, in radians per second. Angular rate is defined 604 * as counterclockwise positive, so this determines how fast to turn 605 * counterclockwise. 606 * 607 * @param newRotationalRate Parameter to modify 608 * @return this object 609 */ 610 public RobotCentric withRotationalRate(double newRotationalRate) { 611 this.RotationalRate = newRotationalRate; 612 return this; 613 } 614 615 /** 616 * Modifies the RotationalRate parameter and returns itself. 617 * <p> 618 * The angular rate to rotate at, in radians per second. Angular rate is defined 619 * as counterclockwise positive, so this determines how fast to turn 620 * counterclockwise. 621 * 622 * @param newRotationalRate Parameter to modify 623 * @return this object 624 */ 625 public RobotCentric withRotationalRate(AngularVelocity newRotationalRate) { 626 this.RotationalRate = newRotationalRate.in(RadiansPerSecond); 627 return this; 628 } 629 630 /** 631 * Modifies the Deadband parameter and returns itself. 632 * <p> 633 * The allowable deadband of the request, in m/s. 634 * 635 * @param newDeadband Parameter to modify 636 * @return this object 637 */ 638 public RobotCentric withDeadband(double newDeadband) { 639 this.Deadband = newDeadband; 640 return this; 641 } 642 643 /** 644 * Modifies the Deadband parameter and returns itself. 645 * <p> 646 * The allowable deadband of the request, in m/s. 647 * 648 * @param newDeadband Parameter to modify 649 * @return this object 650 */ 651 public RobotCentric withDeadband(LinearVelocity newDeadband) { 652 this.Deadband = newDeadband.in(MetersPerSecond); 653 return this; 654 } 655 656 /** 657 * Modifies the RotationalDeadband parameter and returns itself. 658 * <p> 659 * The rotational deadband of the request, in radians per second. 660 * 661 * @param newRotationalDeadband Parameter to modify 662 * @return this object 663 */ 664 public RobotCentric withRotationalDeadband(double newRotationalDeadband) { 665 this.RotationalDeadband = newRotationalDeadband; 666 return this; 667 } 668 669 /** 670 * Modifies the RotationalDeadband parameter and returns itself. 671 * <p> 672 * The rotational deadband of the request, in radians per second. 673 * 674 * @param newRotationalDeadband Parameter to modify 675 * @return this object 676 */ 677 public RobotCentric withRotationalDeadband(AngularVelocity newRotationalDeadband) { 678 this.RotationalDeadband = newRotationalDeadband.in(RadiansPerSecond); 679 return this; 680 } 681 682 /** 683 * Modifies the CenterOfRotation parameter and returns itself. 684 * <p> 685 * The center of rotation the robot should rotate around. This is (0,0) by 686 * default, which will rotate around the center of the robot. 687 * 688 * @param newCenterOfRotation Parameter to modify 689 * @return this object 690 */ 691 public RobotCentric withCenterOfRotation(Translation2d newCenterOfRotation) { 692 this.CenterOfRotation = newCenterOfRotation; 693 return this; 694 } 695 696 /** 697 * Modifies the DriveRequestType parameter and returns itself. 698 * <p> 699 * The type of control request to use for the drive motor. 700 * 701 * @param newDriveRequestType Parameter to modify 702 * @return this object 703 */ 704 public RobotCentric withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) { 705 this.DriveRequestType = newDriveRequestType; 706 return this; 707 } 708 709 /** 710 * Modifies the SteerRequestType parameter and returns itself. 711 * <p> 712 * The type of control request to use for the drive motor. 713 * 714 * @param newSteerRequestType Parameter to modify 715 * @return this object 716 */ 717 public RobotCentric withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) { 718 this.SteerRequestType = newSteerRequestType; 719 return this; 720 } 721 722 /** 723 * Modifies the DesaturateWheelSpeeds parameter and returns itself. 724 * <p> 725 * Whether to desaturate wheel speeds before applying. For more information, see 726 * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}. 727 * 728 * @param newDesaturateWheelSpeeds Parameter to modify 729 * @return this object 730 */ 731 public RobotCentric withDesaturateWheelSpeeds(boolean newDesaturateWheelSpeeds) { 732 this.DesaturateWheelSpeeds = newDesaturateWheelSpeeds; 733 return this; 734 } 735 736 public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) { 737 return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_RobotCentric(parameters.drivetrainId, 738 VelocityX, 739 VelocityY, 740 RotationalRate, 741 Deadband, 742 RotationalDeadband, 743 CenterOfRotation.getX(), 744 CenterOfRotation.getY(), 745 DriveRequestType.value, 746 SteerRequestType.value, 747 DesaturateWheelSpeeds)); 748 } 749 750 public void applyNative(int id) { 751 SwerveJNI.JNI_SetControl_RobotCentric(id, 752 VelocityX, 753 VelocityY, 754 RotationalRate, 755 Deadband, 756 RotationalDeadband, 757 CenterOfRotation.getX(), 758 CenterOfRotation.getY(), 759 DriveRequestType.value, 760 SteerRequestType.value, 761 DesaturateWheelSpeeds); 762 } 763 } 764 765 /** 766 * Sets the swerve drive modules to point to a specified direction. 767 */ 768 public static class PointWheelsAt implements NativeSwerveRequest { 769 /** 770 * The direction to point the modules toward. This direction is still optimized 771 * to what the module was previously at. 772 */ 773 public Rotation2d ModuleDirection = new Rotation2d(); 774 /** 775 * The type of control request to use for the drive motor. 776 */ 777 public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage; 778 /** 779 * The type of control request to use for the drive motor. 780 */ 781 public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position; 782 783 /** 784 * Modifies the ModuleDirection parameter and returns itself. 785 * <p> 786 * The direction to point the modules toward. This direction is still optimized 787 * to what the module was previously at. 788 * 789 * @param newModuleDirection Parameter to modify 790 * @return this object 791 */ 792 public PointWheelsAt withModuleDirection(Rotation2d newModuleDirection) { 793 this.ModuleDirection = newModuleDirection; 794 return this; 795 } 796 797 /** 798 * Modifies the DriveRequestType parameter and returns itself. 799 * <p> 800 * The type of control request to use for the drive motor. 801 * 802 * @param newDriveRequestType Parameter to modify 803 * @return this object 804 */ 805 public PointWheelsAt withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) { 806 this.DriveRequestType = newDriveRequestType; 807 return this; 808 } 809 810 /** 811 * Modifies the SteerRequestType parameter and returns itself. 812 * <p> 813 * The type of control request to use for the drive motor. 814 * 815 * @param newSteerRequestType Parameter to modify 816 * @return this object 817 */ 818 public PointWheelsAt withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) { 819 this.SteerRequestType = newSteerRequestType; 820 return this; 821 } 822 823 public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) { 824 return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_PointWheelsAt(parameters.drivetrainId, 825 ModuleDirection.getRadians(), 826 DriveRequestType.value, 827 SteerRequestType.value)); 828 } 829 830 public void applyNative(int id) { 831 SwerveJNI.JNI_SetControl_PointWheelsAt(id, 832 ModuleDirection.getRadians(), 833 DriveRequestType.value, 834 SteerRequestType.value); 835 } 836 } 837 838 /** 839 * Accepts a generic robot-centric ChassisSpeeds to apply to the drivetrain. 840 */ 841 public static class ApplyRobotSpeeds implements NativeSwerveRequest { 842 /** 843 * The robot-centric chassis speeds to apply to the drivetrain. 844 */ 845 public ChassisSpeeds Speeds = new ChassisSpeeds(); 846 /** 847 * Robot-centric wheel force feedforwards to apply in the X direction, in 848 * newtons. X is defined as forward according to WPILib convention, so this 849 * determines the forward forces to apply. 850 * <p> 851 * These forces should include friction applied to the ground. 852 * <p> 853 * The order of the forces should match the order of the modules returned from 854 * SwerveDrivetrain. 855 */ 856 public double[] WheelForceFeedforwardsX = {}; 857 /** 858 * Robot-centric wheel force feedforwards to apply in the Y direction, in 859 * newtons. Y is defined as to the left according to WPILib convention, so this 860 * determines the forces to apply to the left. 861 * <p> 862 * These forces should include friction applied to the ground. 863 * <p> 864 * The order of the forces should match the order of the modules returned from 865 * SwerveDrivetrain. 866 */ 867 public double[] WheelForceFeedforwardsY = {}; 868 /** 869 * The center of rotation the robot should rotate around. This is (0,0) by 870 * default, which will rotate around the center of the robot. 871 */ 872 public Translation2d CenterOfRotation = new Translation2d(); 873 /** 874 * The type of control request to use for the drive motor. 875 */ 876 public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage; 877 /** 878 * The type of control request to use for the drive motor. 879 */ 880 public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position; 881 /** 882 * Whether to desaturate wheel speeds before applying. For more information, see 883 * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}. 884 */ 885 public boolean DesaturateWheelSpeeds = true; 886 887 /** 888 * Modifies the Speeds parameter and returns itself. 889 * <p> 890 * The robot-centric chassis speeds to apply to the drivetrain. 891 * 892 * @param newSpeeds Parameter to modify 893 * @return this object 894 */ 895 public ApplyRobotSpeeds withSpeeds(ChassisSpeeds newSpeeds) { 896 this.Speeds = newSpeeds; 897 return this; 898 } 899 900 /** 901 * Modifies the WheelForceFeedforwardsX parameter and returns itself. 902 * <p> 903 * Robot-centric wheel force feedforwards to apply in the X direction, in 904 * newtons. X is defined as forward according to WPILib convention, so this 905 * determines the forward forces to apply. 906 * <p> 907 * These forces should include friction applied to the ground. 908 * <p> 909 * The order of the forces should match the order of the modules returned from 910 * SwerveDrivetrain. 911 * 912 * @param newWheelForceFeedforwardsX Parameter to modify 913 * @return this object 914 */ 915 public ApplyRobotSpeeds withWheelForceFeedforwardsX(double[] newWheelForceFeedforwardsX) { 916 this.WheelForceFeedforwardsX = newWheelForceFeedforwardsX; 917 return this; 918 } 919 920 /** 921 * Modifies the WheelForceFeedforwardsX parameter and returns itself. 922 * <p> 923 * Robot-centric wheel force feedforwards to apply in the X direction, in 924 * newtons. X is defined as forward according to WPILib convention, so this 925 * determines the forward forces to apply. 926 * <p> 927 * These forces should include friction applied to the ground. 928 * <p> 929 * The order of the forces should match the order of the modules returned from 930 * SwerveDrivetrain. 931 * 932 * @param newWheelForceFeedforwardsX Parameter to modify 933 * @return this object 934 */ 935 public ApplyRobotSpeeds withWheelForceFeedforwardsX(Force[] newWheelForceFeedforwardsX) { 936 if (this.WheelForceFeedforwardsX.length != newWheelForceFeedforwardsX.length) { 937 this.WheelForceFeedforwardsX = new double[newWheelForceFeedforwardsX.length]; 938 } 939 for (int i = 0; i < this.WheelForceFeedforwardsX.length; ++i) { 940 this.WheelForceFeedforwardsX[i] = newWheelForceFeedforwardsX[i].in(Newtons); 941 } 942 943 return this; 944 } 945 946 /** 947 * Modifies the WheelForceFeedforwardsY parameter and returns itself. 948 * <p> 949 * Robot-centric wheel force feedforwards to apply in the Y direction, in 950 * newtons. Y is defined as to the left according to WPILib convention, so this 951 * determines the forces to apply to the left. 952 * <p> 953 * These forces should include friction applied to the ground. 954 * <p> 955 * The order of the forces should match the order of the modules returned from 956 * SwerveDrivetrain. 957 * 958 * @param newWheelForceFeedforwardsY Parameter to modify 959 * @return this object 960 */ 961 public ApplyRobotSpeeds withWheelForceFeedforwardsY(double[] newWheelForceFeedforwardsY) { 962 this.WheelForceFeedforwardsY = newWheelForceFeedforwardsY; 963 return this; 964 } 965 966 /** 967 * Modifies the WheelForceFeedforwardsY parameter and returns itself. 968 * <p> 969 * Robot-centric wheel force feedforwards to apply in the Y direction, in 970 * newtons. Y is defined as to the left according to WPILib convention, so this 971 * determines the forces to apply to the left. 972 * <p> 973 * These forces should include friction applied to the ground. 974 * <p> 975 * The order of the forces should match the order of the modules returned from 976 * SwerveDrivetrain. 977 * 978 * @param newWheelForceFeedforwardsY Parameter to modify 979 * @return this object 980 */ 981 public ApplyRobotSpeeds withWheelForceFeedforwardsY(Force[] newWheelForceFeedforwardsY) { 982 if (this.WheelForceFeedforwardsY.length != newWheelForceFeedforwardsY.length) { 983 this.WheelForceFeedforwardsY = new double[newWheelForceFeedforwardsY.length]; 984 } 985 for (int i = 0; i < this.WheelForceFeedforwardsY.length; ++i) { 986 this.WheelForceFeedforwardsY[i] = newWheelForceFeedforwardsY[i].in(Newtons); 987 } 988 989 return this; 990 } 991 992 /** 993 * Modifies the CenterOfRotation parameter and returns itself. 994 * <p> 995 * The center of rotation the robot should rotate around. This is (0,0) by 996 * default, which will rotate around the center of the robot. 997 * 998 * @param newCenterOfRotation Parameter to modify 999 * @return this object 1000 */ 1001 public ApplyRobotSpeeds withCenterOfRotation(Translation2d newCenterOfRotation) { 1002 this.CenterOfRotation = newCenterOfRotation; 1003 return this; 1004 } 1005 1006 /** 1007 * Modifies the DriveRequestType parameter and returns itself. 1008 * <p> 1009 * The type of control request to use for the drive motor. 1010 * 1011 * @param newDriveRequestType Parameter to modify 1012 * @return this object 1013 */ 1014 public ApplyRobotSpeeds withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) { 1015 this.DriveRequestType = newDriveRequestType; 1016 return this; 1017 } 1018 1019 /** 1020 * Modifies the SteerRequestType parameter and returns itself. 1021 * <p> 1022 * The type of control request to use for the drive motor. 1023 * 1024 * @param newSteerRequestType Parameter to modify 1025 * @return this object 1026 */ 1027 public ApplyRobotSpeeds withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) { 1028 this.SteerRequestType = newSteerRequestType; 1029 return this; 1030 } 1031 1032 /** 1033 * Modifies the DesaturateWheelSpeeds parameter and returns itself. 1034 * <p> 1035 * Whether to desaturate wheel speeds before applying. For more information, see 1036 * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}. 1037 * 1038 * @param newDesaturateWheelSpeeds Parameter to modify 1039 * @return this object 1040 */ 1041 public ApplyRobotSpeeds withDesaturateWheelSpeeds(boolean newDesaturateWheelSpeeds) { 1042 this.DesaturateWheelSpeeds = newDesaturateWheelSpeeds; 1043 return this; 1044 } 1045 1046 public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) { 1047 return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_ApplyRobotSpeeds(parameters.drivetrainId, 1048 Speeds.vxMetersPerSecond, 1049 Speeds.vyMetersPerSecond, 1050 Speeds.omegaRadiansPerSecond, 1051 WheelForceFeedforwardsX, 1052 WheelForceFeedforwardsY, 1053 CenterOfRotation.getX(), 1054 CenterOfRotation.getY(), 1055 DriveRequestType.value, 1056 SteerRequestType.value, 1057 DesaturateWheelSpeeds)); 1058 } 1059 1060 public void applyNative(int id) { 1061 SwerveJNI.JNI_SetControl_ApplyRobotSpeeds(id, 1062 Speeds.vxMetersPerSecond, 1063 Speeds.vyMetersPerSecond, 1064 Speeds.omegaRadiansPerSecond, 1065 WheelForceFeedforwardsX, 1066 WheelForceFeedforwardsY, 1067 CenterOfRotation.getX(), 1068 CenterOfRotation.getY(), 1069 DriveRequestType.value, 1070 SteerRequestType.value, 1071 DesaturateWheelSpeeds); 1072 } 1073 } 1074 1075 /** 1076 * Accepts a generic robot-centric ChassisSpeeds to apply to the drivetrain. 1077 * 1078 * @deprecated Use {@link ApplyRobotSpeeds} instead 1079 */ 1080 @Deprecated(forRemoval = true, since = "2025") 1081 public static class ApplyChassisSpeeds extends ApplyRobotSpeeds {} 1082 1083 /** 1084 * Accepts a generic field-centric ChassisSpeeds to apply to the drivetrain. 1085 */ 1086 public static class ApplyFieldSpeeds implements NativeSwerveRequest { 1087 /** 1088 * The field-centric chassis speeds to apply to the drivetrain. 1089 */ 1090 public ChassisSpeeds Speeds = new ChassisSpeeds(); 1091 /** 1092 * Field-centric wheel force feedforwards to apply in the X direction, in 1093 * newtons. X is defined as forward according to WPILib convention, so this 1094 * determines the forward forces to apply. 1095 * <p> 1096 * These forces should include friction applied to the ground. 1097 * <p> 1098 * The order of the forces should match the order of the modules returned from 1099 * SwerveDrivetrain. 1100 */ 1101 public double[] WheelForceFeedforwardsX = {}; 1102 /** 1103 * Field-centric wheel force feedforwards to apply in the Y direction, in 1104 * newtons. Y is defined as to the left according to WPILib convention, so this 1105 * determines the forces to apply to the left. 1106 * <p> 1107 * These forces should include friction applied to the ground. 1108 * <p> 1109 * The order of the forces should match the order of the modules returned from 1110 * SwerveDrivetrain. 1111 */ 1112 public double[] WheelForceFeedforwardsY = {}; 1113 /** 1114 * The center of rotation the robot should rotate around. This is (0,0) by 1115 * default, which will rotate around the center of the robot. 1116 */ 1117 public Translation2d CenterOfRotation = new Translation2d(); 1118 /** 1119 * The type of control request to use for the drive motor. 1120 */ 1121 public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage; 1122 /** 1123 * The type of control request to use for the drive motor. 1124 */ 1125 public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position; 1126 /** 1127 * Whether to desaturate wheel speeds before applying. For more information, see 1128 * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}. 1129 */ 1130 public boolean DesaturateWheelSpeeds = true; 1131 /** 1132 * The perspective to use when determining which direction is forward. 1133 */ 1134 public ForwardPerspectiveValue ForwardPerspective = ForwardPerspectiveValue.BlueAlliance; 1135 1136 /** 1137 * Modifies the Speeds parameter and returns itself. 1138 * <p> 1139 * The field-centric chassis speeds to apply to the drivetrain. 1140 * 1141 * @param newSpeeds Parameter to modify 1142 * @return this object 1143 */ 1144 public ApplyFieldSpeeds withSpeeds(ChassisSpeeds newSpeeds) { 1145 this.Speeds = newSpeeds; 1146 return this; 1147 } 1148 1149 /** 1150 * Modifies the WheelForceFeedforwardsX parameter and returns itself. 1151 * <p> 1152 * Field-centric wheel force feedforwards to apply in the X direction, in 1153 * newtons. X is defined as forward according to WPILib convention, so this 1154 * determines the forward forces to apply. 1155 * <p> 1156 * These forces should include friction applied to the ground. 1157 * <p> 1158 * The order of the forces should match the order of the modules returned from 1159 * SwerveDrivetrain. 1160 * 1161 * @param newWheelForceFeedforwardsX Parameter to modify 1162 * @return this object 1163 */ 1164 public ApplyFieldSpeeds withWheelForceFeedforwardsX(double[] newWheelForceFeedforwardsX) { 1165 this.WheelForceFeedforwardsX = newWheelForceFeedforwardsX; 1166 return this; 1167 } 1168 1169 /** 1170 * Modifies the WheelForceFeedforwardsX parameter and returns itself. 1171 * <p> 1172 * Field-centric wheel force feedforwards to apply in the X direction, in 1173 * newtons. X is defined as forward according to WPILib convention, so this 1174 * determines the forward forces to apply. 1175 * <p> 1176 * These forces should include friction applied to the ground. 1177 * <p> 1178 * The order of the forces should match the order of the modules returned from 1179 * SwerveDrivetrain. 1180 * 1181 * @param newWheelForceFeedforwardsX Parameter to modify 1182 * @return this object 1183 */ 1184 public ApplyFieldSpeeds withWheelForceFeedforwardsX(Force[] newWheelForceFeedforwardsX) { 1185 if (this.WheelForceFeedforwardsX.length != newWheelForceFeedforwardsX.length) { 1186 this.WheelForceFeedforwardsX = new double[newWheelForceFeedforwardsX.length]; 1187 } 1188 for (int i = 0; i < this.WheelForceFeedforwardsX.length; ++i) { 1189 this.WheelForceFeedforwardsX[i] = newWheelForceFeedforwardsX[i].in(Newtons); 1190 } 1191 1192 return this; 1193 } 1194 1195 /** 1196 * Modifies the WheelForceFeedforwardsY parameter and returns itself. 1197 * <p> 1198 * Field-centric wheel force feedforwards to apply in the Y direction, in 1199 * newtons. Y is defined as to the left according to WPILib convention, so this 1200 * determines the forces to apply to the left. 1201 * <p> 1202 * These forces should include friction applied to the ground. 1203 * <p> 1204 * The order of the forces should match the order of the modules returned from 1205 * SwerveDrivetrain. 1206 * 1207 * @param newWheelForceFeedforwardsY Parameter to modify 1208 * @return this object 1209 */ 1210 public ApplyFieldSpeeds withWheelForceFeedforwardsY(double[] newWheelForceFeedforwardsY) { 1211 this.WheelForceFeedforwardsY = newWheelForceFeedforwardsY; 1212 return this; 1213 } 1214 1215 /** 1216 * Modifies the WheelForceFeedforwardsY parameter and returns itself. 1217 * <p> 1218 * Field-centric wheel force feedforwards to apply in the Y direction, in 1219 * newtons. Y is defined as to the left according to WPILib convention, so this 1220 * determines the forces to apply to the left. 1221 * <p> 1222 * These forces should include friction applied to the ground. 1223 * <p> 1224 * The order of the forces should match the order of the modules returned from 1225 * SwerveDrivetrain. 1226 * 1227 * @param newWheelForceFeedforwardsY Parameter to modify 1228 * @return this object 1229 */ 1230 public ApplyFieldSpeeds withWheelForceFeedforwardsY(Force[] newWheelForceFeedforwardsY) { 1231 if (this.WheelForceFeedforwardsY.length != newWheelForceFeedforwardsY.length) { 1232 this.WheelForceFeedforwardsY = new double[newWheelForceFeedforwardsY.length]; 1233 } 1234 for (int i = 0; i < this.WheelForceFeedforwardsY.length; ++i) { 1235 this.WheelForceFeedforwardsY[i] = newWheelForceFeedforwardsY[i].in(Newtons); 1236 } 1237 1238 return this; 1239 } 1240 1241 /** 1242 * Modifies the CenterOfRotation parameter and returns itself. 1243 * <p> 1244 * The center of rotation the robot should rotate around. This is (0,0) by 1245 * default, which will rotate around the center of the robot. 1246 * 1247 * @param newCenterOfRotation Parameter to modify 1248 * @return this object 1249 */ 1250 public ApplyFieldSpeeds withCenterOfRotation(Translation2d newCenterOfRotation) { 1251 this.CenterOfRotation = newCenterOfRotation; 1252 return this; 1253 } 1254 1255 /** 1256 * Modifies the DriveRequestType parameter and returns itself. 1257 * <p> 1258 * The type of control request to use for the drive motor. 1259 * 1260 * @param newDriveRequestType Parameter to modify 1261 * @return this object 1262 */ 1263 public ApplyFieldSpeeds withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) { 1264 this.DriveRequestType = newDriveRequestType; 1265 return this; 1266 } 1267 1268 /** 1269 * Modifies the SteerRequestType parameter and returns itself. 1270 * <p> 1271 * The type of control request to use for the drive motor. 1272 * 1273 * @param newSteerRequestType Parameter to modify 1274 * @return this object 1275 */ 1276 public ApplyFieldSpeeds withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) { 1277 this.SteerRequestType = newSteerRequestType; 1278 return this; 1279 } 1280 1281 /** 1282 * Modifies the DesaturateWheelSpeeds parameter and returns itself. 1283 * <p> 1284 * Whether to desaturate wheel speeds before applying. For more information, see 1285 * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}. 1286 * 1287 * @param newDesaturateWheelSpeeds Parameter to modify 1288 * @return this object 1289 */ 1290 public ApplyFieldSpeeds withDesaturateWheelSpeeds(boolean newDesaturateWheelSpeeds) { 1291 this.DesaturateWheelSpeeds = newDesaturateWheelSpeeds; 1292 return this; 1293 } 1294 1295 /** 1296 * Modifies the ForwardPerspective parameter and returns itself. 1297 * <p> 1298 * The perspective to use when determining which direction is forward. 1299 * 1300 * @param newForwardPerspective Parameter to modify 1301 * @return this object 1302 */ 1303 public ApplyFieldSpeeds withForwardPerspective(ForwardPerspectiveValue newForwardPerspective) { 1304 this.ForwardPerspective = newForwardPerspective; 1305 return this; 1306 } 1307 1308 public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) { 1309 return StatusCode.valueOf(SwerveJNI.JNI_Request_Apply_ApplyFieldSpeeds(parameters.drivetrainId, 1310 Speeds.vxMetersPerSecond, 1311 Speeds.vyMetersPerSecond, 1312 Speeds.omegaRadiansPerSecond, 1313 WheelForceFeedforwardsX, 1314 WheelForceFeedforwardsY, 1315 CenterOfRotation.getX(), 1316 CenterOfRotation.getY(), 1317 DriveRequestType.value, 1318 SteerRequestType.value, 1319 DesaturateWheelSpeeds, 1320 ForwardPerspective.value)); 1321 } 1322 1323 public void applyNative(int id) { 1324 SwerveJNI.JNI_SetControl_ApplyFieldSpeeds(id, 1325 Speeds.vxMetersPerSecond, 1326 Speeds.vyMetersPerSecond, 1327 Speeds.omegaRadiansPerSecond, 1328 WheelForceFeedforwardsX, 1329 WheelForceFeedforwardsY, 1330 CenterOfRotation.getX(), 1331 CenterOfRotation.getY(), 1332 DriveRequestType.value, 1333 SteerRequestType.value, 1334 DesaturateWheelSpeeds, 1335 ForwardPerspective.value); 1336 } 1337 } 1338 1339 /** 1340 * Drives the swerve drivetrain in a field-centric manner, maintaining a 1341 * specified heading angle to ensure the robot is facing the desired direction 1342 * <p> 1343 * When users use this request, they specify the direction the robot should 1344 * travel oriented against the field, and the direction the robot should be facing. 1345 * <p> 1346 * An example scenario is that the robot is oriented to the east, the VelocityX 1347 * is +5 m/s, VelocityY is 0 m/s, and TargetDirection is 180 degrees. 1348 * In this scenario, the robot would drive northward at 5 m/s and turn clockwise 1349 * to a target of 180 degrees. 1350 * <p> 1351 * This control request is especially useful for autonomous control, where the 1352 * robot should be facing a changing direction throughout the motion. 1353 */ 1354 public static class FieldCentricFacingAngle implements SwerveRequest { 1355 /** 1356 * The velocity in the X direction, in m/s. 1357 * X is defined as forward according to WPILib convention, 1358 * so this determines how fast to travel forward. 1359 */ 1360 public double VelocityX = 0; 1361 /** 1362 * The velocity in the Y direction, in m/s. 1363 * Y is defined as to the left according to WPILib convention, 1364 * so this determines how fast to travel to the left. 1365 */ 1366 public double VelocityY = 0; 1367 /** 1368 * The desired direction to face. 1369 * 0 Degrees is defined as in the direction of the X axis. 1370 * As a result, a TargetDirection of 90 degrees will point along 1371 * the Y axis, or to the left. 1372 */ 1373 public Rotation2d TargetDirection = new Rotation2d(); 1374 /** 1375 * The rotational rate feedforward to add to the output of the heading 1376 * controller, in radians per second. When using a motion profile for the 1377 * target direction, this can be set to the current velocity reference of 1378 * the profile. 1379 */ 1380 public double TargetRateFeedforward = 0; 1381 1382 /** 1383 * The allowable deadband of the request, in m/s. 1384 */ 1385 public double Deadband = 0; 1386 /** 1387 * The rotational deadband of the request, in radians per second. 1388 */ 1389 public double RotationalDeadband = 0; 1390 /** 1391 * The center of rotation the robot should rotate around. 1392 * This is (0,0) by default, which will rotate around the center of the robot. 1393 */ 1394 public Translation2d CenterOfRotation = new Translation2d(); 1395 1396 /** 1397 * The type of control request to use for the drive motor. 1398 */ 1399 public SwerveModule.DriveRequestType DriveRequestType = SwerveModule.DriveRequestType.OpenLoopVoltage; 1400 /** 1401 * The type of control request to use for the steer motor. 1402 */ 1403 public SwerveModule.SteerRequestType SteerRequestType = SwerveModule.SteerRequestType.Position; 1404 /** 1405 * Whether to desaturate wheel speeds before applying. 1406 * For more information, see the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}. 1407 */ 1408 public boolean DesaturateWheelSpeeds = true; 1409 1410 /** 1411 * The perspective to use when determining which direction is forward. 1412 */ 1413 public ForwardPerspectiveValue ForwardPerspective = ForwardPerspectiveValue.OperatorPerspective; 1414 1415 /** 1416 * The PID controller used to maintain the desired heading. 1417 * Users can specify the PID gains to change how aggressively to maintain 1418 * heading. 1419 * <p> 1420 * This PID controller operates on heading radians and outputs a target 1421 * rotational rate in radians per second. Note that continuous input should 1422 * be enabled on the range [-pi, pi]. 1423 */ 1424 public PhoenixPIDController HeadingController = new PhoenixPIDController(0, 0, 0); 1425 1426 private final FieldCentric m_fieldCentric = new FieldCentric(); 1427 1428 public FieldCentricFacingAngle() { 1429 HeadingController.enableContinuousInput(-Math.PI, Math.PI); 1430 } 1431 1432 public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) { 1433 Rotation2d angleToFace = TargetDirection; 1434 if (ForwardPerspective == ForwardPerspectiveValue.OperatorPerspective) { 1435 /* If we're operator perspective, rotate the direction we want to face by the angle */ 1436 angleToFace = angleToFace.rotateBy(parameters.operatorForwardDirection); 1437 } 1438 1439 double toApplyOmega = TargetRateFeedforward + 1440 HeadingController.calculate( 1441 parameters.currentPose.getRotation().getRadians(), 1442 angleToFace.getRadians(), 1443 parameters.timestamp 1444 ); 1445 1446 return m_fieldCentric 1447 .withVelocityX(VelocityX) 1448 .withVelocityY(VelocityY) 1449 .withRotationalRate(toApplyOmega) 1450 .withDeadband(Deadband) 1451 .withRotationalDeadband(RotationalDeadband) 1452 .withCenterOfRotation(CenterOfRotation) 1453 .withDriveRequestType(DriveRequestType) 1454 .withSteerRequestType(SteerRequestType) 1455 .withDesaturateWheelSpeeds(DesaturateWheelSpeeds) 1456 .withForwardPerspective(ForwardPerspective) 1457 .apply(parameters, modulesToApply); 1458 } 1459 1460 /** 1461 * Modifies the VelocityX parameter and returns itself. 1462 * <p> 1463 * The velocity in the X direction, in m/s. X is defined as forward according to 1464 * WPILib convention, so this determines how fast to travel forward. 1465 * 1466 * @param newVelocityX Parameter to modify 1467 * @return this object 1468 */ 1469 public FieldCentricFacingAngle withVelocityX(double newVelocityX) { 1470 this.VelocityX = newVelocityX; 1471 return this; 1472 } 1473 1474 /** 1475 * Modifies the VelocityX parameter and returns itself. 1476 * <p> 1477 * The velocity in the X direction, in m/s. X is defined as forward according to 1478 * WPILib convention, so this determines how fast to travel forward. 1479 * 1480 * @param newVelocityX Parameter to modify 1481 * @return this object 1482 */ 1483 public FieldCentricFacingAngle withVelocityX(LinearVelocity newVelocityX) { 1484 this.VelocityX = newVelocityX.in(MetersPerSecond); 1485 return this; 1486 } 1487 1488 /** 1489 * Modifies the VelocityY parameter and returns itself. 1490 * <p> 1491 * The velocity in the Y direction, in m/s. Y is defined as to the left 1492 * according to WPILib convention, so this determines how fast to travel to the 1493 * left. 1494 * 1495 * @param newVelocityY Parameter to modify 1496 * @return this object 1497 */ 1498 public FieldCentricFacingAngle withVelocityY(double newVelocityY) { 1499 this.VelocityY = newVelocityY; 1500 return this; 1501 } 1502 1503 /** 1504 * Modifies the VelocityY parameter and returns itself. 1505 * <p> 1506 * The velocity in the Y direction, in m/s. Y is defined as to the left 1507 * according to WPILib convention, so this determines how fast to travel to the 1508 * left. 1509 * 1510 * @param newVelocityY Parameter to modify 1511 * @return this object 1512 */ 1513 public FieldCentricFacingAngle withVelocityY(LinearVelocity newVelocityY) { 1514 this.VelocityY = newVelocityY.in(MetersPerSecond); 1515 return this; 1516 } 1517 1518 /** 1519 * Modifies the TargetDirection parameter and returns itself. 1520 * <p> 1521 * The desired direction to face. 0 Degrees is defined as in the direction of 1522 * the X axis. As a result, a TargetDirection of 90 degrees will point along 1523 * the Y axis, or to the left. 1524 * 1525 * @param newTargetDirection Parameter to modify 1526 * @return this object 1527 */ 1528 public FieldCentricFacingAngle withTargetDirection(Rotation2d newTargetDirection) { 1529 this.TargetDirection = newTargetDirection; 1530 return this; 1531 } 1532 1533 /** 1534 * Modifies the TargetRateFeedforward parameter and returns itself. 1535 * <p> 1536 * The rotational rate feedforward to add to the output of the heading 1537 * controller, in radians per second. When using a motion profile for the 1538 * target direction, this can be set to the current velocity reference of 1539 * the profile. 1540 * 1541 * @param newTargetRateFeedforward Parameter to modify 1542 * @return this object 1543 */ 1544 public FieldCentricFacingAngle withTargetRateFeedforward(double newTargetRateFeedforward) { 1545 this.TargetRateFeedforward = newTargetRateFeedforward; 1546 return this; 1547 } 1548 /** 1549 * Modifies the TargetRateFeedforward parameter and returns itself. 1550 * <p> 1551 * The rotational rate feedforward to add to the output of the heading 1552 * controller, in radians per second. When using a motion profile for the 1553 * target direction, this can be set to the current velocity reference of 1554 * the profile. 1555 * 1556 * @param newTargetRateFeedforward Parameter to modify 1557 * @return this object 1558 */ 1559 public FieldCentricFacingAngle withTargetRateFeedforward(AngularVelocity newTargetRateFeedforward) { 1560 this.TargetRateFeedforward = newTargetRateFeedforward.in(RadiansPerSecond); 1561 return this; 1562 } 1563 1564 /** 1565 * Modifies the Deadband parameter and returns itself. 1566 * <p> 1567 * The allowable deadband of the request, in m/s. 1568 * 1569 * @param newDeadband Parameter to modify 1570 * @return this object 1571 */ 1572 public FieldCentricFacingAngle withDeadband(double newDeadband) { 1573 this.Deadband = newDeadband; 1574 return this; 1575 } 1576 1577 /** 1578 * Modifies the Deadband parameter and returns itself. 1579 * <p> 1580 * The allowable deadband of the request, in m/s. 1581 * 1582 * @param newDeadband Parameter to modify 1583 * @return this object 1584 */ 1585 public FieldCentricFacingAngle withDeadband(LinearVelocity newDeadband) { 1586 this.Deadband = newDeadband.in(MetersPerSecond); 1587 return this; 1588 } 1589 1590 /** 1591 * Modifies the RotationalDeadband parameter and returns itself. 1592 * <p> 1593 * The rotational deadband of the request, in radians per second. 1594 * 1595 * @param newRotationalDeadband Parameter to modify 1596 * @return this object 1597 */ 1598 public FieldCentricFacingAngle withRotationalDeadband(double newRotationalDeadband) { 1599 this.RotationalDeadband = newRotationalDeadband; 1600 return this; 1601 } 1602 1603 /** 1604 * Modifies the RotationalDeadband parameter and returns itself. 1605 * <p> 1606 * The rotational deadband of the request, in radians per second. 1607 * 1608 * @param newRotationalDeadband Parameter to modify 1609 * @return this object 1610 */ 1611 public FieldCentricFacingAngle withRotationalDeadband(AngularVelocity newRotationalDeadband) { 1612 this.RotationalDeadband = newRotationalDeadband.in(RadiansPerSecond); 1613 return this; 1614 } 1615 1616 /** 1617 * Modifies the CenterOfRotation parameter and returns itself. 1618 * <p> 1619 * The center of rotation the robot should rotate around. This is (0,0) by 1620 * default, which will rotate around the center of the robot. 1621 * 1622 * @param newCenterOfRotation Parameter to modify 1623 * @return this object 1624 */ 1625 public FieldCentricFacingAngle withCenterOfRotation(Translation2d newCenterOfRotation) { 1626 this.CenterOfRotation = newCenterOfRotation; 1627 return this; 1628 } 1629 1630 /** 1631 * Modifies the DriveRequestType parameter and returns itself. 1632 * <p> 1633 * The type of control request to use for the drive motor. 1634 * 1635 * @param newDriveRequestType Parameter to modify 1636 * @return this object 1637 */ 1638 public FieldCentricFacingAngle withDriveRequestType(SwerveModule.DriveRequestType newDriveRequestType) { 1639 this.DriveRequestType = newDriveRequestType; 1640 return this; 1641 } 1642 1643 /** 1644 * Modifies the SteerRequestType parameter and returns itself. 1645 * <p> 1646 * The type of control request to use for the drive motor. 1647 * 1648 * @param newSteerRequestType Parameter to modify 1649 * @return this object 1650 */ 1651 public FieldCentricFacingAngle withSteerRequestType(SwerveModule.SteerRequestType newSteerRequestType) { 1652 this.SteerRequestType = newSteerRequestType; 1653 return this; 1654 } 1655 1656 /** 1657 * Modifies the DesaturateWheelSpeeds parameter and returns itself. 1658 * <p> 1659 * Whether to desaturate wheel speeds before applying. For more information, see 1660 * the documentation of {@link SwerveDriveKinematics#desaturateWheelSpeeds}. 1661 * 1662 * @param newDesaturateWheelSpeeds Parameter to modify 1663 * @return this object 1664 */ 1665 public FieldCentricFacingAngle withDesaturateWheelSpeeds(boolean newDesaturateWheelSpeeds) { 1666 this.DesaturateWheelSpeeds = newDesaturateWheelSpeeds; 1667 return this; 1668 } 1669 1670 /** 1671 * Modifies the ForwardPerspective parameter and returns itself. 1672 * <p> 1673 * The perspective to use when determining which direction is forward. 1674 * 1675 * @param newForwardPerspective Parameter to modify 1676 * @return this object 1677 */ 1678 public FieldCentricFacingAngle withForwardPerspective(ForwardPerspectiveValue newForwardPerspective) { 1679 this.ForwardPerspective = newForwardPerspective; 1680 return this; 1681 } 1682 } 1683 1684 /** 1685 * SysId-specific SwerveRequest to characterize the translational 1686 * characteristics of a swerve drivetrain. 1687 */ 1688 public static class SysIdSwerveTranslation implements SwerveRequest { 1689 /** 1690 * Voltage to apply to drive wheels. 1691 */ 1692 public double VoltsToApply = 0; 1693 1694 /* Local reference to a voltage request for the drive motors */ 1695 private final VoltageOut m_driveRequest = new VoltageOut(0); 1696 /* Local reference to a position request for the steer motors */ 1697 private final PositionVoltage m_steerRequest = new PositionVoltage(0); 1698 1699 public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) { 1700 for (int i = 0; i < modulesToApply.length; ++i) { 1701 modulesToApply[i].apply(m_driveRequest.withOutput(VoltsToApply), m_steerRequest.withPosition(0)); 1702 } 1703 return StatusCode.OK; 1704 } 1705 1706 /** 1707 * Sets the voltage to apply to the drive wheels. 1708 * 1709 * @param volts Voltage to apply 1710 * @return this request 1711 */ 1712 public SysIdSwerveTranslation withVolts(double volts) { 1713 VoltsToApply = volts; 1714 return this; 1715 } 1716 /** 1717 * Sets the voltage to apply to the drive wheels. 1718 * 1719 * @param volts Voltage to apply 1720 * @return this request 1721 */ 1722 public SysIdSwerveTranslation withVolts(Voltage volts) { 1723 VoltsToApply = volts.in(Volts); 1724 return this; 1725 } 1726 } 1727 1728 /** 1729 * SysId-specific SwerveRequest to characterize the rotational 1730 * characteristics of a swerve drivetrain. This is useful to 1731 * characterize the heading controller for FieldCentricFacingAngle. 1732 * <p> 1733 * The RotationalRate of this swerve request should be logged. 1734 * When importing the log to SysId, set the "voltage" to 1735 * RotationalRate, "position" to the Pigeon 2 Yaw, and "velocity" 1736 * to the Pigeon 2 AngularVelocityZWorld. Note that the position 1737 * and velocity will both need to be scaled by pi/180. 1738 * <p> 1739 * Alternatively, the MotorVoltage of one of the drive motors can 1740 * be loaded into the SysId "voltage" field, which can be useful 1741 * when determining the MOI of the robot. 1742 */ 1743 public static class SysIdSwerveRotation implements SwerveRequest { 1744 /** 1745 * The angular rate to rotate at, in radians per second. 1746 */ 1747 public double RotationalRate = 0; 1748 1749 /* Local reference to a voltage request for the drive motors */ 1750 private final VoltageOut m_driveRequest = new VoltageOut(0); 1751 /* Local reference to a position request for the steer motors */ 1752 private final PositionVoltage m_steerRequest = new PositionVoltage(0); 1753 1754 public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) { 1755 for (int i = 0; i < modulesToApply.length; ++i) { 1756 var speedMps = RotationalRate * parameters.moduleLocations[i].getNorm(); 1757 var angle = parameters.moduleLocations[i].getAngle().plus(Rotation2d.kCCW_90deg); 1758 modulesToApply[i].apply( 1759 m_driveRequest.withOutput(speedMps / parameters.kMaxSpeedMps * 12.0), 1760 m_steerRequest.withPosition(angle.getRotations()) 1761 ); 1762 } 1763 return StatusCode.OK; 1764 } 1765 1766 /** 1767 * Update the angular rate to rotate at, in radians per second. 1768 * 1769 * @param newRotationalRate Angular rate to rotate at 1770 * @return this request 1771 */ 1772 public SysIdSwerveRotation withRotationalRate(double newRotationalRate) { 1773 RotationalRate = newRotationalRate; 1774 return this; 1775 } 1776 /** 1777 * Update the angular rate to rotate at. 1778 * 1779 * @param newRotationalRate The angular rate to rotate at 1780 * @return this request 1781 */ 1782 public SysIdSwerveRotation withRotationalRate(AngularVelocity newRotationalRate) { 1783 RotationalRate = newRotationalRate.in(RadiansPerSecond); 1784 return this; 1785 } 1786 } 1787 1788 /** 1789 * SysId-specific SwerveRequest to characterize the steer module 1790 * characteristics of a swerve drivetrain. 1791 */ 1792 public static class SysIdSwerveSteerGains implements SwerveRequest { 1793 /** 1794 * Voltage to apply to steer motors. 1795 */ 1796 public double VoltsToApply = 0; 1797 1798 /* Local reference to a coast request for the drive motors */ 1799 private final CoastOut m_driveRequest = new CoastOut(); 1800 /* Local reference to a voltage request for the steer motors */ 1801 private final VoltageOut m_steerRequest = new VoltageOut(0); 1802 1803 public StatusCode apply(SwerveControlParameters parameters, SwerveModule... modulesToApply) { 1804 for (int i = 0; i < modulesToApply.length; ++i) { 1805 modulesToApply[i].apply(m_driveRequest, m_steerRequest.withOutput(VoltsToApply)); 1806 } 1807 return StatusCode.OK; 1808 } 1809 1810 /** 1811 * Sets the voltage to apply to the steer motors. 1812 * 1813 * @param volts Voltage to apply 1814 * @return this request 1815 */ 1816 public SysIdSwerveSteerGains withVolts(double volts) { 1817 VoltsToApply = volts; 1818 return this; 1819 } 1820 /** 1821 * Sets the voltage to apply to the steer motors. 1822 * 1823 * @param volts Voltage to apply 1824 * @return this request 1825 */ 1826 public SysIdSwerveSteerGains withVolts(Voltage volts) { 1827 VoltsToApply = volts.in(Volts); 1828 return this; 1829 } 1830 } 1831}