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

Ramp

Browse Source
This commit is contained in:
mimigamin
2026-03-21 15:49:44 -06:00
parent 75cab187e2
commit d639b3076d
7 changed files with 234 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
+14
View File
@@ -24,6 +24,7 @@ import com.pathplanner.lib.util.PathPlannerLogging;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.units.measure.AngularVelocity;
@@ -433,6 +434,19 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
driveFieldAngle(leftStick, ang);
}
public void offsetOdoPosition(Transform2d offset) {
// Manually performing an addittion on the pose
// WHY doesn't WPILIB have the ability to not transform poses
Pose2d new_pose = new Pose2d(
new Translation2d(
state.currentPose.getX() + offset.getX(),
state.currentPose.getY() + offset.getY()
),
state.currentPose.getRotation()
);
this.io.resetPose(new_pose);
}
public void defenseXPosition(){
io.setControl(new SwerveRequest.SwerveDriveBrake());
}