Skip to content

Commit 38c37c9

Browse files
FIRSTTeam4909FIRSTTeam4909
committed
cleanup and format code
1 parent e4471ba commit 38c37c9

23 files changed

+2363
-2570
lines changed

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

Lines changed: 92 additions & 109 deletions
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,6 @@
66

77
import edu.wpi.first.math.geometry.Pose2d;
88
import edu.wpi.first.math.geometry.Rotation2d;
9-
import edu.wpi.first.math.geometry.Translation2d;
109
import edu.wpi.first.math.util.Units;
1110
import edu.wpi.first.wpilibj.DriverStation;
1211
import edu.wpi.first.wpilibj2.command.Command;
@@ -18,112 +17,96 @@
1817
/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
1918
public class AutoClimbCommand extends Command {
2019

21-
private Pose2d blueStartPose = new Pose2d(7.495, 5.326, Rotation2d.fromDegrees(-90));
22-
private Pose2d blueFarPose = new Pose2d(8.9, 5.026, Rotation2d.fromDegrees(-90));
23-
private Pose2d blueClimbPose = new Pose2d(8.2, 5.026, Rotation2d.fromDegrees(-90));
24-
25-
// var goToClimbStartPose = new DriveToFieldPose(drivetrain, startPose,
26-
// joystick, 3);
27-
28-
// var goToClimbEndPose = new DriveToFieldPose(drivetrain, endPose, joystick,
29-
// .5);
30-
31-
// var goToClimbMidPose = new DriveToFieldPose(drivetrain, ClimbMidPose,
32-
// joystick, 3);
33-
34-
public boolean isBlueAlliance() {
35-
var alliance = DriverStation.getAlliance();
36-
if (alliance.isPresent()) {
37-
return alliance.get() == DriverStation.Alliance.Blue;
38-
}
39-
return false;
40-
}
41-
42-
public static Pose2d fieldCenter = new Pose2d(VisionConstants.aprilTagLayout.getFieldLength() / 2,
43-
VisionConstants.aprilTagLayout.getFieldWidth() / 2, Rotation2d.fromDegrees(0));
44-
45-
public static Pose2d rotatePoseAboutFieldCenter(Pose2d pose) {
46-
47-
var fieldCentricBluePose = pose.relativeTo(fieldCenter);
48-
var fieldCentriclRedPose = fieldCentricBluePose.rotateBy(Rotation2d.fromDegrees(180));
49-
var redPose = fieldCentriclRedPose.relativeTo(new Pose2d(-VisionConstants.aprilTagLayout.getFieldLength() / 2,
50-
-VisionConstants.aprilTagLayout.getFieldWidth() / 2, Rotation2d.fromDegrees(0)));
51-
return redPose;
52-
}
53-
54-
public Command driveCommand;
55-
56-
57-
private CommandXboxController m_js;
58-
private CommandSwerveDrivetrain m_drivetrain;
59-
60-
public AutoClimbCommand(CommandXboxController js, CommandSwerveDrivetrain drivetrain) {
61-
m_js = js;
62-
m_drivetrain = drivetrain;
63-
}
64-
65-
public boolean poseEqualsPoseWithDelta(Pose2d a, Pose2d b) {
66-
return a.getTranslation().getDistance(b.getTranslation()) < Units.inchesToMeters(2)
67-
&& (a.getRotation().getDegrees() - b.getRotation().getDegrees() < 5);
68-
}
69-
70-
71-
// Called when the command is initially scheduled.
72-
@Override
73-
public void initialize() {
74-
75-
System.out.println("isBlue " + isBlueAlliance());
76-
77-
Pose2d startPose;
78-
Pose2d farPose;
79-
Pose2d climbPose;
80-
if (isBlueAlliance()) {
81-
startPose = blueStartPose;
82-
farPose = blueFarPose;
83-
climbPose = blueClimbPose;
84-
} else {
85-
startPose = rotatePoseAboutFieldCenter(blueStartPose);
86-
farPose = rotatePoseAboutFieldCenter(blueFarPose);
87-
climbPose = rotatePoseAboutFieldCenter(blueClimbPose);
88-
}
89-
90-
Command startPoseCommand = new DriveToFieldPose(m_drivetrain, startPose, m_js, 3);
91-
Command farPoseCommand = new DriveToFieldPose(m_drivetrain, farPose, m_js, .5);
92-
Command climbPoseCommand = new DriveToFieldPose(m_drivetrain, climbPose, m_js, 1.5);
93-
94-
if (!poseEqualsPoseWithDelta(m_drivetrain.getState().Pose, startPose ) && !poseEqualsPoseWithDelta(m_drivetrain.getState().Pose, climbPose )) {
95-
driveCommand = startPoseCommand;
96-
} else if(poseEqualsPoseWithDelta(m_drivetrain.getState().Pose, startPose)) {
97-
driveCommand = farPoseCommand;
98-
} else {
99-
driveCommand = climbPoseCommand;
100-
}
101-
102-
103-
// new ConditionalCommand(goToClimbStartPose,
104-
// new ConditionalCommand(goToClimbEndPose, goToClimbMidPose,()->
105-
// !poseEqualsPoseWithDelta(drivetrain.getState().Pose, endPose))
106-
// , ()-> !poseEqualsPoseWithDelta(drivetrain.getState().Pose, startPose ) &&
107-
// ))
108-
driveCommand.initialize();
109-
}
110-
111-
// Called every time the scheduler runs while the command is scheduled.
112-
@Override
113-
public void execute() {
114-
driveCommand.execute();
115-
116-
}
117-
118-
// Called once the command ends or is interrupted.
119-
@Override
120-
public void end(boolean interrupted) {
121-
driveCommand.end(interrupted);
122-
}
123-
124-
// Returns true when the command should end.
125-
@Override
126-
public boolean isFinished() {
127-
return false;
128-
}
20+
private Pose2d blueStartPose = new Pose2d(7.495, 5.326, Rotation2d.fromDegrees(-90));
21+
private Pose2d blueFarPose = new Pose2d(8.9, 5.026, Rotation2d.fromDegrees(-90));
22+
private Pose2d blueClimbPose = new Pose2d(8.2, 5.026, Rotation2d.fromDegrees(-90));
23+
24+
public boolean isBlueAlliance() {
25+
var alliance = DriverStation.getAlliance();
26+
if (alliance.isPresent()) {
27+
return alliance.get() == DriverStation.Alliance.Blue;
28+
}
29+
return false;
30+
}
31+
32+
public static Pose2d fieldCenter = new Pose2d(VisionConstants.aprilTagLayout.getFieldLength() / 2,
33+
VisionConstants.aprilTagLayout.getFieldWidth() / 2, Rotation2d.fromDegrees(0));
34+
35+
public static Pose2d rotatePoseAboutFieldCenter(Pose2d pose) {
36+
37+
var fieldCentricBluePose = pose.relativeTo(fieldCenter);
38+
var fieldCentriclRedPose = fieldCentricBluePose.rotateBy(Rotation2d.fromDegrees(180));
39+
var redPose = fieldCentriclRedPose.relativeTo(new Pose2d(-VisionConstants.aprilTagLayout.getFieldLength() / 2,
40+
-VisionConstants.aprilTagLayout.getFieldWidth() / 2, Rotation2d.fromDegrees(0)));
41+
return redPose;
42+
}
43+
44+
public Command driveCommand;
45+
46+
private CommandXboxController m_js;
47+
private CommandSwerveDrivetrain m_drivetrain;
48+
49+
public AutoClimbCommand(CommandXboxController js, CommandSwerveDrivetrain drivetrain) {
50+
m_js = js;
51+
m_drivetrain = drivetrain;
52+
}
53+
54+
public boolean poseEqualsPoseWithDelta(Pose2d a, Pose2d b) {
55+
return a.getTranslation().getDistance(b.getTranslation()) < Units.inchesToMeters(2)
56+
&& (a.getRotation().getDegrees() - b.getRotation().getDegrees() < 5);
57+
}
58+
59+
// Called when the command is initially scheduled.
60+
@Override
61+
public void initialize() {
62+
63+
System.out.println("isBlue " + isBlueAlliance());
64+
65+
Pose2d startPose;
66+
Pose2d farPose;
67+
Pose2d climbPose;
68+
if (isBlueAlliance()) {
69+
startPose = blueStartPose;
70+
farPose = blueFarPose;
71+
climbPose = blueClimbPose;
72+
} else {
73+
startPose = rotatePoseAboutFieldCenter(blueStartPose);
74+
farPose = rotatePoseAboutFieldCenter(blueFarPose);
75+
climbPose = rotatePoseAboutFieldCenter(blueClimbPose);
76+
}
77+
78+
Command startPoseCommand = new DriveToFieldPose(m_drivetrain, startPose, m_js, 3);
79+
Command farPoseCommand = new DriveToFieldPose(m_drivetrain, farPose, m_js, .5);
80+
Command climbPoseCommand = new DriveToFieldPose(m_drivetrain, climbPose, m_js, 1.5);
81+
82+
if (!poseEqualsPoseWithDelta(m_drivetrain.getState().Pose, startPose)
83+
&& !poseEqualsPoseWithDelta(m_drivetrain.getState().Pose, climbPose)) {
84+
driveCommand = startPoseCommand;
85+
} else if (poseEqualsPoseWithDelta(m_drivetrain.getState().Pose, startPose)) {
86+
driveCommand = farPoseCommand;
87+
} else {
88+
driveCommand = climbPoseCommand;
89+
}
90+
91+
driveCommand.initialize();
92+
}
93+
94+
// Called every time the scheduler runs while the command is scheduled.
95+
@Override
96+
public void execute() {
97+
driveCommand.execute();
98+
99+
}
100+
101+
// Called once the command ends or is interrupted.
102+
@Override
103+
public void end(boolean interrupted) {
104+
driveCommand.end(interrupted);
105+
}
106+
107+
// Returns true when the command should end.
108+
@Override
109+
public boolean isFinished() {
110+
return false;
111+
}
129112
}

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

Lines changed: 14 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -16,22 +16,24 @@
1616
import edu.wpi.first.wpilibj.RobotBase;
1717

1818
/**
19-
* This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running
20-
* on a roboRIO. Change the value of "simMode" to switch between "sim" (physics sim) and "replay"
19+
* This class defines the runtime mode used by AdvantageKit. The mode is always
20+
* "real" when running
21+
* on a roboRIO. Change the value of "simMode" to switch between "sim" (physics
22+
* sim) and "replay"
2123
* (log replay from a file).
2224
*/
2325
public final class Constants {
24-
public static final Mode simMode = Mode.SIM;
25-
public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode;
26+
public static final Mode simMode = Mode.SIM;
27+
public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode;
2628

27-
public static enum Mode {
28-
/** Running on a real robot. */
29-
REAL,
29+
public static enum Mode {
30+
/** Running on a real robot. */
31+
REAL,
3032

31-
/** Running a physics simulator. */
32-
SIM,
33+
/** Running a physics simulator. */
34+
SIM,
3335

34-
/** Replaying from a log file. */
35-
REPLAY
36-
}
36+
/** Replaying from a log file. */
37+
REPLAY
38+
}
3739
}

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

Lines changed: 14 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -6,17 +6,19 @@
66

77
public class DrivetrainTelemetry {
88

9-
/** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */
10-
public void telemeterize(SwerveDriveState state) {
9+
/**
10+
* Accept the swerve drive state and telemeterize it to SmartDashboard and
11+
* SignalLogger.
12+
*/
13+
public void telemeterize(SwerveDriveState state) {
1114

12-
/* Telemeterize the swerve drive state */
13-
Logger.recordOutput("DriveState/Pose", state.Pose);
14-
Logger.recordOutput("DriveState/Speeds", state.Speeds);
15-
Logger.recordOutput("DriveState/ModuleStates", state.ModuleStates);
16-
Logger.recordOutput("DriveState/ModuleTargets", state.ModuleTargets);
17-
Logger.recordOutput("DriveState/ModulePositions", state.ModulePositions);
18-
Logger.recordOutput("DriveState/Timestamp", state.Timestamp);
19-
Logger.recordOutput("DriveState/OdometryPeriod", 1.0 / state.OdometryPeriod);
20-
}
15+
/* Telemeterize the swerve drive state */
16+
Logger.recordOutput("DriveState/Pose", state.Pose);
17+
Logger.recordOutput("DriveState/Speeds", state.Speeds);
18+
Logger.recordOutput("DriveState/ModuleStates", state.ModuleStates);
19+
Logger.recordOutput("DriveState/ModuleTargets", state.ModuleTargets);
20+
Logger.recordOutput("DriveState/ModulePositions", state.ModulePositions);
21+
Logger.recordOutput("DriveState/Timestamp", state.Timestamp);
22+
Logger.recordOutput("DriveState/OdometryPeriod", 1.0 / state.OdometryPeriod);
23+
}
2124
}
22-

0 commit comments

Comments
 (0)