Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: Team_2648:17143ffffb071b402b955d17aff62b5413ef517f
Branches Tags
Team_2648:main Team_2648:faster_elevator Team_2648:maybe_fix_field_oriented Team_2648:brad_elevator_control_rework Team_2648:kraken_swerve Team_2648:2025-2-1-setup
..
compare: Team_2648:8cc14b4cc3a74cfd416b5dc8b139725709eb14ad
Branches Tags
Team_2648:main Team_2648:faster_elevator Team_2648:maybe_fix_field_oriented Team_2648:brad_elevator_control_rework Team_2648:kraken_swerve Team_2648:2025-2-1-setup

2 Commits
17143ffffb ... 8cc14b4cc3

Author SHA1 Message Date
Team 2648
8cc14b4cc3 Merge branch 'main' of https://git.coldlightalchemist.com/Team_2648/2025_Robot_Code 2025-03-21 19:06:55 -04:00
Team 2648
c4134ef713 I AM STUPID MAN 2025-03-21 19:06:45 -04:00
2 changed files with 16 additions and 4 deletions
Expand all files Collapse all files

12
src/main/java/frc/robot/subsystems/Manipulator.java
View File

@@ -73,6 +73,18 @@ public class Manipulator extends SubsystemBase {
); );
}).unless(() -> !coralBeamBreak.get()) }).unless(() -> !coralBeamBreak.get())
.until(() -> !coralBeamBreak.get()); .until(() -> !coralBeamBreak.get());
/*
return run(() -> {
if(getCoralBeamBreak()) {
manipulatorMotor.set(
speed.getAsDouble()
);
} else {
manipulatorMotor.set(
speed.getAsDouble()
);
}
*/
} }
public Command retractCommand(DoubleSupplier retractSpeed){ public Command retractCommand(DoubleSupplier retractSpeed){

8
src/main/java/frc/robot/subsystems/Vision.java
View File

@@ -84,9 +84,9 @@ public class Vision{
// System.out.println(gyroBuffer.getSample(timestamp)); // System.out.println(gyroBuffer.getSample(timestamp));
double distance2d = Units.inchesToMeters(totalDist) * Math.cos(-Units.degreesToRadians( camPose[1]) + Units.degreesToRadians(ty)); double distance2d = Units.inchesToMeters(totalDist) * Math.cos(-camPose[1] + Units.degreesToRadians(ty));
Rotation2d camToTagRotation = new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(Rotation2d.fromRadians(Units.degreesToRadians(camPose[2])).plus(Rotation2d.fromRadians(Units.degreesToRadians(-tx)))); Rotation2d camToTagRotation = new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(Rotation2d.fromRadians(camPose[2]).plus(Rotation2d.fromRadians(Units.degreesToRadians(-tx))));
Pose2d tagPose2d = new Pose2d(Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][0]), Pose2d tagPose2d = new Pose2d(Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][0]),
Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][1]), Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][1]),
@@ -97,8 +97,8 @@ public class Vision{
.getTranslation(); .getTranslation();
Pose2d robotPose = new Pose2d( Pose2d robotPose = new Pose2d(
fieldToCameraTranslation, fieldToCameraTranslation,
new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(new Rotation2d(Units.degreesToRadians( camPose[2])))) new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(new Rotation2d(camPose[2])))
.transformBy(new Transform2d(new Pose2d(new Translation2d(Units.inchesToMeters(camPose[3]), Units.inchesToMeters(camPose[4])), new Rotation2d(Units.degreesToRadians(camPose[2]))), Pose2d.kZero)); .transformBy(new Transform2d(new Pose2d(new Translation2d(Units.inchesToMeters(camPose[3]), Units.inchesToMeters(camPose[4])), new Rotation2d(camPose[2])), Pose2d.kZero));
robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180)))); robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))));