From e39c81d5564d6cba01c2bde126ffee7245b9a442 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 14:25:38 -0600 Subject: [PATCH] shoot working no optimization --- src/main/java/frc4388/robot/Constants.java | 3 +- .../java/frc4388/robot/RobotContainer.java | 45 ++--- .../commands/ShooterCommands/AimToCenter.java | 2 +- .../robot/commands/ShooterCommands/Shoot.java | 154 ++++++++++-------- .../frc4388/robot/subsystems/SwerveDrive.java | 8 +- .../java/frc4388/robot/subsystems/Turret.java | 4 + 6 files changed, 120 insertions(+), 96 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3f0ae83..39814d8 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -278,7 +278,8 @@ public final class Constants { public static final double TURRET_CLIMBING_POS = -3.76; // Shoot Command Constants - public static final Gains SHOOT_GAINS = new Gains(3.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains TURRET_SHOOT_GAINS = new Gains(3.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SWERVE_SHOOT_GAINS = new Gains(4.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 2566688..8c8add1 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -179,19 +179,19 @@ public class RobotContainer { // Swerve Drive with Input m_robotSwerveDrive.setDefaultCommand( new RunCommand(() -> { - if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { + // if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), getDriverController().getLeftY(), getDriverController().getRightX(), getDriverController().getRightY(), - true); } - if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { - m_robotSwerveDrive.driveWithInput( 0, - 0, - 0, - 0, - true); - }} + true); //} + // if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { + // m_robotSwerveDrive.driveWithInput( 0, + // 0, + // 0, + // 0, + // true); + }//} , m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); // Intake with Triggers @@ -278,24 +278,29 @@ public class RobotContainer { .whileHeld(new AimToCenter(m_robotTurret, m_robotVisionOdometry, m_robotSwerveDrive::getOdometry)) .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)); - new JoystickButton(getDriverController(), XboxController.Button.kY.value) - .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()); - - new JoystickButton(getDriverController(), XboxController.Button.kX.value) + // new JoystickButton(getDriverController(), XboxController.Button.kY.value) + // .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()); + + new JoystickButton(getDriverController(), XboxController.Button.kX.value) .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotHood.runHood(0.0), m_robotHood)) .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom)); - /* Operator Buttons */ - + /* Operator Buttons */ + new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); + .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()); - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false))); + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); + + // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false))); // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java index de4b234..806446d 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java @@ -85,7 +85,7 @@ public class AimToCenter extends CommandBase { if (-x > 0) { return (180 + exp); } if (-x < 0) { return (360 + exp); } - if (x == 0 && y > 0) { return (270 - actualGyro); } + if (x == 0 && y > 0) { return (270 - actualGyro); } // TODO: try putting these two lines before exp is calculated if (x == 0 && y < 0) { return (90 - actualGyro); } System.err.println("Invalid case."); diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index 7424701..ffc761a 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -37,13 +37,18 @@ public class Shoot extends CommandBase { private double targetAngle, targetVel, targetHood; // pid - private Gains gains = ShooterConstants.SHOOT_GAINS; + private Gains turretGains = ShooterConstants.TURRET_SHOOT_GAINS; + private Gains swerveGains = ShooterConstants.SWERVE_SHOOT_GAINS; private double error; private double prevError; private double kP, kI, kD; private double proportional, integral, derivative; private double output, normOutput; private double tolerance; + + boolean timerStarted; + long startTime; + private double timeTolerance; private boolean isAimedInTolerance; private int inverted; @@ -67,88 +72,95 @@ public class Shoot extends CommandBase { this.hood = hood; this.toShoot = toShoot; - - addRequirements(this.swerve, this.drum, this.turret, this.hood); - - kP = gains.kP; - kI = gains.kI; - kD = gains.kD; - + + kP = turretGains.kP; + kI = turretGains.kI; + kD = turretGains.kD; + proportional = 0; integral = 0; derivative = 0; - + tolerance = 10.0; + timeTolerance = 500; isAimedInTolerance = false; - } + this.inverted = 1; + + addRequirements(this.swerve, this.drum, this.turret, this.hood); + } + // 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; - if (error > 180) { - error = 360 - error; - this.inverted = -1; } else { this.inverted = 1; } - isAimedInTolerance = (Math.abs(error) <= tolerance); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - - // this.turret.gotoMidpoint(); - - this.odoX = 0;//-m_swerve.getOdometry().getY(); - this.odoY = -8;//-m_swerve.getOdometry().getX(); - - this.distance = Math.hypot(odoX, odoY); - - this.gyroAngle = this.swerve.getRegGyro().getDegrees(); - this.initialSwerveRotation = gyroAngle; - - // get targets (shooter tables) - this.targetVel = drum.getVelocity(distance); - this.targetHood = drum.getHood(distance); - - this.targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); + // this(swerve, drum, turret, hood, false); + // } - // initial error - updateError(); - prevError = error; - } - - /** - * Run custom PID. - */ - public void runPID() { - prevError = error; - updateError(); + /** + * Updates error for custom PID. + */ + public void updateError() { + targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); + + error = (targetAngle - turret.getBoomBoomAngleDegrees()) % 360; + // if (error > 180) { + // error = 360 - error; // TODO: error - 360 + // this.inverted = -1; } else { this.inverted = 1; } + isAimedInTolerance = (Math.abs(error) <= tolerance); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + timerStarted = false; + startTime = 0; + + // this.turret.gotoMidpoint(); + + this.odoX = 0;//-m_swerve.getOdometry().getY(); + this.odoY = -8;//-m_swerve.getOdometry().getX(); + + this.distance = Math.hypot(odoX, odoY); + + this.gyroAngle = this.swerve.getRegGyro().getDegrees(); + this.initialSwerveRotation = gyroAngle; + + // get targets (shooter tables) + this.targetVel = drum.getVelocity(distance); + this.targetHood = drum.getHood(distance); + + this.targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); + + // initial error + updateError(); + prevError = error; + } + + /** + * Run custom PID. + */ + public void runPID() { + prevError = error; + updateError(); - this.proportional = error; - this.integral = integral + (error * Constants.LOOP_TIME); - this.derivative = (error - prevError) / Constants.LOOP_TIME; - this.output = kP * proportional + kI * integral + kD * derivative; - this.normOutput = (output / 360) * inverted; - } - + this.proportional = error; + this.integral = integral + (error * Constants.LOOP_TIME); + this.derivative = (error - prevError) / Constants.LOOP_TIME; + this.output = kP * proportional + kI * integral + kD * derivative; + this.normOutput = (output / 360) * inverted; + } + // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - + runPID(); - + SmartDashboard.putNumber("Error", this.error); SmartDashboard.putNumber("Shoot.java TargetAngle", this.targetAngle); SmartDashboard.putNumber("Normalized Output", this.normOutput); - this.turret.m_boomBoomRotateMotor.set(normOutput); - this.swerve.driveWithInput(0, 0, normOutput, false); // ? should the output be field relative + this.turret.runTurretWithCustomPID(normOutput); + // this.turret.m_boomBoomRotateMotor.set(normOutput); + this.swerve.driveWithInput(0, 0, normOutput * (this.swerveGains.kP/this.turretGains.kP), false); // ? should the output be field relative if (this.toShoot) { this.hood.runAngleAdjustPID(this.targetHood); @@ -160,6 +172,7 @@ public class Shoot extends CommandBase { @Override public void end(boolean interrupted) { + System.err.println("SHOOT IS FINISHED: " + Boolean.toString(interrupted).toUpperCase()); // ? return to initial swerve rotation // swerve.driveWithInput(0, 0, initialSwerveRotation, true); @@ -179,7 +192,12 @@ public class Shoot extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - SmartDashboard.putBoolean("isAimedInTolerance", isAimedInTolerance); - return isAimedInTolerance; + + if (isAimedInTolerance && !timerStarted) { + timerStarted = true; + startTime = System.currentTimeMillis(); + } + SmartDashboard.putBoolean("isDone", isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); + return (isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 72a7194..ed9c193 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -82,10 +82,9 @@ public class SwerveDrive extends SubsystemBase { SmartDashboard.putData("Field", m_field); } - // https://github.com/ZachOrr/MK3-Swerve-Example /** * Method to drive the robot using joystick info. - * + * @link https://github.com/ZachOrr/MK3-Swerve-Example * @param speeds[0] Speed of the robot in the x direction (forward). * @param speeds[1] Speed of the robot in the y direction (sideways). * @param rot Angular rate of the robot. @@ -93,10 +92,7 @@ public class SwerveDrive extends SubsystemBase { * field. */ public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative) { - if (speedX == 0 && speedY == 0 && rot == 0) - ignoreAngles = true; - else - ignoreAngles = false; + ignoreAngles = (speedX == 0) && (speedY == 0) && (rot == 0); Translation2d speed = new Translation2d(speedX, speedY); double mag = speed.getNorm(); diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 1ba15e6..3ebfabc 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -206,6 +206,10 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER * this.speedLimiter * this.calibrationSpeed); } + public void runTurretWithCustomPID(double input) { + m_boomBoomRotateMotor.set(input * this.speedLimiter); + } + public void runShooterRotatePID(double targetAngle) { double softMid = (ShooterConstants.TURRET_FORWARD_SOFT_LIMIT - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) / 2;