diff --git a/clear_pathplanner_folder.bat b/clear_pathplanner_folder.bat new file mode 100644 index 00000000..2820e824 --- /dev/null +++ b/clear_pathplanner_folder.bat @@ -0,0 +1,11 @@ +@echo off + +set user="admin" +set hostname="roboRIO-8248-frc.local" +set autos_path="/home/lvuser/deploy/pathplanner" + +echo Connecting to %user%@%hostname% +echo -------------------------------------- +ssh %user%@%hostname% "rm -rf %autos_path%;echo %autos_path% has been cleared" +echo -------------------------------------- +pause \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/leave 1.path b/src/main/deploy/pathplanner/paths/leave 1.path index 2d636bf7..5dbe6be7 100644 --- a/src/main/deploy/pathplanner/paths/leave 1.path +++ b/src/main/deploy/pathplanner/paths/leave 1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.449326898653798, - "y": 7.0 + "x": 3.541181596252082, + "y": 7.124496534707355 }, "prevControl": { - "x": 5.499618999237998, - "y": 7.0 + "x": 1.5914736968362821, + "y": 7.124496534707355 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b68462c8..d83885dc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,6 +5,8 @@ package frc.robot; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -16,11 +18,13 @@ public class Robot extends TimedRobot { @Override public void robotInit() { m_robotContainer = new RobotContainer(); + SmartDashboard.putNumber("Match Time Left", 0); } @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + SmartDashboard.putNumber("Match Time Left", Timer.getMatchTime()); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ecc72408..588ed041 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,19 +8,25 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.BasicDriveCommand; +import frc.robot.commands.climber.Climb; +import frc.robot.commands.climber.IndividualClimb; import frc.robot.commands.intake.RunIntake; -import frc.robot.commands.shooter.ActuateShield; -import frc.robot.commands.shooter.Aim; +import frc.robot.commands.shooter.PivotMove; import frc.robot.commands.shooter.Shoot; +import frc.robot.commands.shooter.SpinFlywheels; +import frc.robot.commands.shooter.StowShooter; import frc.robot.constants.RobotConfig; import frc.robot.constants.RobotConfig.FieldElement; import frc.robot.constants.RobotConstants.Bindings; import frc.robot.constants.RobotConstants.DriveConstants.OIConstants; +import frc.robot.subsystems.climber.Climber; import frc.robot.subsystems.drive.Drivetrain; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; @@ -33,6 +39,7 @@ public class RobotContainer { private POVButton m_autoAim; private POVButton m_trapAim; + private Climber m_climber; private Shooter m_shooter; private Intake m_intake; private Drivetrain m_robotDrive; @@ -46,6 +53,7 @@ public class RobotContainer { private Vector rightInputVec; public RobotContainer() { + m_climber = new Climber(); m_shooter = new Shooter(); m_intake = new Intake(); m_robotDrive = new Drivetrain(); @@ -62,8 +70,13 @@ public RobotContainer() { autoChooser = AutoBuilder.buildAutoChooser(); configureBindings(); - /*m_shooter.setDefaultCommand( - new RunCommand(() -> m_shooter.runFlywheel(ShooterConfig.kDefaultFlywheelRPM), m_shooter));*/ + autoChooser.setDefaultOption("shoot and leave ", + new SequentialCommandGroup( + NamedCommands.getCommand("shootSpeaker"), + new RunCommand(() -> m_robotDrive.drive(new Vector(-0.2, 0), new Vector(), false, false), m_robotDrive).withTimeout(3))); + + autoChooser.addOption("Leave Top", AutoBuilder.buildAuto("LeaveFromTop")); + SmartDashboard.putData("Auto Chooser", autoChooser); } private void configureBindings() { @@ -90,30 +103,53 @@ private void configureBindings() { .whileTrue(new BasicDriveCommand(m_robotDrive, m_driverController)); // RunIntake constructor boolean is whether or not the intake should run reversed. - new Trigger(this::getIntakeButton).whileTrue(new RunIntake(m_intake, false)); - new Trigger(this::getReverseIntakeButton).whileTrue(new RunIntake(m_intake, true)); + new Trigger(this::getIntakeButton).whileTrue(new RunIntake(m_intake, m_indexer, false)); + new Trigger(this::getReverseIntakeButton).whileTrue(new RunIntake(m_intake, m_indexer, true)); // just shoot on trigger new Trigger(() -> m_operatorController.getRawButton(Bindings.kShoot)) .whileTrue(new Shoot(m_indexer, false)); new Trigger(() -> m_operatorController.getRawButton(Bindings.kShootReverse)) .whileTrue(new Shoot(m_indexer, true)); - new Trigger(() -> m_operatorController.getRawButton(Bindings.kAimAmp)) - .whileTrue(new Aim(m_shooter, FieldElement.AMP)); - new Trigger(() -> m_operatorController.getRawButton(Bindings.kAimSpeaker)) - .whileTrue(new Aim(m_shooter, FieldElement.SPEAKER)); - m_trapAim.whileTrue(new Aim(m_shooter, FieldElement.TRAP)); + new Trigger(() -> m_operatorController.getRawButton(Bindings.kFlywheelAmp)) + .whileTrue( + new SequentialCommandGroup( + new SpinFlywheels(m_shooter, FieldElement.AMP).withTimeout(1.5), + new ParallelCommandGroup( + new Shoot(m_indexer, false), new SpinFlywheels(m_shooter, FieldElement.AMP)))); + + new Trigger(() -> m_operatorController.getRawButton(Bindings.kFlywheelSpeaker)) + .whileTrue( + new SequentialCommandGroup( + new SpinFlywheels(m_shooter, FieldElement.SPEAKER).withTimeout(1.5), + new ParallelCommandGroup( + new Shoot(m_indexer, false), + new SpinFlywheels(m_shooter, FieldElement.SPEAKER)))); + + m_trapAim.whileTrue(new SpinFlywheels(m_shooter, FieldElement.TRAP)); + + new Trigger(() -> m_operatorController.getRawButton(Bindings.kRightClimberUp)) + .whileTrue(new IndividualClimb(m_climber, true, true)); + new Trigger(() -> m_operatorController.getRawButton(Bindings.kRightClimberDown)) + .whileTrue(new IndividualClimb(m_climber, true, false)); + new Trigger(() -> m_operatorController.getRawButton(Bindings.kLeftClimberUp)) + .whileTrue(new IndividualClimb(m_climber, false, true)); + new Trigger(() -> m_operatorController.getRawButton(Bindings.kLeftClimberDown)) + .whileTrue(new IndividualClimb(m_climber, false, false)); + + new Trigger(() -> m_operatorController.getRawButton(Bindings.kBothClimbersUp)) + .whileTrue(new Climb(m_climber, true)); + new Trigger(() -> m_operatorController.getRawButton(Bindings.kBothClimbersDown)) + .whileTrue(new Climb(m_climber, false)); + + new Trigger(() -> m_operatorController.getRawButton(Bindings.kStowShooter)) + .whileTrue(new StowShooter(m_shooter)); - // triggers for extending and retracting shield manually - // don't extend shield - new Trigger(() -> m_operatorController.getRawButton(Bindings.kExtendShield)) - .onTrue(new ActuateShield(m_shooter, false)); - // extend shield - new Trigger(() -> m_operatorController.getRawButton(Bindings.kRetractShield)) - .onTrue(new ActuateShield(m_shooter, true)); + new Trigger(() -> m_operatorController.getRawButton(Bindings.kAimSpeaker)) + .whileTrue(new PivotMove(m_shooter, 0.3)); - autoChooser.setDefaultOption("Leave Top", AutoBuilder.buildAuto("LeaveFromTop")); - SmartDashboard.putData("Auto Chooser", autoChooser); + new Trigger(() -> m_operatorController.getRawButton(Bindings.kAimAmp)) + .whileTrue(new PivotMove(m_shooter, 0.69)); } private void updateInput() { @@ -129,13 +165,27 @@ private void updateInput() { // TODO: fill in placeholder commands with actual functionality private void registerCommands() { - NamedCommands.registerCommand("intakeFromFloor", new RunIntake(m_intake, false)); - NamedCommands.registerCommand("scoreAmp", doNothing()); - NamedCommands.registerCommand("aimAndScoreSpeaker", doNothing()); - } - - private Command doNothing() { - return Commands.none(); + // timeout doesn't need to be set because it is in a race group with the intake path in the + // .path file + NamedCommands.registerCommand("intakeFromFloor", new RunIntake(m_intake, m_indexer, false)); + + NamedCommands.registerCommand( + "shootSpeaker", + new SequentialCommandGroup( + new PivotMove(m_shooter, 0.3).withTimeout(1), + new SpinFlywheels(m_shooter, FieldElement.SPEAKER).withTimeout(1.5), + new ParallelRaceGroup( + new SpinFlywheels(m_shooter, FieldElement.SPEAKER), new Shoot(m_indexer, false)) + .withTimeout(3))); + + NamedCommands.registerCommand( + "shootAmp", + new SequentialCommandGroup( + new PivotMove(m_shooter, 0.69).withTimeout(1), + new SpinFlywheels(m_shooter, FieldElement.AMP).withTimeout(1.5), + new ParallelRaceGroup( + new SpinFlywheels(m_shooter, FieldElement.AMP), new Shoot(m_indexer, false)) + .withTimeout(3))); } /** @@ -144,7 +194,7 @@ private Command doNothing() { * @see RobotConfig.IntakeConfig.Bindings.kIntakeNote */ public boolean getIntakeButton() { - return m_operatorController.getRawButton(RobotConfig.IntakeConfig.Bindings.kIntakeNoteButtonID); + return m_operatorController.getRawButton(Bindings.kIntakeNoteButtonID); } /** @@ -153,8 +203,7 @@ public boolean getIntakeButton() { * @see RobotConfig.IntakeConfig.Bindings.kReverseIntakeButtonID */ public boolean getReverseIntakeButton() { - return m_operatorController.getRawButton( - RobotConfig.IntakeConfig.Bindings.kReverseIntakeButtonID); + return m_operatorController.getRawButton(Bindings.kReverseIntakeButtonID); } public boolean triggerPressed() { diff --git a/src/main/java/frc/robot/commands/VisionTranslateCommand.java b/src/main/java/frc/robot/commands/VisionTranslateCommand.java deleted file mode 100644 index 5ad3c56c..00000000 --- a/src/main/java/frc/robot/commands/VisionTranslateCommand.java +++ /dev/null @@ -1,67 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.RobotConfig.DriveConfig.TranslateConfig; -import frc.robot.constants.RobotConstants.DriveConstants; -import frc.robot.subsystems.drive.Drivetrain; -import frc.robot.subsystems.vision.Vision; -import frc.utils.Vector; - -public class VisionTranslateCommand extends Command { - - private Vision vision; - private Drivetrain drive; - private XboxController controller; - - private PIDController forwardController; - - public VisionTranslateCommand(Vision vision, Drivetrain drive, XboxController controller) { - this.vision = vision; - this.drive = drive; - - this.controller = controller; - SmartDashboard.putNumber(TranslateConfig.kPKey, TranslateConfig.kP); - SmartDashboard.putNumber(TranslateConfig.kIKey, TranslateConfig.kI); - SmartDashboard.putNumber(TranslateConfig.kDKey, TranslateConfig.kD); - forwardController = - new PIDController(TranslateConfig.kP, TranslateConfig.kI, TranslateConfig.kD); - - addRequirements(vision, drive); - - forwardController.setIntegratorRange(TranslateConfig.minIntegral, TranslateConfig.maxIntegral); - } - - @Override - public void execute() { - double forwardSpeed = 0.0; - - if (vision.getHasTarget()) { - double range = vision.getDistToTarget(); - - forwardSpeed = forwardController.calculate(range, 0); - } - - drive.drive( - new Vector( - MathUtil.applyDeadband(controller.getLeftX(), DriveConstants.kDriveDeadband), - MathUtil.applyDeadband(controller.getRightX(), DriveConstants.kDriveDeadband)), - new Vector(MathUtil.applyDeadband(forwardSpeed, DriveConstants.kDriveDeadband), 0), - false, - false); - } - - @Override - public boolean isFinished() { - forwardController.setTolerance(TranslateConfig.kTolerance); - return forwardController.atSetpoint(); - } - - @Override - public void end(boolean interrupted) { - drive.drive(new Vector(0, 0), new Vector(0, 0), false, false); - } -} diff --git a/src/main/java/frc/robot/commands/VisionTurnCommand.java b/src/main/java/frc/robot/commands/VisionTurnCommand.java deleted file mode 100644 index 08a65cf8..00000000 --- a/src/main/java/frc/robot/commands/VisionTurnCommand.java +++ /dev/null @@ -1,61 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.RobotConfig.DriveConfig.TurnConfig; -import frc.robot.constants.RobotConstants.DriveConstants; -import frc.robot.subsystems.drive.Drivetrain; -import frc.robot.subsystems.vision.Vision; -import frc.utils.Vector; - -public class VisionTurnCommand extends Command { - - private Vision vision; - private Drivetrain drive; - private XboxController controller; - - private PIDController turnController; - - public VisionTurnCommand(Vision vision, Drivetrain drive, XboxController controller) { - this.vision = vision; - this.drive = drive; - this.controller = controller; - - addRequirements(vision, drive); - - turnController = new PIDController(TurnConfig.kP, TurnConfig.kI, TurnConfig.kD); - - // set a limit on overshoot compensation - turnController.setIntegratorRange( - TurnConfig.minIntegral, Math.toRadians(TurnConfig.maxIntegral)); - } - - @Override - public void execute() { - double rotationSpeed = 0.0; - - if (vision.getHasTarget()) { - rotationSpeed = turnController.calculate(vision.getBestTarget().getYaw(), 0); - } - - drive.drive( - new Vector( - MathUtil.applyDeadband(controller.getLeftX(), DriveConstants.kDriveDeadband), - MathUtil.applyDeadband(controller.getLeftY(), DriveConstants.kDriveDeadband)), - new Vector(MathUtil.applyDeadband(rotationSpeed, DriveConstants.kDriveDeadband), 0), - false, - false); - } - - public boolean isFinished() { - turnController.setTolerance(TurnConfig.kTolerance); - return turnController.atSetpoint(); - } - - @Override - public void end(boolean interrupted) { - drive.drive(new Vector(0, 0), new Vector(0, 0), false, false); - } -} diff --git a/src/main/java/frc/robot/commands/climber/Climb.java b/src/main/java/frc/robot/commands/climber/Climb.java new file mode 100644 index 00000000..01c333f5 --- /dev/null +++ b/src/main/java/frc/robot/commands/climber/Climb.java @@ -0,0 +1,34 @@ +package frc.robot.commands.climber; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.RobotConfig.ClimberConfig; +import frc.robot.subsystems.climber.Climber; + +public class Climb extends Command { + private final Climber m_climber; + private boolean m_reverse; + + public Climb(Climber climber, boolean reverse) { + m_climber = climber; + m_reverse = reverse; + + addRequirements(climber); + } + + @Override + public void initialize() { + m_climber.setBoth(m_reverse); + } + + @Override + public void end(boolean interrupted) { + m_climber.stopRight(); + m_climber.stopLeft(); + } + + @Override + public boolean isFinished() { + return m_climber.getLeftEncoderPosition() + > ClimberConfig.kUpperRotSoftStop - ClimberConfig.kStopMargin; + } +} diff --git a/src/main/java/frc/robot/commands/climber/IndividualClimb.java b/src/main/java/frc/robot/commands/climber/IndividualClimb.java new file mode 100644 index 00000000..c250d6c0 --- /dev/null +++ b/src/main/java/frc/robot/commands/climber/IndividualClimb.java @@ -0,0 +1,36 @@ +package frc.robot.commands.climber; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.climber.Climber; + +public class IndividualClimb extends Command { + private Climber m_climber; + private boolean m_isRight; + private boolean m_reverse; + + public IndividualClimb(Climber climber, boolean isRight, boolean reverse) { + m_climber = climber; + m_isRight = isRight; + m_reverse = reverse; + + addRequirements(m_climber); + } + + @Override + public void initialize() { + if (m_isRight) { + m_climber.setLeft(m_reverse); + } else { + m_climber.setRight(m_reverse); + } + } + + @Override + public void end(boolean interrupted) { + if (m_isRight) { + m_climber.stopLeft(); + } else { + m_climber.stopRight(); + } + } +} diff --git a/src/main/java/frc/robot/commands/intake/RunIntake.java b/src/main/java/frc/robot/commands/intake/RunIntake.java index eca55966..c75ce0d7 100644 --- a/src/main/java/frc/robot/commands/intake/RunIntake.java +++ b/src/main/java/frc/robot/commands/intake/RunIntake.java @@ -2,43 +2,35 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.RobotConfig; +import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; public class RunIntake extends Command { private final Intake m_intake; + private final Indexer m_indexer; private boolean m_reversed; - /** - * Creates a new RunIntake command, which runs the roller motor on the intake subsystem to intake - * a note - * - * @param intake The subsystem used by this command. - */ - public RunIntake(Intake intake, boolean reversed) { + public RunIntake(Intake intake, Indexer indexer, boolean reversed) { m_intake = intake; + m_indexer = indexer; m_reversed = reversed; addRequirements(intake); } - // Called when the command is initially scheduled. @Override public void initialize() { int multiplier = m_reversed ? -1 : 1; m_intake.run(RobotConfig.IntakeConfig.kDefaultSpeed * multiplier); + m_indexer.startFeedNote(m_reversed); } - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { m_intake.stop(); + m_indexer.stopFeedNote(); } - // Returns true when the command should end. @Override public boolean isFinished() { return false; diff --git a/src/main/java/frc/robot/commands/shooter/Aim.java b/src/main/java/frc/robot/commands/shooter/Aim.java deleted file mode 100644 index 330d72bb..00000000 --- a/src/main/java/frc/robot/commands/shooter/Aim.java +++ /dev/null @@ -1,88 +0,0 @@ -package frc.robot.commands.shooter; - -import edu.wpi.first.units.Angle; -import edu.wpi.first.units.Distance; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.Units; -import edu.wpi.first.units.Velocity; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.RobotConfig.FieldElement; -import frc.robot.constants.RobotConfig.ShooterConfig; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.vision.Vision; - -public class Aim extends Command { - private final Shooter m_shooter; - private final Vision m_vision; - private final FieldElement m_type; - private Measure desiredAngle; - private double desiredVelocity; - - public Aim(Shooter shooter, FieldElement type) { - m_shooter = shooter; - m_vision = new Vision(); - m_type = type; - - addRequirements(m_shooter); - } - - public Aim(Shooter shooter, Vision eyes) { - m_shooter = shooter; - m_vision = eyes; - m_type = null; - addRequirements(m_shooter, m_vision); - } - - @Override - public void initialize() { - if (m_type == null) { - if (m_vision.getHasTarget()) { - double desiredAngle = - Units.Degrees.of(m_vision.getBestTarget().getPitch()).in(Units.Radians); - Measure> desiredVelocity = - m_shooter.calculateVelocity( - m_vision.getDistToTarget() * Math.atan(desiredAngle), - Units.Radians.of(desiredAngle)); - - m_shooter.runFlywheel(m_shooter.convertToRPM(desiredVelocity.magnitude())); - } - } else { - switch (m_type) { - case AMP: - desiredAngle = ShooterConfig.kAmpAngle; - desiredVelocity = ShooterConfig.ampVelocity; - break; - case SPEAKER: - desiredAngle = ShooterConfig.kSpeakerAngle; - desiredVelocity = ShooterConfig.speakerVelocity; - // getVelocity(ShooterConfig.SpeakerHeight); - System.out.println("shooter vel: " + desiredVelocity); - break; - case TRAP: - desiredAngle = ShooterConfig.kTrapAngle; - desiredVelocity = ShooterConfig.trapVelocity; - break; - default: - desiredVelocity = 0; - desiredAngle = Units.Degrees.of(0); - break; - } - m_shooter.runFlywheel(desiredVelocity); - } - } - - public double getVelocity(double elementHeight) { - return m_shooter.convertToRPM( - m_shooter.calculateVelocity(elementHeight, desiredAngle).magnitude()); - } - - @Override - public void end(boolean interrupted) { - m_shooter.stopFlywheel(); - } - - @Override - public boolean isFinished() { - return m_shooter.isAtFlywheelSetpoint(desiredVelocity); - } -} diff --git a/src/main/java/frc/robot/commands/shooter/ManualAdjust.java b/src/main/java/frc/robot/commands/shooter/ManualAdjust.java new file mode 100644 index 00000000..7e720abe --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/ManualAdjust.java @@ -0,0 +1,56 @@ +package frc.robot.commands.shooter; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Measure; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.RobotConfig.*; +import frc.robot.subsystems.shooter.Shooter; + +public class ManualAdjust extends Command { + private final Shooter m_shooter; + private final AdjustType m_type; + private Measure desiredAngle; + private Timer timer; + + public ManualAdjust(Shooter shooter, AdjustType type) { + m_shooter = shooter; + m_type = type; + timer = new Timer(); + addRequirements(m_shooter); + } + + @Override + public void initialize() { + timer.start(); + } + + @Override + public void execute() { + switch (m_type) { + case up: + desiredAngle = m_shooter.getCurrentAngle().plus(ShooterConfig.kAdjustAmountDegrees); + break; + case down: + desiredAngle = m_shooter.getCurrentAngle().minus(ShooterConfig.kAdjustAmountDegrees); + break; + default: + desiredAngle = m_shooter.getCurrentAngle(); + break; + } + + if (timer.get() % 10 == 0) { + m_shooter.setAngle(desiredAngle); + } + + m_shooter.setFF( + Math.cos(Units.rotationsToRadians(m_shooter.getCurrentAngle().magnitude())) + * ShooterConfig.kAngleControlFF); + } + + @Override + public boolean isFinished() { + return m_shooter.isAtAngleSetpoint(desiredAngle.magnitude()); + } +} diff --git a/src/main/java/frc/robot/commands/shooter/PivotMove.java b/src/main/java/frc/robot/commands/shooter/PivotMove.java new file mode 100644 index 00000000..6af82fbf --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/PivotMove.java @@ -0,0 +1,39 @@ +package frc.robot.commands.shooter; + +import edu.wpi.first.units.MutableMeasure; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.RobotConfig.ShooterConfig; +import frc.robot.subsystems.shooter.Shooter; + +public class PivotMove extends Command { + private Shooter m_shooter; + private double m_multiplier; + private double startTime; + + public PivotMove(Shooter shooter, double multiplier) { + m_shooter = shooter; + m_multiplier = multiplier; + + addRequirements(m_shooter); + } + + @Override + public void initialize() { + startTime = Timer.getFPGATimestamp(); + m_shooter.setAngle(MutableMeasure.ofBaseUnits(160 * m_multiplier, Units.Rotations)); + } + + @Override + public boolean isFinished() { + return (Timer.getFPGATimestamp() - startTime) > 1; + } + + @Override + public void execute() { + double ff = + Math.cos(m_shooter.getCurrentAngle().in(Units.Radians)) * ShooterConfig.kAngleControlFF; + m_shooter.setFF(ff); + } +} diff --git a/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java b/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java new file mode 100644 index 00000000..3154a05c --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/SpinFlywheels.java @@ -0,0 +1,74 @@ +package frc.robot.commands.shooter; + +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.RobotConfig.FieldElement; +import frc.robot.constants.RobotConfig.ShooterConfig; +import frc.robot.subsystems.shooter.Shooter; + +public class SpinFlywheels extends Command { + private final Shooter m_shooter; + private final FieldElement m_type; + private double desiredVelocity; + private Measure desiredAngle; + + public SpinFlywheels(Shooter shooter, FieldElement type) { + m_shooter = shooter; + m_type = type; + + addRequirements(m_shooter); + } + + public SpinFlywheels(Shooter shooter) { + m_shooter = shooter; + m_type = null; + addRequirements(m_shooter); + } + + @Override + public void initialize() { + + switch (m_type) { + case AMP: + desiredAngle = ShooterConfig.kAmpAngle; + desiredVelocity = ShooterConfig.kDefaultAmpVelocity; + break; + case SPEAKER: + desiredAngle = ShooterConfig.kSpeakerAngle; + desiredVelocity = ShooterConfig.kDefaultSpeakerVelocity; + break; + case TRAP: + desiredAngle = ShooterConfig.kTrapAngle; + desiredVelocity = ShooterConfig.kDefaultTrapVelocity; + break; + default: + desiredVelocity = 0; + desiredAngle = Units.Degrees.of(0); + break; + } + m_shooter.runFlywheel(desiredVelocity); + } + + @Override + public void execute() { + double ff = + Math.cos(m_shooter.getCurrentAngle().in(Units.Radians)) * ShooterConfig.kAngleControlFF; + m_shooter.setFF(ff); + } + + public boolean isFinished() { + return m_shooter.isAtFlywheelSetpoint(desiredVelocity); + } + + public double getVelocity(double elementHeight) { + return m_shooter.convertToRPM( + m_shooter.calculateVelocity(elementHeight, desiredAngle).magnitude()); + } + + @Override + public void end(boolean interrupted) { + m_shooter.stopFlywheel(); + } +} diff --git a/src/main/java/frc/robot/commands/shooter/StowShooter.java b/src/main/java/frc/robot/commands/shooter/StowShooter.java new file mode 100644 index 00000000..0a0bfb1e --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/StowShooter.java @@ -0,0 +1,36 @@ +package frc.robot.commands.shooter; + +import edu.wpi.first.units.Units; +// done +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.RobotConfig.ShooterConfig; +import frc.robot.subsystems.shooter.Shooter; + +public class StowShooter extends Command { + private final Shooter m_shooter; + + public StowShooter(Shooter shooter) { + m_shooter = shooter; + + addRequirements(m_shooter); + } + + @Override + public void initialize() { + m_shooter.setAngle(Units.Degrees.of(ShooterConfig.kShooterStowAngle)); + } + + @Override + public void execute() { + m_shooter.setFF( + Math.cos(m_shooter.getCurrentAngle().in(Units.Radians)) * ShooterConfig.kAngleControlFF); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return m_shooter.isAtAngleSetpoint(ShooterConfig.kShooterStowAngle); + } +} diff --git a/src/main/java/frc/robot/constants/RobotConfig.java b/src/main/java/frc/robot/constants/RobotConfig.java index 5455a60d..96f26a9a 100644 --- a/src/main/java/frc/robot/constants/RobotConfig.java +++ b/src/main/java/frc/robot/constants/RobotConfig.java @@ -4,6 +4,7 @@ import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Units; import edu.wpi.first.units.Velocity; @@ -15,6 +16,7 @@ * RobotConstants" */ public class RobotConfig { + public enum AdjustType { up, down @@ -26,20 +28,30 @@ public enum FieldElement { TRAP } + public static final class ClimberConfig { + public static final double kDefaultSpeed = 0.4; + public static final double kStallInput = 0.02; + public static final double kUpperRotSoftStop = 5000; + public static final double kStopMargin = 10; + public static final boolean kInverted = true; + public static final Measure buddyClimbExtensionDiff = + Units.Meters.of(Units.Inches.of(5).in(Units.Meters)); + } + public static final class ShooterConfig { // Angle controller PID coefficients - public static final double kAngleControlP = 0; + public static final double kAngleControlP = 0.2; public static final double kAngleControlI = 0; public static final double kAngleControlD = 0; - public static final double kAngleControlFF = 0; - public static final double kAngleControlIZone = 0; - public static final double kAngleControlMinOutput = 0; - public static final double kAngleControlMaxOutput = 0; + public static final double kAngleControlFF = 0.2; + public static final double kAngleControlIZone = 0.0001; + public static final double kAngleControlMinOutput = -1; + public static final double kAngleControlMaxOutput = 1; // top Flywheel controller PID coefficients public static final double kTopFlywheelP = 0.2; public static final double kTopFlywheelI = 0; - public static final double kTopFlywheelD = 0.001; + public static final double kTopFlywheelD = 0.005; public static final double kTopFlywheelFF = 0; public static final double kTopFlywheelIZone = 0.0001; public static final double kTopFlywheelMinOutput = -1; @@ -88,18 +100,25 @@ public static final class ShooterConfig { public static final long kReleaseTime = 5000; public static final long kShieldTime = 2; // seconds + public static final double kAimTimeout = 20; + public static final double kShieldDefaultSpeed = 0.5; + public static final double kEncoderRotsToPivotRot = 160; public static final Measure> kFlywheelError = Units.RPM.of(1); - public static final Measure kAngleError = Units.Radians.of(0.5 * Math.PI / 180); - public static final Measure kSpeakerAngle = Units.Radians.of(75 * Math.PI / 180); - public static final Measure kAmpAngle = Units.Radians.of(109 * Math.PI / 180); - public static final Measure kTrapAngle = Units.Radians.of(105 * Math.PI / 180); - public static final Measure kAdjustAmountDegrees = Units.Radians.of(0.5 * Math.PI / 180); - - // TODO placeholders - public static final double ampVelocity = 1500; // rpm - public static final double trapVelocity = 2000; // rpm - public static final double speakerVelocity = 2500; + public static final Measure kAngleError = + Units.Rotations.of(0.5 / 360 * kEncoderRotsToPivotRot); + public static final Measure kSpeakerAngle = + Units.Rotations.of(75 / 360 * kEncoderRotsToPivotRot); + public static final Measure kAmpAngle = + Units.Rotations.of(109 / 360 * kEncoderRotsToPivotRot); + public static final Measure kTrapAngle = + Units.Rotations.of(105 / 360 * kEncoderRotsToPivotRot); + public static final Measure kAdjustAmountDegrees = + Units.Rotations.of(0.5 / 360 * kEncoderRotsToPivotRot); + + public static final double kDefaultAmpVelocity = 500; // rpm + public static final double kDefaultTrapVelocity = 2000; // rpm + public static final double kDefaultSpeakerVelocity = 4500; // rpm } public static class DriveConfig { @@ -145,7 +164,7 @@ public static class TurnConfig { new ReplanningConfig()); // 4.45 m/s max speed - public static final double kMaxSpeedBase = 4.8; + public static final double kMaxSpeedBase = 9; public static final double kMaxSpeedScaleFactor = 0.9; public static final double kMaxSpeedMetersPerSecond = kMaxSpeedBase * kMaxSpeedScaleFactor; @@ -168,10 +187,5 @@ public static class TurnConfig { public static final class IntakeConfig { // In percentage output public static final double kDefaultSpeed = 1; - - public static final class Bindings { - public static final int kIntakeNoteButtonID = 2; - public static final int kReverseIntakeButtonID = 8; - } } } diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index abfd8dcd..49392f1f 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -19,17 +19,23 @@ public final class RobotConstants { public final class Bindings { - public static final int kAimAmp = 4; - public static final int kAimSpeaker = 3; + public static final int kFlywheelAmp = 4; + public static final int kFlywheelSpeaker = 3; public static final int kShoot = 1; public static final int kShootReverse = 7; - public static final int kAimTrap = 2; - public static final int kStowShooter = 14; - public static final int kToggleFlywheel = 5; - public static final int kRetractShield = 10; - public static final int kExtendShield = 9; - public static final int kManualAdjustDown = 18; - public static final int kManualAdjustUp = 19; + public static final int kIntakeNoteButtonID = 2; + public static final int kReverseIntakeButtonID = 6; + + public static final int kStowShooter = 8; + public static final int kAimAmp = 9; + public static final int kAimSpeaker = 10; + + public static final int kLeftClimberUp = 11; + public static final int kLeftClimberDown = 12; + public static final int kRightClimberUp = 13; + public static final int kRightClimberDown = 14; + public static final int kBothClimbersUp = 15; + public static final int kBothClimbersDown = 16; } public static final class VisionConstants { @@ -50,13 +56,35 @@ public static final class NeoMotorConstants { public static final double kFreeSpeedRpm = 5676; } + public static final class OIConstants { + public static final int kDriverControllerPort = 0; + public static final int kOperatorJoystickPort = 1; + } + + public static final class ClimberConstants { + public static final int kClimberLeaderID = 11; + public static final int kClimberFollowerID = 12; + + public static final double kClimberP = 0.1; + public static final double kClimberI = 0; + public static final double kClimberD = 0; + public static final double kClimberMotorRadius = 0.003175; + public static final double kClimberIZone = 0.001; + public static final double kClimberFeedForward = 0; + public static final double kClimberMaxOutput = 1; + public static final double kClimberMinOutput = -1; + } + public final class ShooterConstants { public static final int kRollerMotorLeftId = 15; public static final int kTopFlywheelMotorId = 16; public static final int kBottomFlywheelMotorId = 17; public static final int kShieldMotorId = 18; - public static final double FlywheelDiameter = 0.0762; - public static final double ShooterLength = 0.4064; + public static final int kAngleMotorLeaderId = 13; + public static final int kAngleMotorFollowerId = 14; + public static final int kLineBreakPort = 6; + public static final double FlywheelDiameter = 0.0762; // meters + public static final double ShooterLength = 0.4064; // meters public static final double Gravity = 9.81; public static final Measure kShieldExtentionAngle = Units.Rotations.of(1); // TODO - Set to number of rotations to fully extend shield @@ -172,9 +200,6 @@ public static final class SwerveModuleConstants { } public static final class IntakeConstants { - public static final int kLineBreakSensor = 0; - - // Roller motor ID public static final int kMotorID = 10; } } diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 42b842e0..ff38788e 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -1,18 +1,94 @@ package frc.robot.subsystems.climber; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.RobotConfig.ClimberConfig; +import frc.robot.constants.RobotConstants.ClimberConstants; public class Climber extends SubsystemBase { - /** Creates a new ExampleSubsystem. */ - public Climber() {} + + private CANSparkMax leftController; + private CANSparkMax rightController; + private int multiplier; + private DigitalInput m_limSwitchLeft; + private DigitalInput m_limSwitchRight; + + public Climber() { + leftController = new CANSparkMax(ClimberConstants.kClimberLeaderID, MotorType.kBrushless); + rightController = new CANSparkMax(ClimberConstants.kClimberFollowerID, MotorType.kBrushless); + + leftController.setIdleMode(IdleMode.kBrake); + rightController.setIdleMode(IdleMode.kBrake); + + m_limSwitchLeft = new DigitalInput(9); + m_limSwitchRight = new DigitalInput(8); + + multiplier = 1; + + leftController.getEncoder().setPosition(0); + rightController.getEncoder().setPosition(0); + + SmartDashboard.putNumber("climber encoder rots", leftController.getEncoder().getPosition()); + } @Override public void periodic() { - // This method will be called once per scheduler run + SmartDashboard.putBoolean("left lim switch", m_limSwitchLeft.get()); + SmartDashboard.putBoolean("right lim switch", m_limSwitchRight.get()); + + SmartDashboard.putNumber("climber encoder rots", leftController.getEncoder().getPosition()); + if (leftController.getEncoder().getPosition() < 0 + || leftController.getEncoder().getPosition() > ClimberConfig.kUpperRotSoftStop) { + // leftController.set(0); + // rightController.set(0); + } + + /*if (!m_limSwitchLeft.get()) { + leftController.set(0); + } + + if (!m_limSwitchRight.get()) { + rightController.set(0); + }*/ } - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation + public double getLeftEncoderPosition() { + return leftController.getEncoder().getPosition(); + } + + public void setBoth(boolean reverse) { + multiplier = reverse ? -1 : 1; + leftController.set(0.9 * multiplier); + rightController.set(-0.9 * multiplier); + } + + public void setLeft(boolean reverse) { + multiplier = reverse ? -1 : 1; + leftController.set(0.9 * multiplier); + } + + public void stopLeft() { + leftController.set(0); + } + + public void stopRight() { + rightController.set(0); + } + + public void setRight(boolean reverse) { + multiplier = reverse ? -1 : 1; + rightController.set(0.9 * multiplier); + } + + public CANSparkMax getLeft() { + return leftController; + } + + public CANSparkMax getRight() { + return rightController; } } diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 8b375dda..9c18c538 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -2,17 +2,26 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.RobotConfig; import frc.robot.constants.RobotConstants.ShooterConstants; public class Indexer extends SubsystemBase { private CANSparkMax m_shooterRollerMotor; + private DigitalInput m_lineBreakSensor; public Indexer() { // Roller m_shooterRollerMotor = new CANSparkMax(ShooterConstants.kRollerMotorLeftId, MotorType.kBrushless); + m_lineBreakSensor = new DigitalInput(ShooterConstants.kLineBreakPort); + } + + @Override + public void periodic() { + SmartDashboard.putBoolean("line break sensor", m_lineBreakSensor.get()); } // runs the rollers @@ -28,4 +37,8 @@ public void startFeedNote(boolean reverse) { public void stopFeedNote() { m_shooterRollerMotor.stopMotor(); } + + public boolean getLineBreak() { + return m_lineBreakSensor.get(); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index d662735a..ef90c3c4 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -2,26 +2,15 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.RobotConstants.IntakeConstants; public class Intake extends SubsystemBase { private final CANSparkMax m_intakeRollerMotor; // Intake roller motor - private final DigitalInput m_linebreak; /** Creates a new ExampleSubsystem. */ public Intake() { m_intakeRollerMotor = new CANSparkMax(IntakeConstants.kMotorID, MotorType.kBrushless); - // TODO maybe use to terminate intake command - m_linebreak = new DigitalInput(IntakeConstants.kLineBreakSensor); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - SmartDashboard.putBoolean("Intake/linebreak sensor", m_linebreak.get()); } /** diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 2f31f27a..542ab781 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,12 +1,13 @@ package frc.robot.subsystems.shooter; +import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; +import com.revrobotics.SparkPIDController.ArbFFUnits; import edu.wpi.first.units.*; -import edu.wpi.first.units.Measure; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -20,6 +21,12 @@ */ public class Shooter extends SubsystemBase { /** 1. create motor and pid controller objects */ + private CANSparkMax m_angleMotorLeader; + + private CANSparkMax m_angleMotorFollower; + private SparkPIDController m_anglePIDController; + private AbsoluteEncoder m_angleEncoder; + private CANSparkMax m_topFlywheelMotor; private CANSparkMax m_bottomFlywheelMotor; @@ -33,9 +40,10 @@ public class Shooter extends SubsystemBase { private MutableMeasure> m_shooterSpeed; private MutableMeasure m_shieldPosition; private MutableMeasure> m_targetVelocity; + private MutableMeasure m_shooterAngle; + private MutableMeasure m_targetAngle; public Shooter() { - // Flywheel m_topFlywheelMotor = new CANSparkMax(ShooterConstants.kTopFlywheelMotorId, MotorType.kBrushless); @@ -74,6 +82,34 @@ public Shooter() { m_targetVelocity = MutableMeasure.zero(Units.MetersPerSecond); + // Angle + m_angleMotorLeader = + new CANSparkMax(ShooterConstants.kAngleMotorLeaderId, MotorType.kBrushless); + m_angleMotorFollower = + new CANSparkMax(ShooterConstants.kAngleMotorFollowerId, MotorType.kBrushless); + // sets follower motor to run inversely to the leader + m_angleMotorFollower.follow(m_angleMotorLeader, true); + m_angleEncoder = m_angleMotorFollower.getAbsoluteEncoder(); + m_angleEncoder.setZeroOffset(28.6 / 360 * 160); + + m_anglePIDController = m_angleMotorLeader.getPIDController(); + m_anglePIDController.setP(RobotConfig.ShooterConfig.kAngleControlP); + m_anglePIDController.setI(RobotConfig.ShooterConfig.kAngleControlI); + m_anglePIDController.setD(RobotConfig.ShooterConfig.kAngleControlD); + m_anglePIDController.setFF(RobotConfig.ShooterConfig.kAngleControlFF); + m_anglePIDController.setIZone(RobotConfig.ShooterConfig.kAngleControlIZone); + m_anglePIDController.setOutputRange( + RobotConfig.ShooterConfig.kAngleControlMinOutput, + RobotConfig.ShooterConfig.kAngleControlMaxOutput); + + m_shooterAngle = MutableMeasure.zero(Units.Revolutions); + m_targetAngle = MutableMeasure.zero(Units.Rotations); + m_targetVelocity = MutableMeasure.zero(Units.MetersPerSecond); + m_shooterSpeed = MutableMeasure.zero(Units.RPM); + m_shieldPosition = MutableMeasure.zero(Units.Rotations); + + SmartDashboard.putNumber("angle pos", 0.1); + if (DriverStation.isTest()) { putAngleOnSmartDashboard(); } @@ -104,39 +140,42 @@ public void putAngleOnSmartDashboard() { @Override public void periodic() { - SmartDashboard.putNumber("shield rots", m_shieldController.getEncoder().getPosition()); - if (DriverStation.isTest()) { - testPeriodic(); - } - - double pval = SmartDashboard.getNumber("flywheel p", 0.1); - if (pval != m_topFlywheelPIDController.getP()) { - m_topFlywheelPIDController.setP(pval); - } - - double ival = SmartDashboard.getNumber("flywheel i", 0.0); - if (pval != m_topFlywheelPIDController.getI()) { - m_topFlywheelPIDController.setP(ival); - } - - double dval = SmartDashboard.getNumber("flywheel d", 0.0); - if (pval != m_topFlywheelPIDController.getD()) { - m_topFlywheelPIDController.setP(dval); - } - - SmartDashboard.putNumber("Shooter/top flywheel output", m_topFlywheelMotor.getAppliedOutput()); - SmartDashboard.putNumber( - "Shooter/bottom flywheel output", m_bottomFlywheelMotor.getAppliedOutput()); double flywheelRPM = SmartDashboard.getNumber("Shooter/Flywheel RPM", m_topFlywheelEncoder.getVelocity()); - SmartDashboard.putNumber("Shooter/Flywheel RPM", flywheelRPM); + SmartDashboard.putNumber("Shooter/Flywheel RPM", m_topFlywheelEncoder.getVelocity()); if (m_topFlywheelEncoder.getVelocity() != flywheelRPM) { flywheelRPM = m_topFlywheelEncoder.getVelocity(); } + + SmartDashboard.putNumber( + "Shooter/angle error", m_targetAngle.magnitude() - m_angleEncoder.getPosition()); } - void testPeriodic() {} + // sets the target angle the shooter should be at, called only once + public void setAngle(Measure targetAngle) { + m_targetAngle.mut_replace(targetAngle); + m_anglePIDController.setReference( + targetAngle.in(Units.Rotations), CANSparkBase.ControlType.kPosition); + } + + // called periodically + public void setFF(double ff) { + m_anglePIDController.setReference( + m_targetAngle.in(Units.Rotations), + CANSparkBase.ControlType.kPosition, + 0, + ff, + ArbFFUnits.kPercentOut); + } + + public Measure getCurrentAngle() { + return m_shooterAngle.mut_replace(m_angleEncoder.getPosition(), Units.Revolutions); + } + + public void stopAngleMotor() { + m_angleMotorLeader.stopMotor(); + } public double degreesToRotations(double angle) { double rotation = angle / 360; @@ -177,14 +216,13 @@ public Measure> calculateVelocity(double targetY, Measure getShieldPosition() { return m_shieldPosition.mut_replace(m_shieldEncoder.getPosition(), Units.Rotations); } + public void setShieldPosition(double position) { + m_shieldController.getEncoder().setPosition(position); + } + public void stopShieldMotor() { m_shieldController.stopMotor(); } + public boolean isAtAngleSetpoint(double setpoint) { + return Math.abs(m_angleMotorLeader.getEncoder().getPosition() - setpoint) + < ShooterConfig.kAngleError.magnitude(); + } + public boolean isAtFlywheelSetpoint(double setpoint) { return Math.abs(m_topFlywheelEncoder.getPosition() - setpoint) < ShooterConfig.kFlywheelError.magnitude(); diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java deleted file mode 100644 index df3b98ee..00000000 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ /dev/null @@ -1,110 +0,0 @@ -package frc.robot.subsystems.vision; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -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.RobotConstants.VisionConstants; -import java.util.Optional; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.PhotonUtils; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -public class Vision extends SubsystemBase { - - private PhotonCamera camera; - private boolean hasTarget; - private PhotonPipelineResult result; - - private AprilTagFieldLayout aprilTagFieldLayout; - // if there's a pose estimator in the drivetrain subsystem, update it with this estimator - private PhotonPoseEstimator poseEstimator; - - public Vision() { - camera = new PhotonCamera("picam"); - - aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - - poseEstimator = - new PhotonPoseEstimator( - aprilTagFieldLayout, - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - camera, - VisionConstants.robotToCam); - } - - @Override - public void periodic() { - PhotonPipelineResult result = camera.getLatestResult(); - hasTarget = result.hasTargets(); - if (hasTarget) { - this.result = result; - } - Optional currentEstPose = getEstimatedGlobalPose(); - if (currentEstPose.isPresent()) { - SmartDashboard.putNumber("vision/estimated x pos", currentEstPose.get().estimatedPose.getX()); - SmartDashboard.putNumber("vision/estimated y pos", currentEstPose.get().estimatedPose.getY()); - SmartDashboard.putNumber("vision/estimated z pos", currentEstPose.get().estimatedPose.getZ()); - } - } - - // Pose functions - - public Optional getEstimatedGlobalPose() { - if (poseEstimator != null) { - return poseEstimator.update(); - } - return null; - } - - public Pose2d getEstimatedPose2d() { - Optional estPose = poseEstimator.update(); - if (estPose.isPresent()) { - return estPoseToPose2d(estPose.get()); - } - return null; - } - - public Pose2d estPoseToPose2d(EstimatedRobotPose est) { // Converts estimated pose to pose 2d - return new Pose2d( - est.estimatedPose.getX(), - est.estimatedPose.getY(), - new Rotation2d( - est.estimatedPose.getRotation().getX(), est.estimatedPose.getRotation().getY())); - } - - public PhotonTrackedTarget getBestTarget() { - if (hasTarget) { - return result.getBestTarget(); - } else { - return null; - } - } - - public double getDistToTarget() { - return PhotonUtils.calculateDistanceToTargetMeters( - VisionConstants.kCameraHeight, - VisionConstants.kTargetHeight, - VisionConstants.kCameraPitchRadians, - Units.degreesToRadians(getBestTarget().getPitch())); - } - - public boolean getHasTarget() { - return hasTarget; - } - - public PhotonCamera getCam() { - return camera; - } - - public PhotonPoseEstimator getPoseEstimator() { - return poseEstimator; - } -} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json deleted file mode 100644 index 8b1044d3..00000000 --- a/vendordeps/photonlib.json +++ /dev/null @@ -1,57 +0,0 @@ -{ - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2024.2.6", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2024", - "mavenUrls": [ - "https://maven.photonvision.org/repository/internal", - "https://maven.photonvision.org/repository/snapshots" - ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-cpp", - "version": "v2024.2.6", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2024.2.6", - "libName": "photontargeting", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-java", - "version": "v2024.2.6" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2024.2.6" - } - ] -} \ No newline at end of file