From b3e0b3894ba48ff388faeeedb8af0c331308379b Mon Sep 17 00:00:00 2001 From: CoryNessCTR Date: Thu, 25 Jan 2024 15:14:11 -0500 Subject: [PATCH] Adds alliance color detection, and uses dev-build for sysid --- .../frc/robot/CommandSwerveDrivetrain.java | 12 ++--- .../main/java/frc/robot/RobotContainer.java | 15 ++++++ .../frc/robot/sysid/SysIdSwerveRotation.java | 33 ------------ .../robot/sysid/SysIdSwerveSteerGains.java | 32 ------------ .../robot/sysid/SysIdSwerveTranslation.java | 32 ------------ .../vendordeps/Phoenix6.json | 52 +++++++++---------- 6 files changed, 47 insertions(+), 129 deletions(-) delete mode 100644 java/SwerveWithPathPlanner/src/main/java/frc/robot/sysid/SysIdSwerveRotation.java delete mode 100644 java/SwerveWithPathPlanner/src/main/java/frc/robot/sysid/SysIdSwerveSteerGains.java delete mode 100644 java/SwerveWithPathPlanner/src/main/java/frc/robot/sysid/SysIdSwerveTranslation.java diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/CommandSwerveDrivetrain.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/CommandSwerveDrivetrain.java index d3381efd..331d76a3 100644 --- a/java/SwerveWithPathPlanner/src/main/java/frc/robot/CommandSwerveDrivetrain.java +++ b/java/SwerveWithPathPlanner/src/main/java/frc/robot/CommandSwerveDrivetrain.java @@ -23,9 +23,6 @@ import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.generated.TunerConstants; -import frc.robot.sysid.SysIdSwerveRotation; -import frc.robot.sysid.SysIdSwerveSteerGains; -import frc.robot.sysid.SysIdSwerveTranslation; /** * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem @@ -37,9 +34,11 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst private double m_lastSimTime; private final SwerveRequest.ApplyChassisSpeeds autoRequest = new SwerveRequest.ApplyChassisSpeeds(); - private final SysIdSwerveTranslation translationCharacterization = new SysIdSwerveTranslation(); - private final SysIdSwerveRotation rotationCharacterization = new SysIdSwerveRotation(); - private final SysIdSwerveSteerGains steerCharacterization = new SysIdSwerveSteerGains(); + private final SwerveRequest.SysIdSwerveTranslation translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); + private final SwerveRequest.SysIdSwerveRotation rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); + private final SwerveRequest.SysIdSwerveSteerGains steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); + + /* Use one of these sysidroutines for your particular test */ private SysIdRoutine m_SysIdRoutineTranslation = new SysIdRoutine( new SysIdRoutine.Config( @@ -121,6 +120,7 @@ public Command getAutoPath(String pathName) { return new PathPlannerAuto(pathName); } + /* Both the sysid commands are specific to one particular sysid routine, change which one you're trying to characterize */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { return m_SysIdRoutineSteer.quasistatic(direction); } diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/RobotContainer.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/RobotContainer.java index 07f27344..9bce3c29 100644 --- a/java/SwerveWithPathPlanner/src/main/java/frc/robot/RobotContainer.java +++ b/java/SwerveWithPathPlanner/src/main/java/frc/robot/RobotContainer.java @@ -8,8 +8,12 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.generated.TunerConstants; @@ -29,6 +33,11 @@ public class RobotContainer { private final SwerveRequest.RobotCentric forwardStraight = new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ + private final Rotation2d blueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0); + /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ + private final Rotation2d redAlliancePerspectiveRotation = Rotation2d.fromDegrees(180); + /* Path follower */ private Command runAuto = drivetrain.getAutoPath("Tests"); @@ -64,6 +73,12 @@ private void configureBindings() { joystick.back().and(joystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); joystick.start().and(joystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); joystick.start().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); + + /* When we get alliance data from Driver Station, forward it to the drivetrain so it knows what perspective is forward for operator control */ + new Trigger(() -> DriverStation.getAlliance().isPresent()) + .whileTrue(new RunCommand(() -> drivetrain.setOperatorPerspectiveForward( + DriverStation.getAlliance().get() == Alliance.Red ? redAlliancePerspectiveRotation + : blueAlliancePerspectiveRotation))); } public RobotContainer() { diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/sysid/SysIdSwerveRotation.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/sysid/SysIdSwerveRotation.java deleted file mode 100644 index f38c281c..00000000 --- a/java/SwerveWithPathPlanner/src/main/java/frc/robot/sysid/SysIdSwerveRotation.java +++ /dev/null @@ -1,33 +0,0 @@ -package frc.robot.sysid; - -import static edu.wpi.first.units.MutableMeasure.mutable; -import static edu.wpi.first.units.Units.Volts; - -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.MutableMeasure; -import edu.wpi.first.units.Voltage; - -public class SysIdSwerveRotation implements SwerveRequest { - public final MutableMeasure VoltsToApply = mutable(Volts.of(0)); - - private VoltageOut m_voltRequest = new VoltageOut(0); - - public StatusCode apply(SwerveControlRequestParameters parameters, SwerveModule... modulesToApply) { - for (int i = 0; i < modulesToApply.length; ++i) { - modulesToApply[i].applyCharacterization(parameters.swervePositions[i].getAngle().plus(Rotation2d.fromDegrees(90)), - m_voltRequest.withOutput(VoltsToApply.in(Volts))); - } - return StatusCode.OK; - } - - public SysIdSwerveRotation withVolts(Measure Volts) { - VoltsToApply.mut_replace(Volts); - return this; - } -} diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/sysid/SysIdSwerveSteerGains.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/sysid/SysIdSwerveSteerGains.java deleted file mode 100644 index 56d213b2..00000000 --- a/java/SwerveWithPathPlanner/src/main/java/frc/robot/sysid/SysIdSwerveSteerGains.java +++ /dev/null @@ -1,32 +0,0 @@ -package frc.robot.sysid; - -import static edu.wpi.first.units.MutableMeasure.mutable; -import static edu.wpi.first.units.Units.Volts; - -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; - -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.MutableMeasure; -import edu.wpi.first.units.Voltage; - -public class SysIdSwerveSteerGains implements SwerveRequest { - public final MutableMeasure VoltsToApply = mutable(Volts.of(0)); - - private VoltageOut m_voltRequest = new VoltageOut(0); - - public StatusCode apply(SwerveControlRequestParameters parameters, SwerveModule... modulesToApply) { - for (int i = 0; i < modulesToApply.length; ++i) { - modulesToApply[i].getSteerMotor().setControl(m_voltRequest.withOutput(VoltsToApply.in(Volts))); - modulesToApply[i].getDriveMotor().setControl(m_voltRequest.withOutput(0)); - } - return StatusCode.OK; - } - - public SysIdSwerveSteerGains withVolts(Measure Volts) { - VoltsToApply.mut_replace(Volts); - return this; - } -} diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/sysid/SysIdSwerveTranslation.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/sysid/SysIdSwerveTranslation.java deleted file mode 100644 index 8b59c7bc..00000000 --- a/java/SwerveWithPathPlanner/src/main/java/frc/robot/sysid/SysIdSwerveTranslation.java +++ /dev/null @@ -1,32 +0,0 @@ -package frc.robot.sysid; - -import static edu.wpi.first.units.MutableMeasure.mutable; -import static edu.wpi.first.units.Units.Volts; - -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.MutableMeasure; -import edu.wpi.first.units.Voltage; - -public class SysIdSwerveTranslation implements SwerveRequest { - public final MutableMeasure VoltsToApply = mutable(Volts.of(0)); - - private VoltageOut m_voltRequest = new VoltageOut(0); - - public StatusCode apply(SwerveControlRequestParameters parameters, SwerveModule... modulesToApply) { - for (int i = 0; i < modulesToApply.length; ++i) { - modulesToApply[i].applyCharacterization(Rotation2d.fromDegrees(0), m_voltRequest.withOutput(VoltsToApply.in(Volts))); - } - return StatusCode.OK; - } - - public SysIdSwerveTranslation withVolts(Measure Volts) { - VoltsToApply.mut_replace(Volts); - return this; - } -} diff --git a/java/SwerveWithPathPlanner/vendordeps/Phoenix6.json b/java/SwerveWithPathPlanner/vendordeps/Phoenix6.json index 69a40798..a7734712 100644 --- a/java/SwerveWithPathPlanner/vendordeps/Phoenix6.json +++ b/java/SwerveWithPathPlanner/vendordeps/Phoenix6.json @@ -1,13 +1,13 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" + "https://maven.ctr-electronics.com/development/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/development/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", "conflictsWith": [ { "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.1.0" + "version": "24.1.0-45-g0e8c329" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.1.0", + "version": "24.1.0-45-g0e8c329", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true,