From 6f015a2f835ad805934c2c5fbfd0b5255255226e Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 19 Mar 2022 22:16:20 -0600 Subject: [PATCH 1/2] sum shoot change --- src/main/java/frc4388/robot/Constants.java | 1 + .../robot/commands/ShooterCommands/Shoot.java | 99 ++++++------------- 2 files changed, 29 insertions(+), 71 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 8f945b7..64e38b1 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -34,6 +34,7 @@ import frc4388.utility.LEDPatterns; */ public final class Constants { public static final double TICKS_PER_ROTATION_FX = 2048; + public static final double LOOP_TIME = 0.02; public static final class SwerveDriveConstants { public static final double ROTATION_SPEED = 3.0; diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index 2d84ef5..7e560be 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.BoomBoom; @@ -21,44 +22,33 @@ public class Shoot extends CommandBase { // subsystems private SwerveDrive swerve; - private BoomBoom drum; private Turret turret; + private BoomBoom drum; private Hood hood; private boolean toShoot; // given - private double gyroAngle; - private double odoX; - private double odoY; + private double odoX, odoY; private double distance; + private double gyroAngle; // targets - private double targetVel; - private double targetHood; - private double targetAngle; - private Pose2d targetPoint; + private double targetAngle, targetVel, targetHood; // pid + private Gains gains = ShooterConstants.SHOOT_GAINS; private double error; private double prevError; - private Gains gains = ShooterConstants.SHOOT_GAINS; private double kP, kI, kD; private double proportional, integral, derivative; - private double time; - private double output; - private double normOutput; + private double output, normOutput; private double tolerance; private boolean isAimedInTolerance; private int inverted; private double initialSwerveRotation; - // testing - private boolean simMode = false; - private DummySensor driveDummy; - private DummySensor turretDummy; - /** * Creates a new shoot command, allowing the robot to aim and be ready to fire a ball * @@ -87,17 +77,9 @@ public class Shoot extends CommandBase { proportional = 0; integral = 0; derivative = 0; - time = 0.02; tolerance = 10.0; isAimedInTolerance = false; - - if (simMode) { - driveDummy = new DummySensor(180); - turretDummy = new DummySensor(180); - - DummySensor.resetAll(); - } } public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) { @@ -108,37 +90,30 @@ public class Shoot extends CommandBase { * Updates error for custom PID. */ public void updateError() { - targetPoint = SwerveDriveConstants.HUB_POSE; - // m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get()); targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); - // error = (m_targetAngle - turretDummy.get() + 360) % 360; error = (targetAngle - turret.getBoomBoomAngleDegrees() + 360) % 360; isAimedInTolerance = (Math.abs(error) <= tolerance); - - if (simMode) { - SmartDashboard.putBoolean("isAimed?", isAimedInTolerance); - System.out.println("Target Angle: " + targetAngle); - System.out.println("Error: " + error); - } } // Called when the command is initially scheduled. @Override public void initialize() { - turret.gotoMidpoint(); + this.turret.gotoMidpoint(); - odoX = 0;//-m_swerve.getOdometry().getY(); - odoY = -8;//-m_swerve.getOdometry().getX(); + this.odoX = 0;//-m_swerve.getOdometry().getY(); + this.odoY = -8;//-m_swerve.getOdometry().getX(); - gyroAngle = swerve.getRegGyro().getDegrees(); - initialSwerveRotation = gyroAngle; + this.distance = Math.hypot(odoX, odoY); + + this.gyroAngle = this.swerve.getRegGyro().getDegrees(); + this.initialSwerveRotation = gyroAngle; // get targets (shooter tables) - targetVel = drum.getVelocity(distance); - targetHood = drum.getHood(distance); + this.targetVel = drum.getVelocity(distance); + this.targetHood = drum.getHood(distance); - targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); + this.targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); // deadzone processing if (AimToCenter.isDeadzone(targetAngle)) {} @@ -147,60 +122,45 @@ public class Shoot extends CommandBase { updateError(); prevError = error; } + /** * Run custom PID. */ public void runPID() { if (error > 180) { error = 360 - error; - inverted = -1; + this.inverted = -1; } else{ - inverted = 1; + this.inverted = 1; } prevError = error; updateError(); - proportional = error; - integral = integral + (error * time); - derivative = (error - prevError) / time; - output = kP * proportional + kI * integral + kD * derivative; - 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() { - - if (simMode) { - System.out.println("Normalized Output: " + normOutput); - } - // custom pid - - if (simMode) { - driveDummy.apply(normOutput); - System.out.println("Drive Dummy: " + driveDummy.get()); - } - runPID(); SmartDashboard.putNumber("Error", this.error); SmartDashboard.putNumber("Shoot.java TargetAngle", this.targetAngle); - SmartDashboard.putNumber("Normalized Output", normOutput); + SmartDashboard.putNumber("Normalized Output", this.normOutput); - swerve.driveWithInput(0, 0, normOutput, true); - turret.m_boomBoomRotateMotor.set(normOutput); + this.swerve.driveWithInput(0, 0, normOutput, true); + this.turret.m_boomBoomRotateMotor.set(normOutput); if (this.toShoot) { this.hood.runAngleAdjustPID(this.targetHood); this.drum.runDrumShooterVelocityPID(this.targetVel); } - - if (simMode) { - turretDummy.apply(normOutput); - System.out.println("Turret Dummy: " + turretDummy.get()); - } } // Called once the command ends or is interrupted. @@ -213,10 +173,7 @@ public class Shoot extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - // if (simMode) { SmartDashboard.putBoolean("isAimedInTolerance", isAimedInTolerance); return isAimedInTolerance; - // } - // return false; } } From a2434a8fde3c70c30f490978a7ee34b7234a69ad Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 19 Mar 2022 22:24:45 -0600 Subject: [PATCH 2/2] very smol change --- .../robot/commands/ShooterCommands/Shoot.java | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index 7e560be..f14f449 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -114,9 +114,6 @@ public class Shoot extends CommandBase { this.targetHood = drum.getHood(distance); this.targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); - - // deadzone processing - if (AimToCenter.isDeadzone(targetAngle)) {} // initial error updateError(); @@ -166,8 +163,17 @@ public class Shoot extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - // return to initial swerve rotation + + // ? return to initial swerve rotation // swerve.driveWithInput(0, 0, initialSwerveRotation, true); + + this.swerve.driveWithInput(0.0, 0.0, 0.0, 0.0, true); + this.turret.m_boomBoomRotateMotor.set(0.0); + + if (this.toShoot) { + this.hood.runHood(0.0); + this.drum.runDrumShooter(0.0); + } } // Returns true when the command should end.