Skip to content

Commit ad1fbe1

Browse files
OrcasphynxOrcasphynx
authored
Merge pull request #6 from IgniteRobotics/feature/vision-subsystem
Feature/vision subsystem
2 parents 0be2e82 + 702c196 commit ad1fbe1

File tree

11 files changed

+1467
-1
lines changed

11 files changed

+1467
-1
lines changed

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,6 @@
1212
import frc.robot.commands.ExampleCommand;
1313
import frc.robot.subsystems.ExampleSubsystem;
1414

15-
1615
/**
1716
* This class is where the bulk of the robot should be declared. Since Command-based is a
1817
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}

‎src/main/java/frc/robot/generated/CommandSwerveDrivetrain.java‎

Lines changed: 240 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,240 @@
1+
package frc.robot.generated;
2+
3+
import static edu.wpi.first.units.Units.*;
4+
5+
import com.ctre.phoenix6.SignalLogger;
6+
import com.ctre.phoenix6.Utils;
7+
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
8+
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
9+
import com.ctre.phoenix6.swerve.SwerveRequest;
10+
import edu.wpi.first.math.Matrix;
11+
import edu.wpi.first.math.geometry.Rotation2d;
12+
import edu.wpi.first.math.numbers.N1;
13+
import edu.wpi.first.math.numbers.N3;
14+
import edu.wpi.first.wpilibj.DriverStation;
15+
import edu.wpi.first.wpilibj.DriverStation.Alliance;
16+
import edu.wpi.first.wpilibj.Notifier;
17+
import edu.wpi.first.wpilibj.RobotController;
18+
import edu.wpi.first.wpilibj2.command.Command;
19+
import edu.wpi.first.wpilibj2.command.Subsystem;
20+
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
21+
import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain;
22+
import java.util.function.Supplier;
23+
24+
/**
25+
* Class that extends the Phoenix 6 SwerveDrivetrain class and implements Subsystem so it can easily
26+
* be used in command-based projects.
27+
*/
28+
public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem {
29+
private static final double kSimLoopPeriod = 0.005; // 5 ms
30+
private Notifier m_simNotifier = null;
31+
private double m_lastSimTime;
32+
33+
/* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
34+
private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero;
35+
/* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
36+
private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg;
37+
/* Keep track if we've ever applied the operator perspective before or not */
38+
private boolean m_hasAppliedOperatorPerspective = false;
39+
40+
/* Swerve requests to apply during SysId characterization */
41+
private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization =
42+
new SwerveRequest.SysIdSwerveTranslation();
43+
private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization =
44+
new SwerveRequest.SysIdSwerveSteerGains();
45+
private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization =
46+
new SwerveRequest.SysIdSwerveRotation();
47+
48+
/* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
49+
private final SysIdRoutine m_sysIdRoutineTranslation =
50+
new SysIdRoutine(
51+
new SysIdRoutine.Config(
52+
null, // Use default ramp rate (1 V/s)
53+
Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout
54+
null, // Use default timeout (10 s)
55+
// Log state with SignalLogger class
56+
state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())),
57+
new SysIdRoutine.Mechanism(
58+
output -> setControl(m_translationCharacterization.withVolts(output)), null, this));
59+
60+
/* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */
61+
private final SysIdRoutine m_sysIdRoutineSteer =
62+
new SysIdRoutine(
63+
new SysIdRoutine.Config(
64+
null, // Use default ramp rate (1 V/s)
65+
Volts.of(7), // Use dynamic voltage of 7 V
66+
null, // Use default timeout (10 s)
67+
// Log state with SignalLogger class
68+
state -> SignalLogger.writeString("SysIdSteer_State", state.toString())),
69+
new SysIdRoutine.Mechanism(
70+
volts -> setControl(m_steerCharacterization.withVolts(volts)), null, this));
71+
72+
/*
73+
* SysId routine for characterizing rotation.
74+
* This is used to find PID gains for the FieldCentricFacingAngle HeadingController.
75+
* See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId.
76+
*/
77+
private final SysIdRoutine m_sysIdRoutineRotation =
78+
new SysIdRoutine(
79+
new SysIdRoutine.Config(
80+
/* This is in radians per second², but SysId only supports "volts per second" */
81+
Volts.of(Math.PI / 6).per(Second),
82+
/* This is in radians per second, but SysId only supports "volts" */
83+
Volts.of(Math.PI),
84+
null, // Use default timeout (10 s)
85+
// Log state with SignalLogger class
86+
state -> SignalLogger.writeString("SysIdRotation_State", state.toString())),
87+
new SysIdRoutine.Mechanism(
88+
output -> {
89+
/* output is actually radians per second, but SysId only supports "volts" */
90+
setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts)));
91+
/* also log the requested output for SysId */
92+
SignalLogger.writeDouble("Rotational_Rate", output.in(Volts));
93+
},
94+
null,
95+
this));
96+
97+
/* The SysId routine to test */
98+
private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation;
99+
100+
/**
101+
* Constructs a CTRE SwerveDrivetrain using the specified constants.
102+
*
103+
* <p>This constructs the underlying hardware devices, so users should not construct the devices
104+
* themselves. If they need the devices, they can access them through getters in the classes.
105+
*
106+
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
107+
* @param modules Constants for each specific module
108+
*/
109+
public CommandSwerveDrivetrain(
110+
SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants<?, ?, ?>... modules) {
111+
super(drivetrainConstants, modules);
112+
if (Utils.isSimulation()) {
113+
startSimThread();
114+
}
115+
}
116+
117+
/**
118+
* Constructs a CTRE SwerveDrivetrain using the specified constants.
119+
*
120+
* <p>This constructs the underlying hardware devices, so users should not construct the devices
121+
* themselves. If they need the devices, they can access them through getters in the classes.
122+
*
123+
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
124+
* @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set to
125+
* 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0.
126+
* @param modules Constants for each specific module
127+
*/
128+
public CommandSwerveDrivetrain(
129+
SwerveDrivetrainConstants drivetrainConstants,
130+
double odometryUpdateFrequency,
131+
SwerveModuleConstants<?, ?, ?>... modules) {
132+
super(drivetrainConstants, odometryUpdateFrequency, modules);
133+
if (Utils.isSimulation()) {
134+
startSimThread();
135+
}
136+
}
137+
138+
/**
139+
* Constructs a CTRE SwerveDrivetrain using the specified constants.
140+
*
141+
* <p>This constructs the underlying hardware devices, so users should not construct the devices
142+
* themselves. If they need the devices, they can access them through getters in the classes.
143+
*
144+
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
145+
* @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set to
146+
* 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0.
147+
* @param odometryStandardDeviation The standard deviation for odometry calculation in the form
148+
* [x, y, theta]ᵀ, with units in meters and radians
149+
* @param visionStandardDeviation The standard deviation for vision calculation in the form [x, y,
150+
* theta]ᵀ, with units in meters and radians
151+
* @param modules Constants for each specific module
152+
*/
153+
public CommandSwerveDrivetrain(
154+
SwerveDrivetrainConstants drivetrainConstants,
155+
double odometryUpdateFrequency,
156+
Matrix<N3, N1> odometryStandardDeviation,
157+
Matrix<N3, N1> visionStandardDeviation,
158+
SwerveModuleConstants<?, ?, ?>... modules) {
159+
super(
160+
drivetrainConstants,
161+
odometryUpdateFrequency,
162+
odometryStandardDeviation,
163+
visionStandardDeviation,
164+
modules);
165+
if (Utils.isSimulation()) {
166+
startSimThread();
167+
}
168+
}
169+
170+
/**
171+
* Returns a command that applies the specified control request to this swerve drivetrain.
172+
*
173+
* @param request Function returning the request to apply
174+
* @return Command to run
175+
*/
176+
public Command applyRequest(Supplier<SwerveRequest> requestSupplier) {
177+
return run(() -> this.setControl(requestSupplier.get()));
178+
}
179+
180+
/**
181+
* Runs the SysId Quasistatic test in the given direction for the routine specified by {@link
182+
* #m_sysIdRoutineToApply}.
183+
*
184+
* @param direction Direction of the SysId Quasistatic test
185+
* @return Command to run
186+
*/
187+
public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
188+
return m_sysIdRoutineToApply.quasistatic(direction);
189+
}
190+
191+
/**
192+
* Runs the SysId Dynamic test in the given direction for the routine specified by {@link
193+
* #m_sysIdRoutineToApply}.
194+
*
195+
* @param direction Direction of the SysId Dynamic test
196+
* @return Command to run
197+
*/
198+
public Command sysIdDynamic(SysIdRoutine.Direction direction) {
199+
return m_sysIdRoutineToApply.dynamic(direction);
200+
}
201+
202+
@Override
203+
public void periodic() {
204+
/*
205+
* Periodically try to apply the operator perspective.
206+
* If we haven't applied the operator perspective before, then we should apply it regardless of DS state.
207+
* This allows us to correct the perspective in case the robot code restarts mid-match.
208+
* Otherwise, only check and apply the operator perspective if the DS is disabled.
209+
* This ensures driving behavior doesn't change until an explicit disable event occurs during testing.
210+
*/
211+
if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
212+
DriverStation.getAlliance()
213+
.ifPresent(
214+
allianceColor -> {
215+
setOperatorPerspectiveForward(
216+
allianceColor == Alliance.Red
217+
? kRedAlliancePerspectiveRotation
218+
: kBlueAlliancePerspectiveRotation);
219+
m_hasAppliedOperatorPerspective = true;
220+
});
221+
}
222+
}
223+
224+
private void startSimThread() {
225+
m_lastSimTime = Utils.getCurrentTimeSeconds();
226+
227+
/* Run simulation at a faster rate so PID gains behave more reasonably */
228+
m_simNotifier =
229+
new Notifier(
230+
() -> {
231+
final double currentTime = Utils.getCurrentTimeSeconds();
232+
double deltaTime = currentTime - m_lastSimTime;
233+
m_lastSimTime = currentTime;
234+
235+
/* use the measured time delta, get battery voltage from WPILib */
236+
updateSimState(deltaTime, RobotController.getBatteryVoltage());
237+
});
238+
m_simNotifier.startPeriodic(kSimLoopPeriod);
239+
}
240+
}

0 commit comments

Comments
 (0)