diff --git a/logs/Log_24-04-01_14-47-21.wpilog b/logs/Log_24-04-01_14-47-21.wpilog new file mode 100644 index 0000000..88ba093 Binary files /dev/null and b/logs/Log_24-04-01_14-47-21.wpilog differ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index af686be..8381082 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,12 +13,8 @@ package frc.robot; -import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; -import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; - import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; @@ -38,6 +34,8 @@ import frc.robot.subsystems.flywheel.FlywheelIO; import frc.robot.subsystems.flywheel.FlywheelIOSim; import frc.robot.subsystems.flywheel.FlywheelIOSparkMax; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -135,13 +133,12 @@ public RobotContainer() { autoChooser.addOption( "Flywheel SysId (Dynamic Reverse)", flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse)); autoChooser.addOption( - "Drive SysID (Quad Test)", Commands.sequence( + "Drive SysID (Quad Test)", + Commands.sequence( drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward), drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse), drive.sysIdDynamic(SysIdRoutine.Direction.kForward), - drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)) - ); - + drive.sysIdDynamic(SysIdRoutine.Direction.kReverse))); // Configure the button bindings configureButtonBindings(); } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 55a1024..1ca182c 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -111,7 +111,7 @@ MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()), new SysIdRoutine( new SysIdRoutine.Config( null, - null, + Volt.of(2.5), Seconds.of(5), (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), new SysIdRoutine.Mechanism( @@ -296,6 +296,8 @@ public void setPose(Pose2d pose) { poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); } + /** Resets Gyro Angle for Driving */ + /** * Adds a vision measurement to the pose estimator. * diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index b20478c..b41a651 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -47,9 +47,9 @@ public Module(ModuleIO io, int index) { switch (Constants.currentMode) { case REAL: case REPLAY: - driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); - driveFeedback = new PIDController(0.05, 0.0, 0.0); - turnFeedback = new PIDController(7.0, 0.0, 0.0); + driveFeedforward = new SimpleMotorFeedforward(0.0011966, 0.15661, 0.018981); + driveFeedback = new PIDController(0.024181, 0.0, 0.0); + turnFeedback = new PIDController(7.5, 0.0, 0.013); break; case SIM: driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13);