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

Merge branch 'cleanup' of https://github.com/Team4388/2022NoWayHome into cleanup

Browse Source
This commit is contained in:
Ryan Manley
2022-04-02 15:30:36 -06:00
parent 5ff011ef73 a6bfaa1b30
commit 16ccd86843
3 changed files with 40 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+9 -6
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;
@@ -531,12 +532,12 @@ public class RobotContainer {
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
// ! DRIVE BACKWARDS AND SHOOT (HOPEFULLY)
return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {1.0, 0.0, 0.0, 0.0}, (5.0 * 12) / distancePerSecond),//0.269), // * go backwards three feet
new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake
// return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {1.0, 0.0, 0.0, 0.0}, (5.0 * 12) / distancePerSecond),//0.269), // * go backwards three feet
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake
weirdAutoShootingGroup, // * shoot
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.5)); // * stop running storage
// weirdAutoShootingGroup, // * shoot
// new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.5)); // * stop running storage
// ! TWO BALL AUTO (HOPEFULLY)
// return new SequentialCommandGroup( new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), // * extend out, in preparation of running intake
@@ -609,7 +610,9 @@ public class RobotContainer {
// new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0))
// );
// ! PathPlanner Testing
return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Move Forward"));
// return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Diamond"));
}
public void switchControlMode() {
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());
}