Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-08 16:28:04 -06:00
Code Issues Packages Projects Releases Wiki Activity

Make auto goto position work, work on vision locaalization

Browse Source
This commit is contained in:
Michael Mikovsky
2025-01-10 21:36:30 -07:00
parent 76b53c95a7
commit 33629e9fdb
5 changed files with 172 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+5 -2
View File
@@ -31,6 +31,7 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
@@ -275,10 +276,12 @@ public final class Constants {
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
}
public static final class VisionConstants {
public static final class VisionConstants {
public static final String CAMERA_NAME = "Camera_Module_v1";
public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(), new Rotation3d());
public static final Rotation2d ROTATE_BY = Rotation2d.fromDegrees(180);
public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(.3048, .2413, .2794), new Rotation3d(0,0.52333,0));
public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
src/main/java/frc4388/robot/RobotContainer.java
+3 -1
View File
@@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
// Autos
import frc4388.utility.controller.VirtualController;
import frc4388.robot.commands.GotoPositionCommand;
import frc4388.robot.commands.Swerve.neoJoystickPlayback;
import frc4388.robot.commands.Swerve.neoJoystickRecorder;
@@ -213,7 +214,8 @@ public class RobotContainer {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return autoPlayback;
//return autoPlayback;
return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
}
/**
src/main/java/frc4388/robot/commands/GotoPositionCommand.java
+120 -16
View File
@@ -1,43 +1,147 @@
package frc4388.robot.commands;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Vision;
import frc4388.utility.Gains;
import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
import frc4388.utility.controller.VirtualController;
public class GotoPositionCommand extends Command {
private boolean isFinished = false;
private Translation2d translation2d= new Translation2d(0,0);
static Gains gains = new Gains(3,0,0);
static double tolerance = 0;
private PID xPID = new PID(gains, 0);
private PID yPID = new PID(gains, 0);
SwerveDrive swerveDrive;
Vision vision;
/**
* Command to drive robot to position.
* @param SwerveDrive m_robotSwerveDrive
*/
public GotoPositionCommand(SwerveDrive swerveDrive) {
public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision) {
this.swerveDrive = swerveDrive;
this.vision = vision;
addRequirements(swerveDrive);
}
@Override
public void initialize() {
isFinished = false;
}
@Override
public void execute() {
}
@Override
public void end(boolean interrupted) {
xPID.initialize();
yPID.initialize();
}
@Override
public boolean isFinished() {
return isFinished;
public void execute() {
double xerr = translation2d.getX() - vision.getPose2d().getX();
double yerr = translation2d.getY() - vision.getPose2d().getY();
SmartDashboard.putNumber("PID X Error", xerr);
SmartDashboard.putNumber("PID Y Error", yerr);
double xoutput = xPID.update(xerr);
double youtput = yPID.update(yerr);
Translation2d leftStick = new Translation2d(
Math.max(Math.min(youtput, 1), -1),
Math.max(Math.min(xoutput, 1), -1)
);
Translation2d rightStick = new Translation2d();
SmartDashboard.putNumber("PID X Output", xoutput);
SmartDashboard.putNumber("PID Y Output", youtput);
swerveDrive.driveWithInput(leftStick, rightStick, true);
}
// @Override
// public void end(boolean interrupted) {
// }
// @Override
// public double getError() {
// return e;
// }
// @Override
// public void runWithOutput(double output) {
// // TODO Auto-generated method stub
// Translation2d leftStick = new Translation2d(Math.max(Math.min(output, 1), -1),0);
// Translation2d rightStick = new Translation2d();
// // System.out.println("Output = " + -output);
// SmartDashboard.putNumber("PID Output", output);
// swerveDrive.driveWithInput(leftStick, rightStick, true);
// }
private class PID {
protected Gains gains;
private double output = 0;
private double tolerance = 0;
/** Creates a new PelvicInflammatoryDisease. */
public PID(double kp, double ki, double kd, double kf, double tolerance) {
gains = new Gains(kp, ki, kd, kf, 0);
this.tolerance = tolerance;
}
public PID(Gains gains, double tolerance) {
this.gains = gains;
this.tolerance = tolerance;
}
// Called when the command is initially scheduled.
public final void initialize() {
output = 0;
}
private double prevError, cumError = 0;
// Called every time the scheduler runs while the command is scheduled.
public double update(double error) {
cumError += error * .02; // 20 ms
double delta = error - prevError;
output = error * gains.kP;
output += cumError * gains.kI;
output += delta * gains.kD;
output += gains.kF;
return output;
}
// // Returns true when the command should end.
// public final boolean isFinished() {
// return Math.abs(getError()) < tolerance;
// }
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+1 -1
View File
@@ -26,7 +26,7 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.Constants.VisionConstants;
import frc4388.utility.Status;
import frc4388.utility.Subsystem;
import frc4388.utility.Status.ReportLevel;
src/main/java/frc4388/robot/subsystems/Vision.java
+43 -19
View File
@@ -3,6 +3,8 @@ package frc4388.robot.subsystems;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
@@ -16,6 +18,7 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionSystemSim;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import com.ctre.phoenix6.Utils;
@@ -43,6 +46,21 @@ public class Vision extends Subsystem {
private final PhotonPoseEstimator photonEstimator;
private Field2d field = new Field2d();
ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
.withSize(2, 2);
GenericEntry sbTag = subsystemLayout
.add("Tag Detected", false)
.withWidget(BuiltInWidgets.kBooleanBox)
.getEntry();
GenericEntry sbCamConnected = subsystemLayout
.add("Camera Connnected", false)
.withWidget(BuiltInWidgets.kBooleanBox)
.getEntry();
public Vision(PhotonCamera camera){
this.camera = camera;
@@ -57,7 +75,15 @@ public class Vision extends Subsystem {
var result = camera.getLatestResult();
isTag = result.hasTargets();
field.setRobotPose(lastPhysOdomPose);
// Optional<MultiTargetPNPResult> multitag = result.getMultiTagResult();
// if (multitag.isEmpty()) {
// Transform3d fieldToCamera = result.getMultiTagResult().estimatedPose.best;
// }else if()
// sbTag.setBoolean(isTag);
// field.setRobotPose(getPose2d());
// sbCamConnected.setBoolean(camera);
@@ -65,6 +91,7 @@ public class Vision extends Subsystem {
if(!isTag){
sbTag.setBoolean(isTag);
field.setRobotPose(getPose2d());
return;
}
@@ -72,11 +99,23 @@ public class Vision extends Subsystem {
// In case the pose estimator fails to estimate the pose, fallback to physical odometry.
if(EstimatedRobotPose.isEmpty()){
sbTag.setBoolean(isTag);
field.setRobotPose(lastPhysOdomPose);
isTag = false;
sbTag.setBoolean(isTag);
field.setRobotPose(getPose2d());
return;
}
lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d();
// lastVisionPose = new Pose2d(
// new Translation2d(lastVisionPose.getY(), lastVisionPose.getX()),
// lastVisionPose.getRotation()
// );
field.setRobotPose(getPose2d());
}
@@ -220,21 +259,6 @@ public class Vision extends Subsystem {
return "Vision";
}
ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
.withSize(2, 2);
GenericEntry sbTag = subsystemLayout
.add("Tag Detected", false)
.withWidget(BuiltInWidgets.kBooleanBox)
.getEntry();
GenericEntry sbCamConnected = subsystemLayout
.add("Camera Connnected", false)
.withWidget(BuiltInWidgets.kBooleanBox)
.getEntry();
// GenericEntry sbShiftState = subsystemLayout
// .add("Shift State", 0)
// .withWidget(BuiltInWidgets.kNumberBar)
@@ -245,7 +269,7 @@ public class Vision extends Subsystem {
public void queryStatus() {
sbTag.setBoolean(isTag);
sbCamConnected.setBoolean(camera.isConnected());
field.setRobotPose(getPose2d());
// field.setRobotPose(getPose2d());
}
@Override