diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f32d291..afeafba 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -79,8 +79,8 @@ public static final class Intake { public static final class Vision { //Target - public static final double kYawTarget = -8; //Degrees - public static final double kDistanceTarget = 1.8; //Meters + public static final double kYawTarget = -9; //Degrees + public static final double kDistanceTarget = 1.9; //Meters public static final AprilTagFieldLayout kAprilTagFieldLayout = AprilTagFieldLayout .loadField(AprilTagFields.kDefaultField); @@ -93,6 +93,15 @@ public static final class Vision { Units.inchesToMeters(158.84), new Rotation2d(0.0)); + public static final Pose2d blueClimbPos = new Pose2d( + Units.inchesToMeters(163.86), + Units.inchesToMeters(21.75 + 11.46), + new Rotation2d(0.0)); + public static final Pose2d redClimbPos = new Pose2d( + Units.inchesToMeters(649.57), + Units.inchesToMeters(152.78 - 11.46), + new Rotation2d(0.0)); + public static final Transform3d kCameraToRobot = new Transform3d( -0.1016, // forward from robot center 0.0, // left/right camera is centered @@ -105,7 +114,7 @@ public static final class Agitator { // Ports public static final int kAgitatorPort = 7;//PWM // Speeds - public static final double kMaxAgitatorSpeed = 1.0; // Agitator Speed = 30% + public static final double kMaxAgitatorSpeed = 0.7; // Agitator Speed = 30% } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8fa786f..3c3d864 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,6 +8,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj.DriverStation; /** @@ -19,6 +21,7 @@ */ public class Robot extends TimedRobot { private Command m_autonomousCommand; + private Command m_driveForwardCommand; private final RobotContainer m_robotContainer; // Added fields @@ -82,16 +85,22 @@ public void disabledPeriodic() { @Override public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + m_driveForwardCommand = m_robotContainer.DriveForwardCommand(); // schedule the autonomous command (example) if (m_autonomousCommand != null) { - CommandScheduler.getInstance().schedule(m_autonomousCommand); + CommandScheduler.getInstance().schedule(Commands.sequence( + m_driveForwardCommand, + new WaitCommand(2.0), // Wait 2 Seconds + m_autonomousCommand.repeatedly()) + ); } } /** This function is called periodically during autonomous. */ @Override public void autonomousPeriodic() { + } @Override @@ -102,6 +111,7 @@ public void teleopInit() { // this line or comment it out. if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); + m_driveForwardCommand.cancel(); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 24b7952..6dd4811 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -43,8 +43,8 @@ public class RobotContainer { private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem(); private final AgitatorSubsystem m_AgitatorSubsystem = new AgitatorSubsystem(); private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); - private final PhotonVision m_photonVision = new PhotonVision(m_driveSubsystem, m_ShooterSubsystem, m_AgitatorSubsystem, m_IntakeSubsystem); - + private final PhotonVision m_photonVision = new PhotonVision(m_driveSubsystem, m_ShooterSubsystem, m_AgitatorSubsystem, m_IntakeSubsystem, m_ClimberSubsystem); + // Replace with CommandPS4Controller or CommandJoystick if needed private final CommandXboxController m_driverController = new CommandXboxController(Driver.kJoystickID); private final CommandXboxController m_operatorController = new CommandXboxController(Operator.kJoystickID); @@ -57,6 +57,7 @@ public class RobotContainer { */ public RobotContainer() { + autoChooser = AutoBuilder.buildAutoChooser(); // Configure the trigger bindings @@ -107,9 +108,8 @@ private double applyDeadbandAndScale(double value) { // Register named commands for use in PathPlanner autonomous paths private void registerNamedCommands() { - NamedCommands.registerCommand("Shoot_1", m_ShooterSubsystem.shoot_1_Command()); NamedCommands.registerCommand("Shooter_ON", m_ShooterSubsystem.StartShoot()); - NamedCommands.registerCommand("Shooter_OFF", m_ShooterSubsystem.StopShoot()); + NamedCommands.registerCommand("Shoot_1", m_ShooterSubsystem.StartShoot()); NamedCommands.registerCommand("Climb_UP", m_ClimberSubsystem.UpClimb()); NamedCommands.registerCommand("Climb_DOWN", m_ClimberSubsystem.DownClimb()); NamedCommands.registerCommand("Intake_ON", m_IntakeSubsystem.StartIntake()); @@ -122,12 +122,22 @@ private void configureBindings() { // Driver Controls // Shooter control m_driverController.rightBumper() - .whileTrue(m_photonVision.AimShoot()); + .onTrue(m_photonVision.AimShoot()); + m_operatorController.leftBumper() + .onTrue(m_photonVision.AimClimb()); + // Start Shooter (constant speed) m_driverController.rightTrigger() - .whileTrue(Commands.parallel(m_ShooterSubsystem.StartShoot(), m_AgitatorSubsystem.StartAgitator(), m_IntakeSubsystem.StartIntake())); - // .onFalse(Commands.parallel(m_ShooterSubsystem.StopShoot(), m_AgitatorSubsystem.StopAgitator(), m_IntakeSubsystem.StopIntake())); + .onTrue(Commands.parallel(m_ShooterSubsystem.StartShoot(), m_AgitatorSubsystem.StartAgitator(), m_IntakeSubsystem.StartIntake())); + + + // m_operatorController.b() + // .whileTrue(m_photonVision.AimClimb()) + // .onFalse(m_photonVision.resetClimb()); + +m_driverController.rightBumper().negate().and(m_driverController.rightTrigger().negate()) + .onTrue(Commands.parallel(m_ShooterSubsystem.StopShoot(), m_AgitatorSubsystem.StopAgitator(), m_IntakeSubsystem.StopIntake())); // Climber control m_operatorController.a() @@ -172,6 +182,10 @@ private void configureBindings() { public Command getAutonomousCommand() { // An example command will be run in autonomous - return autoChooser.getSelected(); + return m_photonVision.AimShoot(); + } + public Command DriveForwardCommand() { + // An example command will be run in autonomous + return m_driveSubsystem.DriveForward(); } } diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index ae5faeb..2d6d128 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -4,14 +4,15 @@ package frc.robot.commands; -import frc.robot.subsystems.AgitatorSubsystem; +import frc.robot.subsystems.PhotonVision; +import frc.robot.subsystems.DriveSubsystem; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; public final class Autos { /** Example static factory for an autonomous command. */ - public static Command exampleAuto(AgitatorSubsystem subsystem) { - return Commands.sequence(subsystem.StartAgitator(), new ExampleCommand(subsystem)); + public static Command exampleAuto(PhotonVision vision, DriveSubsystem drive) { + return Commands.sequence(vision.AimShoot()); } private Autos() { diff --git a/src/main/java/frc/robot/subsystems/AgitatorSubsystem.java b/src/main/java/frc/robot/subsystems/AgitatorSubsystem.java index c3f89b1..9fa4fc6 100644 --- a/src/main/java/frc/robot/subsystems/AgitatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/AgitatorSubsystem.java @@ -61,6 +61,9 @@ public Command StartAgitator() { } + public void StartAgitatorVoid() { + AgitatorMotor.set(-Constants.Subsystems.Agitator.kMaxAgitatorSpeed); } + public Command StopAgitator() { return this.run(() -> { diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index e6a44e8..e04faac 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -3,13 +3,13 @@ import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Field2d; - // For CAN import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; @@ -24,17 +24,17 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.*; +import frc.robot.subsystems.PhotonVision; import java.util.Optional; import com.ctre.phoenix6.hardware.Pigeon2; //Gyro public class DriveSubsystem extends SubsystemBase { - - private final Field2d field = new Field2d(); + private final Field2d field = new Field2d(); private final Pigeon2 pigeon = new Pigeon2(Constants.Subsystems.Drive.kGyroPort); @@ -50,8 +50,8 @@ public class DriveSubsystem extends SubsystemBase { private final RelativeEncoder rightEncoder = rightMaster.getEncoder(); private final RelativeEncoder leftEncoder = leftMaster.getEncoder(); - - + boolean back = false; + boolean done = false; double leftDis = 0; double leftPos = 0; double rightPos = 0; @@ -65,27 +65,21 @@ public class DriveSubsystem extends SubsystemBase { private Pose2d currentPose = new Pose2d(); - private final DifferentialDrivePoseEstimator m_poseEstimator = - new DifferentialDrivePoseEstimator( - Constants.Subsystems.Drive.kinematics, - pigeon.getRotation2d(), - leftEncoder.getPosition() / 8.45 * wheelCircumference, - rightEncoder.getPosition() / 8.45 * wheelCircumference, - new Pose2d(), - VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)), - VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30))); - - + private final DifferentialDrivePoseEstimator m_poseEstimator = new DifferentialDrivePoseEstimator( + Constants.Subsystems.Drive.kinematics, + pigeon.getRotation2d(), + leftEncoder.getPosition() / 8.45 * wheelCircumference, + rightEncoder.getPosition() / 8.45 * wheelCircumference, + new Pose2d(), + VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)), + VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30))); public DriveSubsystem() { - SmartDashboard.putData("Field", field); - leftMaster.setInverted(true); - leftFollower.setInverted(true); - - - + SmartDashboard.putData("Field", field); + leftMaster.setInverted(true); + leftFollower.setInverted(true); pigeon.setYaw(0.0); @@ -127,6 +121,33 @@ public void arcadeDrive(double fwd, double rot) { // } } + + + public Command DriveForward() { + leftDis = 0; + rightDis = 0; + done = false; + + return new RunCommand(() -> { + if (leftDis < 0.3 && !back) { + drive.arcadeDrive(-1, 0); + SmartDashboard.putNumber("Dis", leftDis); + } else if (leftDis > -0.3) { + back = true; + drive.arcadeDrive(1, 0); + + } else { + + drive.arcadeDrive(0, 0); + done = true; + + + } + + }, this).until(() -> done); + + } + public void tankDrive(double left, double right) { drive.tankDrive(left, right); } @@ -162,7 +183,7 @@ public void driveRobotRelative(ChassisSpeeds speeds) { speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond); } - + public Command resetPigeon() { return this.run(() -> { @@ -172,9 +193,14 @@ public Command resetPigeon() { } + // public Command driveForward(){ + // if () + // // drive.arcadeDrive(-0.4, 0); + + // } public Pose2d get2dPose() { - Pose2d pose = m_poseEstimator.getEstimatedPosition(); + Pose2d pose = m_poseEstimator.getEstimatedPosition(); return pose; } @@ -185,14 +211,14 @@ public void addVisionMeasurement(Pose2d visionPose, double timestampSeconds) { @Override public void periodic() { // Put periodic subsystem code here (telemetry, safety checks) - m_poseEstimator.update( - pigeon.getRotation2d(), leftDis, rightDis); + m_poseEstimator.update( + pigeon.getRotation2d(), leftDis, rightDis); double leftPosition = leftPos - leftEncoder.getPosition() / 8.45 * wheelCircumference; double rightPosition = rightPos - rightEncoder.getPosition() / 8.45 * wheelCircumference; leftPos = leftEncoder.getPosition() / 8.45 * wheelCircumference; rightPos = rightEncoder.getPosition() / 8.45 * wheelCircumference; - + leftDis = leftDis + leftPosition; rightDis = rightDis + rightPosition; SmartDashboard.putNumber("Dis", rightDis); @@ -203,8 +229,8 @@ public void periodic() { SmartDashboard.putNumber("Pigeon Yaw", pigeon.getYaw().getValueAsDouble()); - Pose2d pose = m_poseEstimator.getEstimatedPosition(); + Pose2d pose = m_poseEstimator.getEstimatedPosition(); - field.setRobotPose(pose); + field.setRobotPose(pose); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index cc31495..baad9c9 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -4,12 +4,15 @@ package frc.robot.subsystems; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; // For CAN import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkMaxConfig; // For PWM @@ -18,7 +21,8 @@ public class IntakeSubsystem extends SubsystemBase { /** Creates a new ExampleSubsystem. */ private SparkMax IntakeMotor = new SparkMax(Constants.Subsystems.Intake.kIntakePort, MotorType.kBrushless); - + private final RelativeEncoder intakeEncoder = IntakeMotor.getEncoder(); + PIDController speedPID = new PIDController(0.0008, 0.0005, 0.00005); public IntakeSubsystem() { @@ -53,14 +57,23 @@ public IntakeSubsystem() { @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Intake", intakeEncoder.getVelocity()); } public Command StartIntake() { return this.run(() -> { - IntakeMotor.set(Constants.Subsystems.Intake.kMaxIntakeSpeed); + double intake = speedPID.calculate(intakeEncoder.getVelocity(), 900); + IntakeMotor.set(intake); }); } + + public void StartIntakeVoid(){ + IntakeMotor.set(Constants.Subsystems.Intake.kMaxIntakeSpeed); + } + + + public Command ReverseIntake() { return this.run(() -> { IntakeMotor.set(-Constants.Subsystems.Intake.kMaxIntakeSpeed); diff --git a/src/main/java/frc/robot/subsystems/PhotonVision.java b/src/main/java/frc/robot/subsystems/PhotonVision.java index 1011f00..eadb42e 100644 --- a/src/main/java/frc/robot/subsystems/PhotonVision.java +++ b/src/main/java/frc/robot/subsystems/PhotonVision.java @@ -8,10 +8,12 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import frc.robot.Constants.Subsystems.Shooter; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.math.MathUtil; @@ -30,15 +32,21 @@ public class PhotonVision extends SubsystemBase { private final ShooterSubsystem m_ShooterSubsystem; private final AgitatorSubsystem m_AgitatorSubsystem; private final IntakeSubsystem m_IntakeSubsystem; + private final ClimberSubsystem m_ClimberSubsystem; private PhotonPoseEstimator photonEstimator; private PhotonCamera camera; - PIDController turnPID = new PIDController(0.08, 0.0, 0); - PIDController drivePID = new PIDController(0.6, 0, 0); + PIDController turnPID = new PIDController(0.1, 0.005, 0.01); + PIDController drivePID = new PIDController(1.05, 0.005, 0.1); + PIDController climbDrivePID = new PIDController(1.05, 0.005, 0.1); + PIDController climbTurnPID = new PIDController(0.1, 0.005, 0.01); Rotation2d targetYaw; + Rotation2d climbTargetYaw; double distanceToTarget; + double climbDistanceToTarget; double rotaioionSpeed; + boolean climbFirst = false; /** * Construct PhotonVision with shared subsystem references. @@ -47,52 +55,31 @@ public class PhotonVision extends SubsystemBase { * @param shooter shared ShooterSubsystem * @param agitator shared AgitatorSubsystem */ - public PhotonVision(DriveSubsystem drive, ShooterSubsystem shooter, AgitatorSubsystem agitator, IntakeSubsystem intake) { + public PhotonVision(DriveSubsystem drive, ShooterSubsystem shooter, AgitatorSubsystem agitator, IntakeSubsystem intake, ClimberSubsystem climb) { this.m_driveSubsystem = drive; this.m_ShooterSubsystem = shooter; this.m_AgitatorSubsystem = agitator; this.m_IntakeSubsystem = intake; + this.m_ClimberSubsystem = climb; camera = new PhotonCamera("MainCamera"); turnPID.setTolerance(3); // degrees - drivePID.setTolerance(0.06); // meters + drivePID.setTolerance(0.1524); // meters + climbTurnPID.setTolerance(3); + climbDrivePID.setTolerance(Units.inchesToMeters(2)); photonEstimator = new PhotonPoseEstimator( Constants.Subsystems.Vision.kAprilTagFieldLayout, Constants.Subsystems.Vision.kCameraToRobot); } - - - - /** - * Example command factory method. - * - * @return a command - * - * public Command exampleMethodCommand() { - * // Inline construction of command goes here. - * // Subsystem::RunOnce implicitly requires `this` subsystem. - * return runOnce( - * () -> { - * one-time action goes here - * }); - * } - * - * /** - * An example method querying a boolean state of the subsystem (for - * example, a digital sensor). - * - * @return value of some boolean subsystem state, such as a digital sensor. - * - * public boolean exampleCondition() { - * // Query some boolean state, such as a digital sensor. - * return false; - * } - */ @Override public void periodic() { + RunCamera(); + } + + public void RunCamera(){ var result = camera.getLatestResult(); if (result.hasTargets()) { Optional visionEst = Optional.empty(); @@ -109,66 +96,115 @@ public void periodic() { } } + public Command AimClimb() { + return new RunCommand(() -> { + RunCamera(); + double distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), + Constants.Subsystems.Vision.blueClimbPos); + Rotation2d targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), + Constants.Subsystems.Vision.blueClimbPos); + + var allianceOptional = DriverStation.getAlliance(); + DriverStation.Alliance alliance = allianceOptional.get(); + + if (alliance == DriverStation.Alliance.Red) { + // Distance + distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), + Constants.Subsystems.Vision.redClimbPos); + // Rotation + targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.redClimbPos); + + } else if (alliance == DriverStation.Alliance.Blue) { + // Distance + distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), + Constants.Subsystems.Vision.blueClimbPos); + + // Rotation + targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.blueClimbPos); + } else { + System.out.println("Error loading Allance color"); + } + + System.out.println("Yaw" + targetYaw.getDegrees()); + double rotaioionSpeed = turnPID.calculate(targetYaw.getDegrees(), 0); + + double driveSpeed = drivePID.calculate(distanceToTarget, 0); + + // Clamp to safty range + rotaioionSpeed = MathUtil.clamp(rotaioionSpeed, -0.7, 0.7); + driveSpeed = MathUtil.clamp(driveSpeed, -1, 1); + + if (!turnPID.atSetpoint()) { + m_driveSubsystem.arcadeDrive(0, rotaioionSpeed); + } else { + m_driveSubsystem.arcadeDrive(driveSpeed, 0); + } + + System.out.println("Turn: " + turnPID.atSetpoint() + "Drive" + drivePID.atSetpoint()); + if (turnPID.atSetpoint() && drivePID.atSetpoint()) { + m_driveSubsystem.arcadeDrive(0, 0); + m_ClimberSubsystem.UpClimb(); + System.out.println("Climbing"); + } + }, m_driveSubsystem, m_ClimberSubsystem); + } + public Command AimShoot() { return new RunCommand(() -> { - - double distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), - Constants.Subsystems.Vision.kHubPoseBlue); - Rotation2d targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), - Constants.Subsystems.Vision.kHubPoseBlue); - - var allianceOptional = DriverStation.getAlliance(); - DriverStation.Alliance alliance = allianceOptional.get(); - - if (alliance == DriverStation.Alliance.Red) { - // Distance - distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), - Constants.Subsystems.Vision.kHubPoseRed); - // Rotation - targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.kHubPoseRed); - - } else if (alliance == DriverStation.Alliance.Blue) { - // Distance - distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), - Constants.Subsystems.Vision.kHubPoseBlue); + RunCamera(); + double distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), + Constants.Subsystems.Vision.kHubPoseBlue); + Rotation2d targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), + Constants.Subsystems.Vision.kHubPoseBlue); + + var allianceOptional = DriverStation.getAlliance(); + DriverStation.Alliance alliance = allianceOptional.get(); + + if (alliance == DriverStation.Alliance.Red) { + // Distance + distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), + Constants.Subsystems.Vision.kHubPoseRed); + // Rotation + targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.kHubPoseRed); + + } else if (alliance == DriverStation.Alliance.Blue) { + // Distance + distanceToTarget = PhotonUtils.getDistanceToPose(m_driveSubsystem.get2dPose(), + Constants.Subsystems.Vision.kHubPoseBlue); - // Rotation - targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.kHubPoseBlue); - } else { - System.out.println("Error loading Allance color"); - } + // Rotation + targetYaw = PhotonUtils.getYawToPose(m_driveSubsystem.get2dPose(), Constants.Subsystems.Vision.kHubPoseBlue); + } else { + System.out.println("Error loading Allance color"); + } - System.out.println("Yaw" + targetYaw.getDegrees()); - double rotaioionSpeed = turnPID.calculate(targetYaw.getDegrees(), Constants.Subsystems.Vision.kYawTarget); + System.out.println("Yaw" + targetYaw.getDegrees()); + double rotaioionSpeed = turnPID.calculate(targetYaw.getDegrees(), Constants.Subsystems.Vision.kYawTarget); - double driveSpeed = drivePID.calculate(distanceToTarget, Constants.Subsystems.Vision.kDistanceTarget); + double driveSpeed = drivePID.calculate(distanceToTarget, Constants.Subsystems.Vision.kDistanceTarget); - // Clamp to safty range - rotaioionSpeed = MathUtil.clamp(rotaioionSpeed, -0.7, - 0.7); - driveSpeed = MathUtil.clamp(driveSpeed, -1, - 1); - - - if (!turnPID.atSetpoint()) { - m_driveSubsystem.arcadeDrive(0, rotaioionSpeed); - } else { - m_driveSubsystem.arcadeDrive(driveSpeed, 0); - } - - System.out.println("Turn: " + turnPID.atSetpoint() + "Drive" + drivePID.atSetpoint()); - if (turnPID.atSetpoint() && drivePID.atSetpoint()) { - m_driveSubsystem.arcadeDrive(0, 0); - m_ShooterSubsystem.StartShoot(); - m_AgitatorSubsystem.StartAgitator(); - m_IntakeSubsystem.StartIntake(); - } else { - m_ShooterSubsystem.StopShoot(); - m_AgitatorSubsystem.StopAgitator(); - m_IntakeSubsystem.StopIntake(); - } - + // Clamp to safty range + rotaioionSpeed = MathUtil.clamp(rotaioionSpeed, -0.7, + 0.7); + driveSpeed = MathUtil.clamp(driveSpeed, -1, + 1); + + + if (!turnPID.atSetpoint()) { + m_driveSubsystem.arcadeDrive(0, rotaioionSpeed); + } else { + m_driveSubsystem.arcadeDrive(driveSpeed, 0); + } + System.out.println("Turn: " + turnPID.atSetpoint() + "Drive" + drivePID.atSetpoint()); + if (turnPID.atSetpoint() && drivePID.atSetpoint()) { + m_driveSubsystem.arcadeDrive(0, 0); + m_ShooterSubsystem.StartShootVoid(); + m_AgitatorSubsystem.StartAgitatorVoid(); + System.out.println("Shooting"); + m_IntakeSubsystem.StartIntakeVoid(); + } }, m_driveSubsystem, m_ShooterSubsystem, m_AgitatorSubsystem, m_IntakeSubsystem); - }} \ No newline at end of file + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 39ec41d..9a9cd9e 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -8,6 +8,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.math.controller.PIDController; + // For CAN import com.revrobotics.spark.SparkMax; @@ -24,7 +26,7 @@ public class ShooterSubsystem extends SubsystemBase { private SparkMax Shooter1Motor = new SparkMax(Constants.Subsystems.Shooter.kShooter1Port, MotorType.kBrushless); private PWMVictorSPX Shooter2Motor = new PWMVictorSPX(Constants.Subsystems.Shooter.kShooter2Port); private final RelativeEncoder ShooterEncoder = Shooter1Motor.getEncoder(); - // private final PIDController pid = new PIDController(0, 0.0, 0.0); + PIDController shooterPID = new PIDController(0.00027, 0.00017, 0.000017); public ShooterSubsystem() { @@ -69,32 +71,37 @@ public void periodic() { public Command StartShoot() { return this.run(() -> { + double shooter = shooterPID.calculate(ShooterEncoder.getVelocity(), 5000); + - - Shooter1Motor.set(Constants.Subsystems.Shooter.kMaxShooterSpeedOut1); - if (ShooterEncoder.getVelocity() > 2000){ + Shooter1Motor.set(shooter); + if (ShooterEncoder.getVelocity() > 4000){ Shooter2Motor.set(Constants.Subsystems.Shooter.kMaxShooterSpeedOut2); }}); } + public void StartShootVoid(){ + + double shooter = shooterPID.calculate(ShooterEncoder.getVelocity(), 5000); + Shooter1Motor.set(shooter); + if (ShooterEncoder.getVelocity() > 2000){ + Shooter2Motor.set(Constants.Subsystems.Shooter.kMaxShooterSpeedOut2); + } } + + + public Command StopShoot() { return this.run(() -> { - Shooter1Motor.set(0.0); - Shooter2Motor.set(0.0); - }); - } + - // Named command for PathPlanner - public Command shoot_1_Command() { - return this.run(() -> { - ShooterEncoder.setPosition(0.0); // Reset encoder position to 0 - if (ShooterEncoder.getPosition() < 42) { - StartShoot(); - } else { - StopShoot(); - } - }); + Shooter1Motor.set(0); + + Shooter2Motor.set(0); + + }); } + + }