From 5ff011ef731fb980c5b5f092897278ee46339ecd Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 2 Apr 2022 15:28:55 -0600 Subject: [PATCH] SWERVE FIXES n SUCH --- src/main/deploy/ShooterData.csv | 8 +-- src/main/java/frc4388/robot/Constants.java | 4 +- src/main/java/frc4388/robot/Robot.java | 8 +-- src/main/java/frc4388/robot/RobotMap.java | 1 + .../commands/ShooterCommands/TrackTarget.java | 20 ++++---- .../frc4388/robot/subsystems/SwerveDrive.java | 18 +++---- .../java/frc4388/robot/subsystems/Turret.java | 50 +++++++++---------- .../robot/subsystems/VisionOdometry.java | 4 +- 8 files changed, 59 insertions(+), 54 deletions(-) diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index 074bf5c..0b2b5aa 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -1,14 +1,16 @@ Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) 0 ,-29.5 ,8000 78.5 ,-29.5 ,8000 -99 ,-39.62 ,8500 +88 ,-34.2 ,8600 +99 ,-39.62 ,8600 111 ,-42 ,9000 127.25 ,-49.12 ,9500 141 ,-59.4 ,9900 150 ,-66.22 ,10000 164.5 ,-75.52 ,10000 -186 ,-76.24 ,10000 +189.9 ,-81.39 ,11000 207 ,-104.07 ,11000 227 ,-105.32 ,11500 +239 ,-105.5 ,12380 255.5 ,-105.8 ,13500 -999 ,-105.8 ,13500 \ No newline at end of file +999 ,-105.8 ,13500 diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f9bbba7..e517fb3 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -156,7 +156,7 @@ public final class Constants { } public static final class ExtenderConstants { - public static final double EXTENDER_FORWARD_LIMIT = 235.0;//250.0; + public static final double EXTENDER_FORWARD_LIMIT = 200.3;//250.0; public static final double EXTENDER_REVERSE_LIMIT = 0.0; } @@ -164,7 +164,7 @@ public final class Constants { public static final int STORAGE_CAN_ID = 18; public static final int BEAM_SENSOR_SHOOTER = 28; //TODO public static final int BEAM_SENSOR_INTAKE = 29; //TODO - public static final double STORAGE_SPEED = 0.9; + public static final double STORAGE_SPEED = 1.0;//0.9; } public static final class LEDConstants { public static final int LED_SPARK_ID = 0; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index e5ad04b..6eab374 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -75,7 +75,7 @@ public class Robot extends TimedRobot { SmartDashboard.putData(CommandScheduler.getInstance()); // desmosServer.start(); - m_robotContainer.m_robotVisionOdometry.setLEDs(true); + m_robotContainer.m_robotVisionOdometry.setLEDs(false); // DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0}); } @@ -146,11 +146,13 @@ public class Robot extends TimedRobot { LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); } - m_robotContainer.m_robotVisionOdometry.setLEDs(true); + m_robotContainer.m_robotVisionOdometry.setLEDs(false); } @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + // m_robotContainer.m_robotVisionOdometry.setLEDs(false); + } /** * This autonomous runs the autonomous command selected by your diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 19a8264..32c387a 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -300,6 +300,7 @@ void configureExtenderMotors() { void configureStorageMotors() { storageMotor.restoreFactoryDefaults(); + storageMotor.setIdleMode(IdleMode.kCoast); } } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 02e4823..94f9bc8 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -94,7 +94,7 @@ public class TrackTarget extends CommandBase { //// points = filterPoints(points); Point average = VisionOdometry.averagePoint(points); - double output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; + double output = ((average.x + 40) - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; output *= 2.1; m_turret.runTurretWithInput(output); @@ -132,8 +132,6 @@ public class TrackTarget extends CommandBase { } catch (Exception e){ e.printStackTrace(); } - - // run storage } @@ -197,14 +195,14 @@ public class TrackTarget extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (this.isAuto) { - if (targetLocked && !timerStarted) { - timerStarted = true; - startTime = System.currentTimeMillis(); - } - return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); - } else { + // if (this.isAuto) { + // if (targetLocked && !timerStarted) { + // timerStarted = true; + // startTime = System.currentTimeMillis(); + // } + // return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); + // } else { return false; - } + // } } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 43eb4f8..855ce04 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -106,9 +106,9 @@ public class SwerveDrive extends SubsystemBase { double ySpeedMetersPerSecond = speed.getY(); chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2))) + -rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2))) : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond, - -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust); + -rot * SwerveDriveConstants.ROTATION_SPEED * 2); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds); setModuleStates(states); } @@ -128,8 +128,8 @@ public class SwerveDrive extends SubsystemBase { double ySpeedMetersPerSecond = speed.getY(); chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2))) - : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * speedAdjust); + rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2))) + : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * 2); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates( chassisSpeeds); // if (ignoreAngles) { @@ -173,12 +173,12 @@ public class SwerveDrive extends SubsystemBase { public void periodic() { updateOdometry(); - updateSmartDash(); + // updateSmartDash(); - SmartDashboard.putNumber("Pigeon getRotation2d", m_gyro.getRotation2d().getDegrees()); - SmartDashboard.putNumber("Pigeon getAngle", m_gyro.getAngle()); - SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw()); - SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360); + // SmartDashboard.putNumber("Pigeon getRotation2d", m_gyro.getRotation2d().getDegrees()); + // SmartDashboard.putNumber("Pigeon getAngle", m_gyro.getAngle()); + // SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw()); + // SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360); m_field.setRobotPose(m_odometry.getPoseMeters()); super.periodic(); diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 92c5d62..c6a09fe 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -114,41 +114,41 @@ public class Turret extends SubsystemBase { // SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); // SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); - SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition()); - SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); - SmartDashboard.putBoolean("Turret Left Limit", m_boomBoomLeftLimit.isPressed()); - SmartDashboard.putBoolean("Turret Right Limit", m_boomBoomRightLimit.isPressed()); + // SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition()); + // SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); + // SmartDashboard.putBoolean("Turret Left Limit", m_boomBoomLeftLimit.isPressed()); + // SmartDashboard.putBoolean("Turret Right Limit", m_boomBoomRightLimit.isPressed()); // limit switch annoying time thing but actually worked first try wtf - leftState = m_boomBoomLeftLimit.isPressed(); // * Get the state of the left limit switch (true for pressed). + // leftState = m_boomBoomLeftLimit.isPressed(); // * Get the state of the left limit switch (true for pressed). - hasLeftSwitchChanged = (leftState != leftPrevState); // * Get whether the state of the left limit switch has changed, based on its previous state. + // hasLeftSwitchChanged = (leftState != leftPrevState); // * Get whether the state of the left limit switch has changed, based on its previous state. - if (leftState && hasLeftSwitchChanged) { // * If the left limit switch is pressed, and it recently changed, start the time. - leftCurrentTime = System.currentTimeMillis(); - leftElapsedTime = 0; - } + // if (leftState && hasLeftSwitchChanged) { // * If the left limit switch is pressed, and it recently changed, start the time. + // leftCurrentTime = System.currentTimeMillis(); + // leftElapsedTime = 0; + // } - if (!m_boomBoomRightLimit.isPressed()) recentlyPressed = false; + // if (!m_boomBoomRightLimit.isPressed()) recentlyPressed = false; - if(m_boomBoomRightLimit.isPressed() && !recentlyPressed){ - recentlyPressed = true; - m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/); - } - SmartDashboard.putBoolean("Recently Pressed", recentlyPressed); + // if(m_boomBoomRightLimit.isPressed() && !recentlyPressed){ + // recentlyPressed = true; + // m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/); + // } + // // SmartDashboard.putBoolean("Recently Pressed", recentlyPressed); - if (leftState && !hasLeftSwitchChanged) { // * If the left limit switch is still pressed, but the state hasn't changed, then calculate elapsed time. - leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; - } + // if (leftState && !hasLeftSwitchChanged) { // * If the left limit switch is still pressed, but the state hasn't changed, then calculate elapsed time. + // leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; + // } - if (leftState && (leftElapsedTime > 500)) { // * If the left limit switch is pressed for more than half a second, update the encoder position. - m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT); - } + // if (leftState && (leftElapsedTime > 500)) { // * If the left limit switch is pressed for more than half a second, update the encoder position. + // m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT); + // } - leftPrevState = leftState; // * Update the state of the left limit switch. + // leftPrevState = leftState; // * Update the state of the left limit switch. - // * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output). - runVelocityRamping(); + // // * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output). + // runVelocityRamping(); } public void runVelocityRamping() { diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java index 85f4b82..49a2a61 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -44,7 +44,9 @@ public class VisionOdometry extends SubsystemBase { * @param shooter The turret subsystem */ public VisionOdometry(SwerveDrive drive, Turret shooter) { - m_camera = new PhotonCamera(VisionConstants.NAME); + // do{ + m_camera = new PhotonCamera(VisionConstants.NAME); + // }while(m_camera.getLatestResult().getLatencyMillis() == 0.d); m_drive = drive; m_shooter = shooter;