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

fix 2025 module possitions

Browse Source
This commit is contained in:
C4llSiqn
2025-01-16 19:41:08 -07:00
parent a8bfc1543f
commit 1bd3c12327
4 changed files with 37 additions and 30 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
@@ -5,6 +5,7 @@
package frc4388.robot.subsystems;
import java.util.Optional;
import java.util.function.DoubleSupplier;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.hardware.CANcoder;
@@ -69,8 +70,13 @@ public class SwerveDrive extends Subsystem {
// Handle exception as needed
config = null;
}
// DoubleSupplier a = () -> 1.d;
AutoBuilder.configure(
() -> swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()), // Robot pose supplier
() -> {
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)
}, // Robot pose supplier
swerveDriveTrain::resetPose, // 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()
@@ -121,7 +127,7 @@ public class SwerveDrive extends Subsystem {
if (fieldRelative) {
swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
.withVelocityX(leftStick.getX()*speedAdjust)
.withVelocityY(leftStick.getY()*speedAdjust)
.withVelocityY(-leftStick.getY()*speedAdjust)
.withRotationalRate(rightStick.getX()*rotSpeedAdjust)
);
// double rot = 0;
@@ -159,8 +165,8 @@ public class SwerveDrive extends Subsystem {
// chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1));
} else { // Create robot-relative speeds.
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
.withVelocityX(leftStick.getX()*-speedAdjust)
.withVelocityY(leftStick.getY()*speedAdjust)
.withVelocityX(leftStick.getX()*speedAdjust)
.withVelocityY(-leftStick.getY()*speedAdjust)
.withRotationalRate(rightStick.getX()*rotSpeedAdjust)
);
}