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

I AM STUPID MAN

Browse Source
This commit is contained in:
Team 2648 2025-03-21 19:06:45 -04:00
parent 3e6fa986e7
commit c4134ef713
2 changed files with 16 additions and 4 deletions
Show Stats Download Patch File Download Diff File 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())
.until(() -> !coralBeamBreak.get());
/*
return run(() -> {
if(getCoralBeamBreak()) {
manipulatorMotor.set(
speed.getAsDouble()
);
} else {
manipulatorMotor.set(
speed.getAsDouble()
);
}
*/
}
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));
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]),
Units.inchesToMeters(VisionConstants.globalTagCoords[tagID][1]),
@ -97,8 +97,8 @@ public class Vision{
.getTranslation();
Pose2d robotPose = new Pose2d(
fieldToCameraTranslation,
new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))).plus(new Rotation2d(Units.degreesToRadians( 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));
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(camPose[2])), Pose2d.kZero));
robotPose = new Pose2d(robotPose.getTranslation(), new Rotation2d(Units.degreesToRadians(gyroBuffer.getSample(timestamp).get()+Units.degreesToRadians(180))));