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

Add code from photonvision

Browse Source
This commit is contained in:
Michael Mikovsky
2025-01-09 12:57:04 -07:00
parent bdef8352d4
commit d81c4b381d
10 changed files with 436 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+7 -8
View File
@@ -33,6 +33,8 @@ import frc4388.utility.Status.ReportLevel;
public class SwerveDrive extends Subsystem {
private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
private Vision vision;
private int gear_index = SwerveDriveConstants.STARTING_GEAR;
private boolean stopped = false;
@@ -45,16 +47,14 @@ public class SwerveDrive extends Subsystem {
public Rotation2d orientRotTarget = new Rotation2d();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
private Field2d field = new Field2d();
/** Creates a new SwerveDrive. */
public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
super();
SmartDashboard.putData(field);
this.swerveDriveTrain = swerveDriveTrain;
this.vision = vision;
}
// public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){
// // double ang = Math.atan2(rightStick.getY(), rightStick.getX());
// // rightStick.getAngle()
@@ -172,10 +172,9 @@ public class SwerveDrive extends Subsystem {
SmartDashboard.putNumber("Gyro", getGyroAngle());
SmartDashboard.putNumber("RotTartget", rotTarget);
Optional<Pose2d> e = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds());
vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(Vision.getTime()));
if(e.isPresent())
field.setRobotPose(e.get());
// if(e.isPresent())
}
private void reset_index() {