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

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); maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION);
ArrayList<Command> commands = new ArrayList<Command>(); ArrayList<Command> commands = new ArrayList<Command>();
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
PIDController xController = SwerveDriveConstants.X_CONTROLLER; PIDController xController = SwerveDriveConstants.X_CONTROLLER;
PIDController yController = SwerveDriveConstants.Y_CONTROLLER; PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
@@ -503,6 +504,15 @@ public class RobotContainer {
*/ */
public Command getAutonomousCommand() { 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 // ! 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); // // * 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); // * 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(Units.degreesToRadians(1)), // TODO: tune
// VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune // VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune
m_odometry = new SwerveDriveOdometry( m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
m_kinematics,
getRegGyro(),//m_gyro.getRotation2d(),
new Pose2d()); // TODO: tune
// m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
m_gyro.reset(); m_gyro.reset();
SmartDashboard.putData("Field", m_field); SmartDashboard.putData("Field", m_field);
@@ -192,9 +187,9 @@ public class SwerveDrive extends SubsystemBase {
// chassis speeds // chassis speeds
// TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate 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: X", chassisSpeeds.vxMetersPerSecond);
SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond); // SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond); // 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 ACTUAL GYRO", actual.getDegrees());
SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees()); SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees());
m_odometry.update( actualDWI,//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()), m_odometry.update( m_gyro.getRotation2d(),//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
modules[0].getState(), modules[0].getState(),
modules[1].getState(), modules[1].getState(),
modules[2].getState(), modules[2].getState(),
modules[3].getState()); modules[3].getState());
} }