Skip to content

Commit

Permalink
path fault
Browse files Browse the repository at this point in the history
  • Loading branch information
@Vignesh1234587
Vignesh1234587 committed Aug 15, 2024
1 parent b87e886 commit b2176b8
Show file tree
Hide file tree
Showing 6 changed files with 105 additions and 31 deletions.
10 changes: 5 additions & 5 deletions src/main/deploy/pathplanner/paths/$s!c4.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,12 +16,12 @@
},
{
"anchor": {
"x": 7.07383441672713,
"y": 2.3836410004658575
"x": 6.535619743411246,
"y": 2.076352426858445
},
"prevControl": {
"x": 6.405905953466024,
"y": 2.3836410004658575
"x": 5.86769128015014,
"y": 2.076352426858445
},
"nextControl": null,
"isLocked": false,
Expand Down Expand Up @@ -49,7 +49,7 @@
"rotateFast": false
},
"reversed": false,
"folder": "$s!p-c5-c4",
"folder": null,
"previewStartingState": {
"rotation": -43.53119928561413,
"velocity": 0
Expand Down
12 changes: 6 additions & 6 deletions src/main/java/frc/robot/BuildConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,14 +3,14 @@
/** Automatically generated file containing build version information. */
public final class BuildConstants {
public static final String MAVEN_GROUP = "";
public static final String MAVEN_NAME = "2024RobotCode";
public static final String MAVEN_NAME = "2024 Robot Code";
public static final String VERSION = "unspecified";
public static final int GIT_REVISION = 314;
public static final String GIT_SHA = "dbfb048d3f7aa479526221e9947ca94220ee5472";
public static final String GIT_DATE = "2024-07-28 19:57:18 EDT";
public static final int GIT_REVISION = 317;
public static final String GIT_SHA = "b87e886bb4404f32a660af7d73877f5015d1e1f5";
public static final String GIT_DATE = "2024-08-14 00:51:38 EDT";
public static final String GIT_BRANCH = "note-filtering";
public static final String BUILD_DATE = "2024-08-02 11:57:52 EDT";
public static final long BUILD_UNIX_TIME = 1722614272911L;
public static final String BUILD_DATE = "2024-08-14 20:34:11 EDT";
public static final long BUILD_UNIX_TIME = 1723682051564L;
public static final int DIRTY = 1;

private BuildConstants() {}
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ public static Mode getMode() {
};
}

public static final Mode currentMode = Mode.SIM;
public static final Mode currentMode = Mode.REAL;
public static final boolean tuningMode = true;
public static final String CANBUS = "CAN Bus 2";
public static final double LOOP_PERIOD_SECS = 0.02;
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/commands/AimbotTele.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ public AimbotTele(

switch (Constants.currentMode) {
case REAL:
gains[0] = 3.14;
gains[0] = 3.7;
gains[1] = 0;
gains[2] = 0;
break;
Expand All @@ -78,7 +78,7 @@ public AimbotTele(
}

pid = new PIDController(gains[0], gains[1], gains[2], 0.02);
pid.setTolerance(3);
pid.setTolerance(2);
pid.enableContinuousInput(-180, 180);
}

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

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

Expand Down
28 changes: 17 additions & 11 deletions src/main/java/frc/robot/commands/AlignToNoteAuto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@
package frc.robot.commands;

import com.pathplanner.lib.auto.AutoBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
Expand Down Expand Up @@ -78,21 +77,28 @@ public void initialize() {
pivot.setPivotGoal(Constants.PivotConstants.INTAKE_SETPOINT_DEG);
targetNoteLocation = noteLocations.get(drive.getTargetNote());
useGeneratedPathCommand =
drive.getCachedNoteLocation().getDistance(targetNoteLocation) < 1.323
drive.getCachedNoteLocation().getDistance(targetNoteLocation) < 2.5
&& drive.getCachedNoteLocation() != null;
Logger.recordOutput(
"cached note distance ", drive.getCachedNoteLocation().getDistance(targetNoteLocation));
Logger.recordOutput("useGeneratedPath command", useGeneratedPathCommand);
// useGeneratedPathCommand = false;
generatedPathCommand = AutoBuilder.followPath(drive.generatePathToNote());
// generatedPathCommand = AutoBuilder.followPath(drive.generatePathToNote());
if (useGeneratedPathCommand) {
generatedPathCommand = AutoBuilder.followPath(drive.generatePathToNote());

generatedPathCommand.initialize();
} else {
targetNoteRotation =
new Rotation2d(
targetNoteLocation.getX() - drive.getPose().getX(),
targetNoteLocation.getY() - drive.getPose().getY());
// targetNoteRotation =
// new Rotation2d(
// targetNoteLocation.getX() - drive.getPose().getX(),
// targetNoteLocation.getY() - drive.getPose().getY());
// targetNotePathCommand =
// drive.generateTrajectory(
// new Pose2d(targetNoteLocation, targetNoteRotation), 3, 2.45, 100, 180, 0.5);
targetNotePathCommand =
drive.generateTrajectory(
new Pose2d(targetNoteLocation, targetNoteRotation), 3, 2.45, 100, 180, 0.5);
AutoBuilder.followPath(drive.generatePathToNoteBlind(targetNoteLocation));

targetNotePathCommand.initialize();
}
}
Expand Down Expand Up @@ -126,7 +132,7 @@ public boolean isFinished() {
Logger.recordOutput("isFinished align note", shooter.seesNote());
// return false;
return shooter.seesNote() == NoteState.SENSOR
|| shooter.seesNote() == NoteState.CURRENT
|| finished;
|| shooter.seesNote() == NoteState.CURRENT
|| finished;
}
}
78 changes: 73 additions & 5 deletions src/main/java/frc/robot/subsystems/drive/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -244,14 +244,19 @@ public void periodic() {
0,
0,
0);
Logger.recordOutput(
"vision something", DriverStation.getAlliance().isPresent() && visionInputs.aTV);
Logger.recordOutput("isDisabled", DriverStation.isDisabled());

if (DriverStation.getAlliance().isPresent() && visionInputs.aTV) {
Logger.recordOutput(
"tags > 1 or disabled ", visionInputs.tagCount > 1 || DriverStation.isDisabled());

if (visionInputs.tagCount > 1 || DriverStation.isDisabled()) {
visionLogic();
} else {
mt2TagFiltering();
}


}

Logger.recordOutput("note time", getCachedNoteTime());
Expand Down Expand Up @@ -348,7 +353,7 @@ public void visionLogic() {
double yMeterStds;
double headingDegStds;

double poseDifference = getVisionPoseDifference(limelightMeasurement.pose);
// double poseDifference = getVisionPoseDifference(limelightMeasurement.pose);

boolean isFlipped =
DriverStation.getAlliance().isPresent()
Expand All @@ -360,11 +365,11 @@ public void visionLogic() {
xMeterStds = 0.7;
yMeterStds = 0.7;
headingDegStds = 8;
} else if (limelightMeasurement.tagCount == 1 && poseDifference < 0.5) {
} else if (limelightMeasurement.tagCount == 1) { // && poseDifference < 0.5
xMeterStds = 5;
yMeterStds = 5;
headingDegStds = 30;
} else if (limelightMeasurement.tagCount == 1 && poseDifference < 3) {
} else if (limelightMeasurement.tagCount == 1) { // && poseDifference < 3
xMeterStds = 11.43;
yMeterStds = 11.43;
headingDegStds = 9999;
Expand Down Expand Up @@ -792,6 +797,69 @@ public PathPlannerPath generatePathToNote() {
}
}

public PathPlannerPath generatePathToNoteBlind(Translation2d targetNoteLocation) {
// if (visionInputs.iTX != 0.0) {
// double taThreshold = 0;
// if (visionInputs.iTA >= taThreshold) {
// lastNoteLocT2d.translation =
// calculateNotePositionFieldRelative().getTranslation().toTranslation2d();
// lastNoteLocT2d.time = Timer.getFPGATimestamp();
// }
// }
// Pose2d targetNoteLocation = noteLocations.get(drive.getTargetNote());

Rotation2d targetRotation;
Logger.recordOutput("note timeess", getCachedNoteTime());
// led.setState(LED_STATE.FLASHING_RED);
// Translation2d targetNoteLocation = getCachedNoteLocation();
Logger.recordOutput("better translate", targetNoteLocation);
if (noteImageIsNew()) {

targetRotation =
new Rotation2d(
targetNoteLocation.getX() - getPose().getX(),
targetNoteLocation.getY() - getPose().getY());
List<Translation2d> pointsToNote;
if (DriverStation.getAlliance().get().equals(Alliance.Blue)) {
Logger.recordOutput(
"goal point blue",
new Pose2d(targetNoteLocation.getX(), targetNoteLocation.getY(), targetRotation));
pointsToNote =
PathPlannerPath.bezierFromPoses(
new Pose2d(getPose().getX(), getPose().getY(), targetRotation),
new Pose2d(targetNoteLocation.getX(), targetNoteLocation.getY(), targetRotation));
} else {
Logger.recordOutput(
"goal point red",
new Pose2d(targetNoteLocation.getX(), targetNoteLocation.getY(), targetRotation));
pointsToNote =
PathPlannerPath.bezierFromPoses(
new Pose2d(getPose().getX(), getPose().getY(), targetRotation),
new Pose2d(targetNoteLocation.getX(), targetNoteLocation.getY(), targetRotation));
}
PathPlannerPath path =
new PathPlannerPath(
pointsToNote,
new PathConstraints(
3, 2.45, Units.degreesToRadians(100), Units.degreesToRadians(180)),
new GoalEndState(0.5, targetRotation, true));

path.preventFlipping = true;
// AutoBuilder.followPath(path).schedule();
Logger.recordOutput("follow path", true);
return path;
} else {
// return;
// led.setState(LED_STATE.PAPAYA_ORANGE);
return new PathPlannerPath(
PathPlannerPath.bezierFromPoses(
new Pose2d(getPose().getX(), getPose().getY(), getPose().getRotation()),
new Pose2d(getPose().getX(), getPose().getY(), getPose().getRotation())),
new PathConstraints(0.1, 1.5, Units.degreesToRadians(100), Units.degreesToRadians(180)),
new GoalEndState(0.5, getPose().getRotation(), true));
}
}

public Command createPathFindingCommand(Pose2d target) {
Pose2d coord = target;
PathConstraints constraints =
Expand Down

0 comments on commit b2176b8

Please sign in to comment.