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

odometry is back baby

Browse Source
This commit is contained in:
aarav18
2022-04-02 14:45:22 -06:00
parent 842565f62e
commit 2d9d4fc426
2 changed files with 19 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+10
View File
@@ -458,6 +458,7 @@ public class RobotContainer {
maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION);
ArrayList<Command> commands = new ArrayList<Command>();
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
@@ -503,6 +504,15 @@ public class RobotContainer {
*/
public Command getAutonomousCommand() {
// ! PathPlanner Testing
if (!this.currentClimberMode.equals(null)) {
return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Drive Forward"));
}
if (!this.currentClimberMode.equals(null)) {
return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Diamond"));
}
// ! ways to not coast
// // * 1. try zero joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.0, 0.0, false), m_robotSwerveDrive);
// * 2. try opposite joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, false), m_robotSwerveDrive);
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+9 -14
View File
@@ -75,12 +75,7 @@ public class SwerveDrive extends SubsystemBase {
// VecBuilder.fill(Units.degreesToRadians(1)), // TODO: tune
// VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune
m_odometry = new SwerveDriveOdometry(
m_kinematics,
getRegGyro(),//m_gyro.getRotation2d(),
new Pose2d()); // TODO: tune
// m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
m_gyro.reset();
SmartDashboard.putData("Field", m_field);
@@ -192,9 +187,9 @@ public class SwerveDrive extends SubsystemBase {
// chassis speeds
// TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds
SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond);
SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
// SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond);
// SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
// SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
}
/**
@@ -246,11 +241,11 @@ public class SwerveDrive extends SubsystemBase {
SmartDashboard.putNumber("AUTO ACTUAL GYRO", actual.getDegrees());
SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees());
m_odometry.update( actualDWI,//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
modules[0].getState(),
modules[1].getState(),
modules[2].getState(),
modules[3].getState());
m_odometry.update( m_gyro.getRotation2d(),//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
modules[0].getState(),
modules[1].getState(),
modules[2].getState(),
modules[3].getState());
}