Skip to content

Commit 9169771

Browse files
StarbuckBaristaStarbuckBarista
committed
Arena Setup & Angle Conversions
1 parent 22b9874 commit 9169771

5 files changed

Lines changed: 37 additions & 24 deletions

File tree

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

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -9,12 +9,13 @@
99
import frc.robot.commands.swerve.Drive;
1010
import frc.robot.commands.swerve.ZeroGyro;
1111

12+
import org.ironmaple.simulation.SimulatedArena;
1213
import org.littletonrobotics.junction.Logger;
1314

1415
import edu.wpi.first.math.MathUtil;
15-
import edu.wpi.first.math.geometry.Pose2d;
1616
import edu.wpi.first.math.geometry.Pose3d;
1717
import edu.wpi.first.math.geometry.Rotation3d;
18+
import edu.wpi.first.math.util.Units;
1819
import edu.wpi.first.wpilibj.RobotBase;
1920
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
2021
import frc.robot.commands.ConfirmAlliance;
@@ -67,8 +68,10 @@ public RobotContainer () {
6768
() -> this.funnel.getVelocity() * FunnelConstants.PITCH_DIAMETER * Math.PI
6869
);
6970

70-
SmartDashboard.putData("Load Funnel", Commands.runOnce(() -> gamePieceSim.loadFunnel(), gamePieceSim));
71-
SmartDashboard.putData("Remove Coral", Commands.runOnce(() -> gamePieceSim.removeCoral(), gamePieceSim));
71+
SmartDashboard.putData("Reset Field", Commands.runOnce(() -> SimulatedArena.getInstance().resetFieldForAuto()).ignoringDisable(true));
72+
SmartDashboard.putData("Load Coral", Commands.runOnce(() -> gamePieceSim.loadCoral(), gamePieceSim).ignoringDisable(true));
73+
SmartDashboard.putData("Preload Coral", Commands.runOnce(() -> gamePieceSim.preloadCoral(), gamePieceSim).ignoringDisable(true));
74+
SmartDashboard.putData("Remove Coral", Commands.runOnce(() -> gamePieceSim.removeCoral(), gamePieceSim).ignoringDisable(true));
7275
}
7376

7477
this.configureBindings();
@@ -102,7 +105,7 @@ public void updateComponents () {
102105
new Pose3d(0.0, 0.0, MathUtil.clamp(this.elevator.getHeight(), ElevatorConstants.FIRST_STAGE_TRANSITION, ElevatorConstants.SECOND_STAGE_TRANSITION), new Rotation3d()),
103106
new Pose3d(0.0, 0.0, Math.max(this.elevator.getHeight(), ElevatorConstants.FIRST_STAGE_TRANSITION), new Rotation3d()),
104107
new Pose3d(0.0, 0.0, this.elevator.getHeight(), new Rotation3d()),
105-
new Pose3d(0.249, -0.114, 0.337 + this.elevator.getHeight(), new Rotation3d(0.0, this.endEffector.getAlgaeWristPosition() * 2 * Math.PI, 0.0)),
108+
new Pose3d(0.249, -0.114, 0.337 + this.elevator.getHeight(), new Rotation3d(0.0, Units.rotationsToRadians(this.endEffector.getAlgaeWristPosition()), 0.0)),
106109
});
107110
}
108111
}

‎src/main/java/frc/robot/subsystems/end_effector/EndEffectorIOSim.java‎

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55

66
import edu.wpi.first.math.system.plant.DCMotor;
77
import edu.wpi.first.math.system.plant.LinearSystemId;
8+
import edu.wpi.first.math.util.Units;
89
import edu.wpi.first.wpilibj.simulation.FlywheelSim;
910
import edu.wpi.first.wpilibj.simulation.RoboRioSim;
1011
import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
@@ -46,10 +47,10 @@ public class EndEffectorIOSim extends EndEffectorIOVortex {
4647
DCMotor.getNeoVortex(1),
4748
(125.0 / 1.0) * (42.0 / 24.0),
4849
0.25,
49-
-Math.PI / 6,
50-
Math.PI / 2,
50+
Units.degreesToRadians(-30.0),
51+
Units.degreesToRadians(90.0),
5152
true,
52-
Math.PI / 2
53+
Units.degreesToRadians(90.0)
5354
);
5455

5556
@Override
@@ -77,7 +78,7 @@ public void updateInputs(EndEffectorIOInputs inputs) {
7778
);
7879

7980
this.algaeWristMotor.iterate(
80-
-this.algaeWrist.getVelocityRadPerSec() / (2 * Math.PI),
81+
Units.radiansToRotations(-this.algaeWrist.getVelocityRadPerSec()),
8182
RoboRioSim.getVInVoltage(),
8283
0.02
8384
);

‎src/main/java/frc/robot/subsystems/swerve/vision/VisionConstants.java‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -3,16 +3,17 @@
33
import edu.wpi.first.math.geometry.Rotation3d;
44
import edu.wpi.first.math.geometry.Transform3d;
55
import edu.wpi.first.math.geometry.Translation3d;
6+
import edu.wpi.first.math.util.Units;
67

78
public class VisionConstants {
89

910
public static final Transform3d LEFT_TRANSFORM = new Transform3d(
1011
new Translation3d(0.202, -0.278, 0.494),
11-
new Rotation3d(0.0, -27.0 * (Math.PI / 180.0), 20.0 * (Math.PI / 180.0))
12+
new Rotation3d(0.0, Units.degreesToRadians(-27.0), Units.degreesToRadians(20.0))
1213
);
1314

1415
public static final Transform3d RIGHT_TRANSFORM = new Transform3d(
1516
new Translation3d(0.202, 0.278, 0.494),
16-
new Rotation3d(0.0, -27.0 * (Math.PI / 180.0), -20.0 * (Math.PI / 180.0))
17+
new Rotation3d(0.0, Units.degreesToRadians(-27.0), Units.degreesToRadians(-20.0))
1718
);
1819
}

‎src/main/java/frc/robot/util/BuildConstants.java‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -7,12 +7,12 @@ public final class BuildConstants {
77
public static final String MAVEN_GROUP = "";
88
public static final String MAVEN_NAME = "Reefscape2025";
99
public static final String VERSION = "unspecified";
10-
public static final int GIT_REVISION = 333;
11-
public static final String GIT_SHA = "6ae08e3bdbd63e137976e4ee8a21ca78e3734097";
12-
public static final String GIT_DATE = "2025-05-28 23:00:03 CDT";
10+
public static final int GIT_REVISION = 334;
11+
public static final String GIT_SHA = "22b98747ede7c39096c463f5e3bf606ab8be986c";
12+
public static final String GIT_DATE = "2025-05-29 04:52:43 CDT";
1313
public static final String GIT_BRANCH = "only_simulation";
14-
public static final String BUILD_DATE = "2025-05-29 04:42:40 CDT";
15-
public static final long BUILD_UNIX_TIME = 1748511760283L;
14+
public static final String BUILD_DATE = "2025-05-29 05:24:24 CDT";
15+
public static final long BUILD_UNIX_TIME = 1748514264943L;
1616
public static final int DIRTY = 1;
1717

1818
private BuildConstants(){}

‎src/main/java/frc/robot/util/GamePieceSim.java‎

Lines changed: 17 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
import edu.wpi.first.math.geometry.Translation2d;
1919
import edu.wpi.first.math.geometry.Translation3d;
2020
import edu.wpi.first.math.kinematics.ChassisSpeeds;
21+
import edu.wpi.first.math.util.Units;
2122
import edu.wpi.first.wpilibj2.command.SubsystemBase;
2223

2324
public class GamePieceSim extends SubsystemBase {
@@ -95,10 +96,10 @@ public void periodic () {
9596
);
9697
} else if (this.coralTransform.getX() > 0.130) {
9798

98-
this.coralTransform = this.project(this.coralTransform, this.coralIntakeVelocitySupplier.get() * 0.02, new Rotation3d(0.0, 30.0 * (Math.PI / 180.0), 0.0));
99+
this.coralTransform = this.project(this.coralTransform, this.coralIntakeVelocitySupplier.get() * 0.02, new Rotation3d(0.0, Units.degreesToRadians(30.0), 0.0));
99100
} else {
100101

101-
this.coralTransform = this.project(this.coralTransform, this.funnelVelocitySupplier.get() * 0.02, new Rotation3d(0.0, 30.0 * (Math.PI / 180.0), 0.0));
102+
this.coralTransform = this.project(this.coralTransform, this.funnelVelocitySupplier.get() * 0.02, new Rotation3d(0.0, Units.degreesToRadians(30.0), 0.0));
102103
}
103104
} else {
104105

@@ -115,13 +116,13 @@ public void periodic () {
115116

116117
if (this.coralTransform.getZ() > 0.178) {
117118

118-
this.coralTransform = this.project(this.coralTransform, this.fallingVelocity * 0.02, new Rotation3d(0.0, 30.0 * (Math.PI / 180.0), 0.0));
119-
this.coralTransform = this.project(this.coralTransform, this.fallingGravity * 0.02, new Rotation3d(0.0, 90.0 * (Math.PI / 180.0), 0.0));
119+
this.coralTransform = this.project(this.coralTransform, this.fallingVelocity * 0.02, new Rotation3d(0.0, Units.degreesToRadians(30.0), 0.0));
120+
this.coralTransform = this.project(this.coralTransform, this.fallingGravity * 0.02, new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0));
120121
this.fallingGravity += (9.8 * 0.02);
121122
} else {
122123

123124
this.coralStatus = CoralStatus.STUCK;
124-
this.coralTransform = new Transform3d(new Translation3d(0.216, 0.0, 0.076), new Rotation3d(0.0, 0.0, Math.PI / 2.0));
125+
this.coralTransform = new Transform3d(new Translation3d(0.216, 0.0, 0.076), new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0)));
125126
}
126127
}
127128

@@ -133,31 +134,38 @@ public void periodic () {
133134

134135
if (this.coralStatus == CoralStatus.INDEXING) {
135136

136-
Translation3d elevatorTranslation = new Translation3d(0, 0, this.elevatorHeightSupplier.get() - 0.139);
137+
Translation3d elevatorTranslation = new Translation3d(0.0, 0.0, this.elevatorHeightSupplier.get() - 0.139);
137138
transform = transform.plus(new Transform3d(elevatorTranslation, new Rotation3d()));
138139
}
139140

140141
coral.add(new Pose3d(this.robotPoseSupplier.get()).plus(transform));
141142
}
142143

143144
Logger.recordOutput("Visualization/Coral", coral.toArray(new Pose3d[0]));
145+
Logger.recordOutput("Visualization/Algae", SimulatedArena.getInstance().getGamePiecesArrayByType("Algae"));
144146
}
145147

146-
public void loadFunnel () {
148+
public void loadCoral () {
147149

148150
if (this.coralStatus == null) {
149151

150152
this.coralStatus = CoralStatus.FUNNELING;
151-
this.coralTransform = new Transform3d(new Translation3d(-0.145, 0.0, 0.495), new Rotation3d(0.0, 30.0 * (Math.PI / 180.0), 0.0));
153+
this.coralTransform = new Transform3d(new Translation3d(-0.145, 0.0, 0.495), new Rotation3d(0.0, Units.degreesToRadians(30.0), 0.0));
152154
}
153155
}
154156

157+
public void preloadCoral () {
158+
159+
this.coralStatus = CoralStatus.INDEXING;
160+
this.coralTransform = new Transform3d(new Translation3d(0.214, 0.0, 0.269), new Rotation3d(0.0, Units.degreesToRadians(30.0), 0.0));
161+
}
162+
155163
public void removeCoral () {
156164

157165
this.coralStatus = null;
158166
}
159167

160-
public boolean getCoralBeambreak () {
168+
public static boolean getCoralBeambreak () {
161169

162170
return coralBeambreak;
163171
}

0 commit comments

Comments
 (0)