diff --git a/RapidReact2606/src/main/java/frc/robot/Constants.java b/RapidReact2606/src/main/java/frc/robot/Constants.java index 22b0ba8..15a05c9 100644 --- a/RapidReact2606/src/main/java/frc/robot/Constants.java +++ b/RapidReact2606/src/main/java/frc/robot/Constants.java @@ -102,6 +102,7 @@ public static final class VisionConstants{ //Sim Position public static final org.photonvision.SimVisionTarget farTarget = new SimVisionTarget(farTargetPose,targetHeightAboveGround, targetWidth, targetHeight); + public static final double cameraHeight = .5; } public static final class IndexerConstants{ public static final int firstDIO = 11; diff --git a/RapidReact2606/src/main/java/frc/robot/RobotContainer.java b/RapidReact2606/src/main/java/frc/robot/RobotContainer.java index 02577ff..d797127 100644 --- a/RapidReact2606/src/main/java/frc/robot/RobotContainer.java +++ b/RapidReact2606/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,7 @@ import frc.robot.commands.Intake.SimpleIntakeOn; import frc.robot.commands.Intake.SimpleIntakeOnVar; import frc.robot.commands.Shooter.SimpleShooterOn; + // import frc.robot.commands.ExampleCommand; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.IndexerSubsystem; @@ -42,7 +43,7 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final DriveSubsystem robotDrive = new DriveSubsystem(); private final IntakeSubsystem intakeSystem = new IntakeSubsystem(); - private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(); + private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(robotDrive.getCamera()); private final IndexerSubsystem indexSubsystem = new IndexerSubsystem(); diff --git a/RapidReact2606/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/RapidReact2606/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 7edbbfe..059a7e3 100644 --- a/RapidReact2606/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/RapidReact2606/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -10,18 +10,28 @@ import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonUtils; +import org.photonvision.targeting.PhotonPipelineResult; + +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ShooterConstants; -import frc.robot.commands.Shooter.isShooterUpToSpeed; +import frc.robot.Constants.VisionConstants; public class ShooterSubsystem extends SubsystemBase { + /** + * + */ + private static final int shooterTolerance = 50; + CANSparkMax shooter = new CANSparkMax(ShooterConstants.shooterCan, MotorType.kBrushless); - + RelativeEncoder shootEncoder; SparkMaxPIDController pidController; public double kP, kI, kD, KIz, kFF, kMaxOutput, kMinOutput, maxRPM; - + private PhotonCamera camera; public enum MOTOR_STATUS { ON, OFF @@ -30,12 +40,13 @@ public enum MOTOR_STATUS { MOTOR_STATUS motorState = MOTOR_STATUS.OFF; /** Creates a new ExampleSubsystem. */ - public ShooterSubsystem() { + public ShooterSubsystem(PhotonCamera cam) { shooter.setIdleMode(IdleMode.kCoast); shootEncoder = shooter.getEncoder(); pidController = shooter.getPIDController(); + camera = cam; - //PID coefficients + // PID coefficients kP = 6e-5; kI = 0; kD = 0; @@ -45,8 +56,8 @@ public ShooterSubsystem() { kMinOutput = -1; maxRPM = 5700; - pidController.setP(kP); //position - pidController.setI(kI); //Integral + pidController.setP(kP); // position + pidController.setI(kI); // Integral pidController.setD(kD); // Derivitive pidController.setIZone(KIz); // INtegral zone pidController.setFF(kFF); // feed forward gain value @@ -55,10 +66,10 @@ public ShooterSubsystem() { @Override public void periodic() { - //updateMotorsProp(); + // updateMotorsProp(); updateMotorsVel(); SmartDashboard.putNumber("Encoder Vel", shootEncoder.getVelocity()); - SmartDashboard.putBoolean("Is Up to Speed", isUpToSpeed()); + SmartDashboard.putBoolean("Is Up to Speed", isUpToSpeed()); } public void updateMotorsProp() { @@ -69,24 +80,33 @@ public void updateMotorsProp() { } } - public void updateMotorsVel(){ - if(motorState == MOTOR_STATUS.OFF){ + public void updateMotorsVel() { + if (motorState == MOTOR_STATUS.OFF) { shooter.set(0.0); - } - else{ - pidController.setReference(5000*-4.284*ShooterConstants.shootSpeed, CANSparkMax.ControlType.kVelocity); + } else { + pidController.setReference(getSpeedTarget(), CANSparkMax.ControlType.kVelocity); } } + public double getSpeedTarget() { + PhotonPipelineResult result = camera.getLatestResult(); + double speed = 5000 * -4.284 * ShooterConstants.shootSpeed; - + if (result.hasTargets()) { + double range = PhotonUtils.calculateDistanceToTargetMeters(VisionConstants.cameraHeight, VisionConstants.targetHeight, 0, Units.degreesToRadians(result.getBestTarget().getPitch())); + //vary speed based on range + speed = 5000 * -4.284 * ShooterConstants.shootSpeed; + } + + return speed; + } public void setMode(MOTOR_STATUS mode) { motorState = mode; } - public boolean isUpToSpeed(){ - return shootEncoder.getVelocity() < -5250; + public boolean isUpToSpeed() { + return Math.abs(shootEncoder.getVelocity() - getSpeedTarget()) < shooterTolerance; } @Override