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

make those autos

Browse Source
This commit is contained in:
Michael Mikovsky
2025-01-23 19:32:12 -07:00
parent 52d7659f3e
commit 4a45243828
26 changed files with 1137 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+10 -4
View File
@@ -32,6 +32,7 @@ import frc4388.utility.Status;
import frc4388.utility.Subsystem;
import frc4388.utility.Status.ReportLevel;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import com.pathplanner.lib.config.PIDConstants;
@@ -52,6 +53,8 @@ public class SwerveDrive extends Subsystem {
public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
// 25%
public Pose2d initalPose2d = null;
public double rotTarget = 0.0;
public Rotation2d orientRotTarget = new Rotation2d();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
@@ -75,11 +78,9 @@ public class SwerveDrive extends Subsystem {
// DoubleSupplier a = () -> 1.d;
AutoBuilder.configure(
() -> {
var pose = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d());
// pose = new Pose2d(pose.getX(), pose.getY(), pose.getRotation().times(-1));
return pose;// getRotation().times(-1)
return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d);
}, // Robot pose supplier
swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting
this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting
// pose)
() -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
(speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds()
@@ -108,6 +109,11 @@ public class SwerveDrive extends Subsystem {
}
public void setOdoPose(Pose2d pose) {
initalPose2d = pose;
swerveDriveTrain.resetPose(pose);
}
// public void oneModuleTest(SwerveModule module, Translation2d leftStick,
// Translation2d rightStick){
// // double ang = Math.atan2(rightStick.getY(), rightStick.getX());