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

sad

Browse Source
This commit is contained in:
aarav18
2022-02-08 19:42:54 -07:00
parent 60f5cc6097
commit 1d58130aca
3 changed files with 11 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+7 -3
View File
@@ -54,6 +54,7 @@ public class SwerveDrive extends SubsystemBase {
public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
public boolean ignoreAngles;
public Rotation2d rotTarget = new Rotation2d();;
private final Field2d m_field = new Field2d();
@@ -125,7 +126,6 @@ public class SwerveDrive extends SubsystemBase {
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
setModuleStates(states);
}
private Rotation2d rotTarget = new Rotation2d();
public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative)
{
ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
@@ -217,8 +217,7 @@ public class SwerveDrive extends SubsystemBase {
/** Updates the field relative position of the robot. */
public void updateOdometry() {
Rotation2d offset = new Rotation2d(Math.PI/2);
m_poseEstimator.update( m_gyro.getRotation2d()/*.plus(offset)*/,
m_poseEstimator.update( m_gyro.getRotation2d(),
modules[0].getState(),
modules[1].getState(),
modules[2].getState(),
@@ -237,6 +236,11 @@ public class SwerveDrive extends SubsystemBase {
// Timer.getFPGATimestamp() - 0.1);
}
public void resetGyro(){
m_gyro.reset();
rotTarget = new Rotation2d(0);
}
public void stopModules() {
modules[0].stop();
modules[1].stop();