diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 004b378..fbae619 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -272,7 +272,7 @@ public class RobotContainer { .whenReleased(() -> m_robotSwerveDrive.stopModules()); new JoystickButton(getDriverController(), XboxController.Button.kB.value) - .whileHeld(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)) + .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) @@ -341,12 +341,20 @@ public class RobotContainer { new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) + .whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret)) + .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) + .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood)) + .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) // .whenPressed(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(false), m_robotClimber)) - + .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) // .whenReleased(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(true), m_robotClimber)) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java index 882e010..de4b234 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java @@ -4,6 +4,9 @@ package frc4388.robot.commands.ShooterCommands; +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; @@ -15,9 +18,11 @@ import frc4388.robot.subsystems.VisionOdometry; public class AimToCenter extends CommandBase { /** Creates a new AimWithOdometry. */ Turret m_turret; - SwerveDrive m_drive; VisionOdometry m_visionOdometry; + Supplier supplier; + Pose2d odo; + // use odometry to find x and y later double x; double y; @@ -25,28 +30,35 @@ public class AimToCenter extends CommandBase { // public static Gains m_aimGains; - public AimToCenter(Turret turret, SwerveDrive drive, VisionOdometry visionOdometry) { + public AimToCenter(Turret turret, VisionOdometry visionOdometry, Supplier supplier) { // Use addRequirements() here to declare subsystem dependencies. m_turret = turret; - m_drive = drive; m_visionOdometry = visionOdometry; - addRequirements(m_turret, m_drive, m_visionOdometry); + + this.supplier = supplier; + + addRequirements(m_turret, m_visionOdometry); } // Called when the command is initially scheduled. @Override public void initialize() { - x = -m_drive.getOdometry().getY(); - y = -m_drive.getOdometry().getX(); - + odo = this.supplier.get(); + // ! Yes I realize this stupid, yes it works I promise, coordinate system is funky + x = -odo.getY(); + y = -odo.getX(); + SmartDashboard.putNumber("trans x", x); SmartDashboard.putNumber("trans y", y); } - + // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_targetAngle = (aaravAngleToCenter(x, y, m_drive.getOdometry().getRotation().getDegrees())) % 360; + + odo = this.supplier.get(); // * update odometry using really cool supplier -aarav + + m_targetAngle = (aaravAngleToCenter(x, y, odo.getRotation().getDegrees())) % 360; SmartDashboard.putNumber("Target Angle", m_targetAngle); m_turret.runShooterRotatePID(m_targetAngle); diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java new file mode 100644 index 0000000..a706512 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.ShooterCommands; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Hood; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.VisionOdometry; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class Seek extends SequentialCommandGroup { + + /** Seeks. + * Seeking -> Sought + * @author Aarav Shah + * @blame Aarav Shah (thomas did this) + */ + 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)); + } +} diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index 339785d..d172256 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -20,22 +20,24 @@ import frc4388.utility.Gains; public class Shoot extends CommandBase { // subsystems - private SwerveDrive m_swerve; - private BoomBoom m_boomBoom; - private Turret m_turret; - private Hood m_hood; + private SwerveDrive swerve; + private BoomBoom drum; + private Turret turret; + private Hood hood; + + private boolean toShoot; // given - private double m_gyroAngle; - private double m_odoX; - private double m_odoY; - private double m_distance; + private double gyroAngle; + private double odoX; + private double odoY; + private double distance; // targets - private double m_targetVel; - private double m_targetHood; - private double m_targetAngle; - private Pose2d m_targetPoint; + private double targetVel; + private double targetHood; + private double targetAngle; + private Pose2d targetPoint; // pid private double error; @@ -59,20 +61,24 @@ public class Shoot extends CommandBase { /** * Creates a new shoot command, allowing the robot to aim and be ready to fire a ball - * TODO: Velocity Correction - * @param sDrive Drive Train - * @param sShooter Shooter Drum - * @param sTurret Shooter Turret - * @param sHood Shooter Hood + * + * @param swerve Drive Train + * @param drum Shooter Drum + * @param turret Shooter Turret + * @param hood Shooter Hood + * + * @author Aarav Shah */ - public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Turret sTurret, Hood sHood) { + public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, boolean toShoot) { // Use addRequirements() here to declare subsystem dependencies. - m_swerve = sDrive; - m_boomBoom = sShooter; - m_turret = sTurret; - m_hood = sHood; - - addRequirements(m_swerve, m_boomBoom, m_turret, m_hood); + this.swerve = swerve; + this.drum = drum; + this.turret = turret; + this.hood = hood; + + this.toShoot = toShoot; + + addRequirements(this.swerve, this.drum, this.turret, this.hood); kP = gains.kP; kI = gains.kI; @@ -83,7 +89,7 @@ public class Shoot extends CommandBase { derivative = 0; time = 0.02; - tolerance = 5.0; + tolerance = 10.0; isAimedInTolerance = false; if (simMode) { @@ -94,20 +100,24 @@ public class Shoot extends CommandBase { } } + public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) { + this(swerve, drum, turret, hood, false); + } + /** * Updates error for custom PID. */ public void updateError() { - m_targetPoint = SwerveDriveConstants.HUB_POSE; + targetPoint = SwerveDriveConstants.HUB_POSE; // m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get()); - m_targetAngle = AimToCenter.aaravAngleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees()); + targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); // error = (m_targetAngle - turretDummy.get() + 360) % 360; - error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360; + error = (targetAngle - turret.getBoomBoomAngleDegrees() + 360) % 360; isAimedInTolerance = (Math.abs(error) <= tolerance); if (simMode) { SmartDashboard.putBoolean("isAimed?", isAimedInTolerance); - System.out.println("Target Angle: " + m_targetAngle); + System.out.println("Target Angle: " + targetAngle); System.out.println("Error: " + error); } } @@ -116,34 +126,32 @@ public class Shoot extends CommandBase { @Override public void initialize() { - m_turret.gotoMidpoint(); + turret.gotoMidpoint(); - m_odoX = 0;//-m_swerve.getOdometry().getY(); - m_odoY = -8;//-m_swerve.getOdometry().getX(); + odoX = 0;//-m_swerve.getOdometry().getY(); + odoY = -8;//-m_swerve.getOdometry().getX(); - m_gyroAngle = m_swerve.getRegGyro().getDegrees(); - initialSwerveRotation = m_gyroAngle; + gyroAngle = swerve.getRegGyro().getDegrees(); + initialSwerveRotation = gyroAngle; // get targets (shooter tables) - m_targetVel = m_boomBoom.getVelocity(m_distance); - m_targetHood = m_boomBoom.getHood(m_distance); + targetVel = drum.getVelocity(distance); + targetHood = drum.getHood(distance); - m_targetAngle = AimToCenter.aaravAngleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees()); - // m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.; + targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); // deadzone processing - if (AimToCenter.isDeadzone(m_targetAngle)) {} + if (AimToCenter.isDeadzone(targetAngle)) {} // initial error updateError(); - // System.out.println("Error: " + error); prevError = error; } /** * Run custom PID. */ public void runPID() { - if (error > 180){ + if (error > 180) { error = 360 - error; inverted = -1; } @@ -154,10 +162,10 @@ public class Shoot extends CommandBase { updateError(); proportional = error; - integral = integral + error * time; + integral = integral + (error * time); derivative = (error - prevError) / time; output = kP * proportional + kI * integral + kD * derivative; - normOutput = output/360 * inverted; + normOutput = (output / 360) * inverted; } // Called every time the scheduler runs while the command is scheduled. @@ -174,30 +182,32 @@ public class Shoot extends CommandBase { driveDummy.apply(normOutput); System.out.println("Drive Dummy: " + driveDummy.get()); } - + runPID(); + SmartDashboard.putNumber("Error", this.error); - SmartDashboard.putNumber("Shoot.java TargetAngle", this.m_targetAngle); + SmartDashboard.putNumber("Shoot.java TargetAngle", this.targetAngle); SmartDashboard.putNumber("Normalized Output", normOutput); - m_swerve.driveWithInput(0, 0, normOutput, true); // i have no idea if this is how you rotate the - // entire swerve drive or its the line below - // m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true); - - // m_hood.runAngleAdjustPID(m_targetHood); - // m_boomBoom.runDrumShooterVelocityPID(m_targetVel); + + swerve.driveWithInput(0, 0, normOutput, true); + 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()); } - m_turret.m_boomBoomRotateMotor.set(normOutput); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { // return to initial swerve rotation - m_swerve.driveWithInput(0, 0, initialSwerveRotation, true); + // swerve.driveWithInput(0, 0, initialSwerveRotation, true); } // Returns true when the command should end. diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 5f720d4..6176c9a 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -35,6 +35,7 @@ public class Hood extends SubsystemBase { public double m_fireAngle; public double speedLimiter; + public double calibrationSpeed = 1.0; /** Creates a new Hood. */ public Hood(CANSparkMax angleAdjusterMotor) { @@ -63,23 +64,30 @@ public class Hood extends SubsystemBase { SmartDashboard.putNumber("Hood Angle", m_angleEncoder.getPosition()); // * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output). - double currentPos = this.getEncoderPosition(); - double forwardDistance = Math.abs(currentPos - ShooterConstants.HOOD_FORWARD_SOFT_LIMIT); - double reverseDistance = Math.abs(currentPos - ShooterConstants.HOOD_REVERSE_SOFT_LIMIT); - if (forwardDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) { - this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)); - } + if (areSoftLimitsEnabled()) { + double currentPos = this.getEncoderPosition(); + double forwardDistance = Math.abs(currentPos - ShooterConstants.HOOD_FORWARD_SOFT_LIMIT); + double reverseDistance = Math.abs(currentPos - ShooterConstants.HOOD_REVERSE_SOFT_LIMIT); - if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) { - this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)); - } + if (forwardDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) { + this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)); + } - if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) { - this.speedLimiter = 1.0; + if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) { + this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)); + } + + if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) { + this.speedLimiter = 1.0; + } } } + public boolean areSoftLimitsEnabled() { + return this.m_angleAdjusterMotor.isSoftLimitEnabled(SoftLimitDirection.kForward) && this.m_angleAdjusterMotor.isSoftLimitEnabled(SoftLimitDirection.kReverse); + } + /** * Set status of hood motor soft limits. * @param set Boolean to set soft limits to. @@ -107,7 +115,7 @@ public class Hood extends SubsystemBase { * @param input value from -1.0 to 1.0, postive is upward (more horizontal shootijng angle) */ public void runHood(double input) { - m_angleAdjusterMotor.set(input * this.speedLimiter); + m_angleAdjusterMotor.set(input * this.speedLimiter * this.calibrationSpeed); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 95a41d0..d8c59d4 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -46,7 +46,8 @@ public class Turret extends SubsystemBase { long leftCurrentTime; long leftElapsedTime; - double speedLimiter; + public double speedLimiter; + public double calibrationSpeed = 1.0; public Turret(CANSparkMax boomBoomRotateMotor) { @@ -148,23 +149,31 @@ public class Turret extends SubsystemBase { // * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output). - double currentPos = this.getEncoderPosition(); - double forwardDistance = Math.abs(currentPos - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT); - double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); - if (forwardDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) { - this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)); - } + if (areSoftLimitsEnabled()) { - if (reverseDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) { - this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)); - } + double currentPos = this.getEncoderPosition(); + double forwardDistance = Math.abs(currentPos - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT); + double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); - if ((forwardDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)) { - this.speedLimiter = 1.0; + if (forwardDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) { + this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)); + } + + if (reverseDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) { + this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)); + } + + if ((forwardDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)) { + this.speedLimiter = 1.0; + } } } + public boolean areSoftLimitsEnabled() { + return this.m_boomBoomRotateMotor.isSoftLimitEnabled(SoftLimitDirection.kForward) && this.m_boomBoomRotateMotor.isSoftLimitEnabled(SoftLimitDirection.kReverse); + } + /** * Set status of turret motor soft limits. * @param set Boolean to set soft limits to. @@ -192,7 +201,7 @@ public class Turret extends SubsystemBase { * @param input from -1.0 to 1.0, positive is clockwise */ public void runTurretWithInput(double input) { - m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER * this.speedLimiter); + m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER * this.speedLimiter * this.calibrationSpeed); } public void runShooterRotatePID(double targetAngle) {