Skip to content

Commit b2176b8

Browse files
Vignesh1234587Vignesh1234587
committed
path fault
1 parent b87e886 commit b2176b8

File tree

6 files changed

+105
-31
lines changed

6 files changed

+105
-31
lines changed

‎src/main/deploy/pathplanner/paths/$s!c4.path

+5-5
Original file line numberDiff line numberDiff line change
@@ -16,12 +16,12 @@
1616
},
1717
{
1818
"anchor": {
19-
"x": 7.07383441672713,
20-
"y": 2.3836410004658575
19+
"x": 6.535619743411246,
20+
"y": 2.076352426858445
2121
},
2222
"prevControl": {
23-
"x": 6.405905953466024,
24-
"y": 2.3836410004658575
23+
"x": 5.86769128015014,
24+
"y": 2.076352426858445
2525
},
2626
"nextControl": null,
2727
"isLocked": false,
@@ -49,7 +49,7 @@
4949
"rotateFast": false
5050
},
5151
"reversed": false,
52-
"folder": "$s!p-c5-c4",
52+
"folder": null,
5353
"previewStartingState": {
5454
"rotation": -43.53119928561413,
5555
"velocity": 0

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

+6-6
Original file line numberDiff line numberDiff line change
@@ -3,14 +3,14 @@
33
/** Automatically generated file containing build version information. */
44
public final class BuildConstants {
55
public static final String MAVEN_GROUP = "";
6-
public static final String MAVEN_NAME = "2024RobotCode";
6+
public static final String MAVEN_NAME = "2024 Robot Code";
77
public static final String VERSION = "unspecified";
8-
public static final int GIT_REVISION = 314;
9-
public static final String GIT_SHA = "dbfb048d3f7aa479526221e9947ca94220ee5472";
10-
public static final String GIT_DATE = "2024-07-28 19:57:18 EDT";
8+
public static final int GIT_REVISION = 317;
9+
public static final String GIT_SHA = "b87e886bb4404f32a660af7d73877f5015d1e1f5";
10+
public static final String GIT_DATE = "2024-08-14 00:51:38 EDT";
1111
public static final String GIT_BRANCH = "note-filtering";
12-
public static final String BUILD_DATE = "2024-08-02 11:57:52 EDT";
13-
public static final long BUILD_UNIX_TIME = 1722614272911L;
12+
public static final String BUILD_DATE = "2024-08-14 20:34:11 EDT";
13+
public static final long BUILD_UNIX_TIME = 1723682051564L;
1414
public static final int DIRTY = 1;
1515

1616
private BuildConstants() {}

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

+1-1
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@ public static Mode getMode() {
3838
};
3939
}
4040

41-
public static final Mode currentMode = Mode.SIM;
41+
public static final Mode currentMode = Mode.REAL;
4242
public static final boolean tuningMode = true;
4343
public static final String CANBUS = "CAN Bus 2";
4444
public static final double LOOP_PERIOD_SECS = 0.02;

‎src/main/java/frc/robot/commands/AimbotTele.java

+3-3
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,7 @@ public AimbotTele(
5656

5757
switch (Constants.currentMode) {
5858
case REAL:
59-
gains[0] = 3.14;
59+
gains[0] = 3.7;
6060
gains[1] = 0;
6161
gains[2] = 0;
6262
break;
@@ -78,7 +78,7 @@ public AimbotTele(
7878
}
7979

8080
pid = new PIDController(gains[0], gains[1], gains[2], 0.02);
81-
pid.setTolerance(3);
81+
pid.setTolerance(2);
8282
pid.enableContinuousInput(-180, 180);
8383
}
8484

@@ -115,7 +115,7 @@ public void angleShooter() {
115115
double shootingSpeed = calculateShooterSpeed(Units.metersToFeet(distanceToSpeakerMeter));
116116

117117
shooter.setFlywheelRPMs(shootingSpeed, shootingSpeed + 100);
118-
} else shooter.setFlywheelRPMs(5400 - 200, 5400 - 400);
118+
} else shooter.setFlywheelRPMs(5400 - 100, 5400 - 800);
119119
pivot.setPivotGoal(calculatePivotAngleDeg(distanceToSpeakerMeter));
120120
}
121121

‎src/main/java/frc/robot/commands/AlignToNoteAuto.java

+17-11
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,6 @@
55
package frc.robot.commands;
66

77
import com.pathplanner.lib.auto.AutoBuilder;
8-
import edu.wpi.first.math.geometry.Pose2d;
98
import edu.wpi.first.math.geometry.Rotation2d;
109
import edu.wpi.first.math.geometry.Translation2d;
1110
import edu.wpi.first.wpilibj2.command.Command;
@@ -78,21 +77,28 @@ public void initialize() {
7877
pivot.setPivotGoal(Constants.PivotConstants.INTAKE_SETPOINT_DEG);
7978
targetNoteLocation = noteLocations.get(drive.getTargetNote());
8079
useGeneratedPathCommand =
81-
drive.getCachedNoteLocation().getDistance(targetNoteLocation) < 1.323
80+
drive.getCachedNoteLocation().getDistance(targetNoteLocation) < 2.5
8281
&& drive.getCachedNoteLocation() != null;
82+
Logger.recordOutput(
83+
"cached note distance ", drive.getCachedNoteLocation().getDistance(targetNoteLocation));
84+
Logger.recordOutput("useGeneratedPath command", useGeneratedPathCommand);
8385
// useGeneratedPathCommand = false;
84-
generatedPathCommand = AutoBuilder.followPath(drive.generatePathToNote());
86+
// generatedPathCommand = AutoBuilder.followPath(drive.generatePathToNote());
8587
if (useGeneratedPathCommand) {
88+
generatedPathCommand = AutoBuilder.followPath(drive.generatePathToNote());
8689

8790
generatedPathCommand.initialize();
8891
} else {
89-
targetNoteRotation =
90-
new Rotation2d(
91-
targetNoteLocation.getX() - drive.getPose().getX(),
92-
targetNoteLocation.getY() - drive.getPose().getY());
92+
// targetNoteRotation =
93+
// new Rotation2d(
94+
// targetNoteLocation.getX() - drive.getPose().getX(),
95+
// targetNoteLocation.getY() - drive.getPose().getY());
96+
// targetNotePathCommand =
97+
// drive.generateTrajectory(
98+
// new Pose2d(targetNoteLocation, targetNoteRotation), 3, 2.45, 100, 180, 0.5);
9399
targetNotePathCommand =
94-
drive.generateTrajectory(
95-
new Pose2d(targetNoteLocation, targetNoteRotation), 3, 2.45, 100, 180, 0.5);
100+
AutoBuilder.followPath(drive.generatePathToNoteBlind(targetNoteLocation));
101+
96102
targetNotePathCommand.initialize();
97103
}
98104
}
@@ -126,7 +132,7 @@ public boolean isFinished() {
126132
Logger.recordOutput("isFinished align note", shooter.seesNote());
127133
// return false;
128134
return shooter.seesNote() == NoteState.SENSOR
129-
|| shooter.seesNote() == NoteState.CURRENT
130-
|| finished;
135+
|| shooter.seesNote() == NoteState.CURRENT
136+
|| finished;
131137
}
132138
}

‎src/main/java/frc/robot/subsystems/drive/Drive.java

+73-5
Original file line numberDiff line numberDiff line change
@@ -244,14 +244,19 @@ public void periodic() {
244244
0,
245245
0,
246246
0);
247+
Logger.recordOutput(
248+
"vision something", DriverStation.getAlliance().isPresent() && visionInputs.aTV);
249+
Logger.recordOutput("isDisabled", DriverStation.isDisabled());
250+
247251
if (DriverStation.getAlliance().isPresent() && visionInputs.aTV) {
252+
Logger.recordOutput(
253+
"tags > 1 or disabled ", visionInputs.tagCount > 1 || DriverStation.isDisabled());
254+
248255
if (visionInputs.tagCount > 1 || DriverStation.isDisabled()) {
249256
visionLogic();
250257
} else {
251258
mt2TagFiltering();
252259
}
253-
254-
255260
}
256261

257262
Logger.recordOutput("note time", getCachedNoteTime());
@@ -348,7 +353,7 @@ public void visionLogic() {
348353
double yMeterStds;
349354
double headingDegStds;
350355

351-
double poseDifference = getVisionPoseDifference(limelightMeasurement.pose);
356+
// double poseDifference = getVisionPoseDifference(limelightMeasurement.pose);
352357

353358
boolean isFlipped =
354359
DriverStation.getAlliance().isPresent()
@@ -360,11 +365,11 @@ public void visionLogic() {
360365
xMeterStds = 0.7;
361366
yMeterStds = 0.7;
362367
headingDegStds = 8;
363-
} else if (limelightMeasurement.tagCount == 1 && poseDifference < 0.5) {
368+
} else if (limelightMeasurement.tagCount == 1) { // && poseDifference < 0.5
364369
xMeterStds = 5;
365370
yMeterStds = 5;
366371
headingDegStds = 30;
367-
} else if (limelightMeasurement.tagCount == 1 && poseDifference < 3) {
372+
} else if (limelightMeasurement.tagCount == 1) { // && poseDifference < 3
368373
xMeterStds = 11.43;
369374
yMeterStds = 11.43;
370375
headingDegStds = 9999;
@@ -792,6 +797,69 @@ public PathPlannerPath generatePathToNote() {
792797
}
793798
}
794799

800+
public PathPlannerPath generatePathToNoteBlind(Translation2d targetNoteLocation) {
801+
// if (visionInputs.iTX != 0.0) {
802+
// double taThreshold = 0;
803+
// if (visionInputs.iTA >= taThreshold) {
804+
// lastNoteLocT2d.translation =
805+
// calculateNotePositionFieldRelative().getTranslation().toTranslation2d();
806+
// lastNoteLocT2d.time = Timer.getFPGATimestamp();
807+
// }
808+
// }
809+
// Pose2d targetNoteLocation = noteLocations.get(drive.getTargetNote());
810+
811+
Rotation2d targetRotation;
812+
Logger.recordOutput("note timeess", getCachedNoteTime());
813+
// led.setState(LED_STATE.FLASHING_RED);
814+
// Translation2d targetNoteLocation = getCachedNoteLocation();
815+
Logger.recordOutput("better translate", targetNoteLocation);
816+
if (noteImageIsNew()) {
817+
818+
targetRotation =
819+
new Rotation2d(
820+
targetNoteLocation.getX() - getPose().getX(),
821+
targetNoteLocation.getY() - getPose().getY());
822+
List<Translation2d> pointsToNote;
823+
if (DriverStation.getAlliance().get().equals(Alliance.Blue)) {
824+
Logger.recordOutput(
825+
"goal point blue",
826+
new Pose2d(targetNoteLocation.getX(), targetNoteLocation.getY(), targetRotation));
827+
pointsToNote =
828+
PathPlannerPath.bezierFromPoses(
829+
new Pose2d(getPose().getX(), getPose().getY(), targetRotation),
830+
new Pose2d(targetNoteLocation.getX(), targetNoteLocation.getY(), targetRotation));
831+
} else {
832+
Logger.recordOutput(
833+
"goal point red",
834+
new Pose2d(targetNoteLocation.getX(), targetNoteLocation.getY(), targetRotation));
835+
pointsToNote =
836+
PathPlannerPath.bezierFromPoses(
837+
new Pose2d(getPose().getX(), getPose().getY(), targetRotation),
838+
new Pose2d(targetNoteLocation.getX(), targetNoteLocation.getY(), targetRotation));
839+
}
840+
PathPlannerPath path =
841+
new PathPlannerPath(
842+
pointsToNote,
843+
new PathConstraints(
844+
3, 2.45, Units.degreesToRadians(100), Units.degreesToRadians(180)),
845+
new GoalEndState(0.5, targetRotation, true));
846+
847+
path.preventFlipping = true;
848+
// AutoBuilder.followPath(path).schedule();
849+
Logger.recordOutput("follow path", true);
850+
return path;
851+
} else {
852+
// return;
853+
// led.setState(LED_STATE.PAPAYA_ORANGE);
854+
return new PathPlannerPath(
855+
PathPlannerPath.bezierFromPoses(
856+
new Pose2d(getPose().getX(), getPose().getY(), getPose().getRotation()),
857+
new Pose2d(getPose().getX(), getPose().getY(), getPose().getRotation())),
858+
new PathConstraints(0.1, 1.5, Units.degreesToRadians(100), Units.degreesToRadians(180)),
859+
new GoalEndState(0.5, getPose().getRotation(), true));
860+
}
861+
}
862+
795863
public Command createPathFindingCommand(Pose2d target) {
796864
Pose2d coord = target;
797865
PathConstraints constraints =

0 commit comments

Comments
 (0)