From e34a89a46f4497d0cb043b0ea52836e6ab3c2ff3 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 01:42:34 -0600 Subject: [PATCH] shoot is janky --- src/main/java/frc4388/robot/Constants.java | 4 ++-- .../java/frc4388/robot/RobotContainer.java | 10 ++++---- .../robot/commands/ShooterCommands/Seek.java | 2 +- .../robot/commands/ShooterCommands/Shoot.java | 24 ++++++++----------- .../frc4388/robot/subsystems/SwerveDrive.java | 23 ++++++++++-------- 5 files changed, 31 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 64e38b1..3f0ae83 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -37,7 +37,7 @@ public final class Constants { public static final double LOOP_TIME = 0.02; public static final class SwerveDriveConstants { - public static final double ROTATION_SPEED = 3.0; + public static final double ROTATION_SPEED = 2.0; public static final double WIDTH = 23.75; public static final double HEIGHT = 23.75; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11; @@ -278,7 +278,7 @@ public final class Constants { public static final double TURRET_CLIMBING_POS = -3.76; // Shoot Command Constants - public static final Gains SHOOT_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOOT_GAINS = new Gains(3.0, 0.0, 0.0, 0.0, 0, 1.0); /* Turret Constants */ // ID diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a0a97f7..5743e3c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -265,7 +265,7 @@ public class RobotContainer { .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); new JoystickButton(getDriverController(), XboxController.Button.kA.value) - .whileHeld(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 1.0, 0.0, true)) + .whileHeld(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.2, true)) .whenReleased(() -> m_robotSwerveDrive.stopModules()); new JoystickButton(getDriverController(), XboxController.Button.kB.value) @@ -273,7 +273,7 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)); new JoystickButton(getDriverController(), XboxController.Button.kY.value) - .whileHeld(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood)) + .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, false)) .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) .whenReleased(() -> m_robotSwerveDrive.stopModules()); @@ -306,9 +306,9 @@ public class RobotContainer { new JoystickButton(getOperatorController(), XboxController.Button.kX.value) .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); - new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret)) - .whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret)); + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + // .whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret)) + // .whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret)); // Right Bumper > Storage In // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java index a706512..efcbaa3 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java @@ -24,6 +24,6 @@ public class Seek extends SequentialCommandGroup { public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); - addCommands(new Shoot(swerve, drum, turret, hood), new TrackTarget(turret, drum, hood, visionOdometry)); + addCommands(new Shoot(swerve, drum, turret, hood, false), new TrackTarget(turret, drum, hood, visionOdometry)); } } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index f14f449..64a79dc 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -82,16 +82,19 @@ public class Shoot extends CommandBase { isAimedInTolerance = false; } - public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) { - this(swerve, drum, turret, hood, false); - } + // public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) { + // this(swerve, drum, turret, hood, false); + // } /** * Updates error for custom PID. */ public void updateError() { targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); - error = (targetAngle - turret.getBoomBoomAngleDegrees() + 360) % 360; + error = (targetAngle - turret.getBoomBoomAngleDegrees()) % 360; + if (error > 180) { + error = 360 - error; + this.inverted = -1; } else { this.inverted = 1; } isAimedInTolerance = (Math.abs(error) <= tolerance); } @@ -99,7 +102,7 @@ public class Shoot extends CommandBase { @Override public void initialize() { - this.turret.gotoMidpoint(); + // this.turret.gotoMidpoint(); this.odoX = 0;//-m_swerve.getOdometry().getY(); this.odoY = -8;//-m_swerve.getOdometry().getX(); @@ -124,13 +127,6 @@ public class Shoot extends CommandBase { * Run custom PID. */ public void runPID() { - if (error > 180) { - error = 360 - error; - this.inverted = -1; - } - else{ - this.inverted = 1; - } prevError = error; updateError(); @@ -151,8 +147,8 @@ public class Shoot extends CommandBase { SmartDashboard.putNumber("Shoot.java TargetAngle", this.targetAngle); SmartDashboard.putNumber("Normalized Output", this.normOutput); - this.swerve.driveWithInput(0, 0, normOutput, true); this.turret.m_boomBoomRotateMotor.set(normOutput); + this.swerve.driveWithInput(0, 0, normOutput, true); if (this.toShoot) { this.hood.runAngleAdjustPID(this.targetHood); @@ -167,7 +163,7 @@ public class Shoot extends CommandBase { // ? return to initial swerve rotation // swerve.driveWithInput(0, 0, initialSwerveRotation, true); - this.swerve.driveWithInput(0.0, 0.0, 0.0, 0.0, true); + // this.swerve.driveWithInput(0.0, 0.0, 0.0, 0.0, true); this.turret.m_boomBoomRotateMotor.set(0.0); if (this.toShoot) { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 2c5600b..35cd854 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -105,13 +105,13 @@ public class SwerveDrive extends SubsystemBase { double ySpeedMetersPerSecond = speed.getY(); chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180))) + -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2))) : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond, -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds); setModuleStates(states); } - + // new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180))) public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) { ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0; Translation2d speed = new Translation2d(leftX, leftY); @@ -119,6 +119,9 @@ public class SwerveDrive extends SubsystemBase { if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND) rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1)); double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians(); + if (ignoreAngles) { + rot = 0; + } double xSpeedMetersPerSecond = speed.getX(); double ySpeedMetersPerSecond = speed.getY(); chassisSpeeds = fieldRelative @@ -127,14 +130,14 @@ public class SwerveDrive extends SubsystemBase { : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * speedAdjust); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates( chassisSpeeds); - if (ignoreAngles) { - SwerveModuleState[] lockedStates = new SwerveModuleState[states.length]; - for (int i = 0; i < states.length; i ++) { - SwerveModuleState state = states[i]; - lockedStates[i]= new SwerveModuleState(0, state.angle); - } - setModuleStates(lockedStates); - } + // if (ignoreAngles) { + // SwerveModuleState[] lockedStates = new SwerveModuleState[states.length]; + // for (int i = 0; i < states.length; i ++) { + // SwerveModuleState state = states[i]; + // lockedStates[i]= new SwerveModuleState(0, state.angle); + // } + // setModuleStates(lockedStates); + // } setModuleStates(states); // SmartDashboard.putNumber("rot", rot); // SmartDashboard.putNumber("rotarget", rotTarget.getDegrees());