Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 00:38:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

Rotate and Autos

Browse Source
This commit is contained in:
Shikhar
2026-01-19 13:42:16 -07:00
parent c68329c9ca
commit f600317435
14 changed files with 258 additions and 183 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
+2 -1
View File
@@ -289,7 +289,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
Translation2d fieldPosLead = robotSpeed.times(SwerveDriveConstants.AIM_LEAD_TIME.get()).plus(fieldPos);
// Calculate the angle between the current position and the lead position
Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle();
Rotation2d ang = fieldPosLead.minus(getPose2d().getTranslation()).getAngle();
driveFieldAngle(leftStick, ang);
}
src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java
+1 -1
View File
@@ -164,7 +164,7 @@ public final class SwerveDriveConstants {
// TODO: Replace this with a static constant
public static final ConfigurableDouble AIM_kP = new ConfigurableDouble("Aim kP", 2.5);
public static final ConfigurableDouble AIM_kP = new ConfigurableDouble("Aim kP", 15);
public static final ConfigurableDouble AIM_kI = new ConfigurableDouble("Aim kI", 0);
public static final ConfigurableDouble AIM_kD = new ConfigurableDouble("Aim kD", 0.1);
// public static final Gains AIM_GAINS = new Gains(2.5, 0, 0.1);
src/main/java/frc4388/robot/subsystems/vision/Vision.java
+1
View File
@@ -43,6 +43,7 @@ public class Vision extends SubsystemBase implements Queryable {
io[i].updateInputs(state[i]);
Logger.processInputs("Vision/Camera" + i , state[i]);
}
Logger.recordOutput("Vision/isTagDectected", isTag());
}
public List<PoseObservation> getPosesToAdd(){
src/main/java/frc4388/robot/subsystems/vision/VisionReal.java
+2 -2
View File
@@ -45,12 +45,12 @@ public class VisionReal implements VisionIO {
var result = results.get(results.size()-1);
state.isTagDetected = state.isTagDetected | result.hasTargets();
// If there are no tags
if(!result.hasTargets())
return;
state.isTagDetected = state.isTagDetected | result.hasTargets();
Optional<EstimatedRobotPose> estimatedRobotPose = getEstimatedGlobalPose(result, estimator);
// If the tag was failed to be processed