Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -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/Constants.java
+1 -1
View File
@@ -68,7 +68,7 @@ public final class Constants {
public static final PIDController X_CONTROLLER = new PIDController(0.5, 0.0, 0.0);
public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0);
public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(
15.0, 0.1, 0.3, new TrapezoidProfile.Constraints(Math.PI, Math.PI));
15.0, 0.1, 0.3, new TrapezoidProfile.Constraints(Math.PI, Math.PI));
// swerve configuration
public static final double NEUTRAL_DEADBAND = 0.04;
src/main/java/frc4388/robot/RobotContainer.java
+3 -3
View File
@@ -95,7 +95,7 @@ public class RobotContainer {
/* Driver Buttons */
new JoystickButton(getDriverController(), XboxController.Button.kY.value)
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.Y_BUTTON)
.whenPressed(m_robotSwerveDrive.m_gyro::reset);
.whenPressed(() -> m_robotSwerveDrive.resetGyro());
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.LEFT_BUMPER_BUTTON)
@@ -142,7 +142,7 @@ public class RobotContainer {
return new SequentialCommandGroup(
new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()),
new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(state.poseMeters.getTranslation(), state.holonomicRotation))),
new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(state.poseMeters.getTranslation(), state.holonomicRotation))),//.plus(new Rotation2d(Math.PI/2))))),
//new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(traj.getInitialPose())),
ppSCC,
new InstantCommand(() -> m_robotSwerveDrive.stopModules())
@@ -186,7 +186,7 @@ public class RobotContainer {
// new InstantCommand(() -> m_robotSwerveDrive.stopModules())
// );
// return runAuto("Move Forward", 1.0, 1.0);
return runAuto("Move Down", 1.0, 1.0);
return runAuto("Test", 1.0, 1.0);
}
/**
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();