From 0e6b9e85d0fd442dcf73d1ff2f5eed975651aaf0 Mon Sep 17 00:00:00 2001 From: Programming SR <85456157+ProgrammingSR@users.noreply.github.com> Date: Sat, 2 Mar 2024 14:37:04 -0800 Subject: [PATCH] Shoot command (#22) * WIP * Constants Tuning: - Always boot up or deploy code to the robot in arm raised mode - There is a negative sign on the calculate function because it technically goes from 0 to -193 (Set inverted does absolutely nothing that has been deleted) - constant angles have been updated to reflect the offset tuning - kDistanceSensorThreshold as 10 actually works. Needs tuning - Intake Speed adjusted to 0.5. Needs tuning for the ground I suppose. - kEncoderOffset adjusted * Testing: - Distance sensor now uses MXP port instead of onboard (added set automatic mode) - Pid needs tuning -> tolerance set to 15 degrees - Fixed switch logic - Added setSetpoint at the end of setArmPosition so that the setarmpositioncommand does not automatically end - Have note functional - deleted intakemotor guards because inherent and unnecessary (add later if want) * Modified smart dashboard to be exact * Commit message# t abort qqqt :wqq# Your branch is up to date with 'origin/shoot-command'. * testing --- src/main/java/frc/robot/Constants.java | 17 ++++--- src/main/java/frc/robot/Robot.java | 10 ++-- src/main/java/frc/robot/RobotContainer.java | 24 ++++----- .../frc/robot/commands/NoteIntakeCommand.java | 3 +- .../commands/ShooterSetSpeedCommand.java | 51 +++++++++++++++++++ .../frc/robot/subsystems/IntakeSubsystem.java | 34 +++++++++---- .../robot/subsystems/ShooterSubsystem.java | 40 +++++++++++++-- 7 files changed, 138 insertions(+), 41 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6260daa..59dcb39 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -101,23 +101,24 @@ public static final class IntakeConstants { public static final int kArmEncoderChannel = 0; // In degrees - // In degrees - public static final double kIntakeLoweredAngle = 0; - public static final double kIntakeRaisedAngle = 194; - public static final double kIntakeAmpScoringAngle = 100; + public static final double kIntakeLoweredAngle = -193; + public static final double kIntakeRaisedAngle = 0; + public static final double kIntakeAmpScoringAngle = -93; // 193 - 100 (previous angle) /** Encoder offset in rotations */ - public static final double kArmEncoderOffset = 0.6692; + public static final double kArmEncoderOffset = 0.2252; - public static final double kIntakeSpeed = 3.0; + public static final double kIntakeSpeed = 0.5; // TODO: Tune distance sensor threshold for detecting note public static final double kDistanceSensorThreshold = 10; } public static final class ShooterConstants { - public static final int kTopShooterMotorPort = 35; - public static final int kBottomShooterMotorPort = 20; + public static final int kTopShooterMotorPort = 20; + public static final int kBottomShooterMotorPort = 35; + public static final double kShooterSpeed = 0.7; + public static final double kShooterOff = 0; } public static class ClimberConstants { public final static int leftForwardChannel = 0; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 882ff45..d715be3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -23,7 +23,7 @@ public class Robot extends TimedRobot { private RobotContainer m_robotContainer; - public ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); + // public ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); private Timer m_buttonTimer = new Timer(); /** @@ -59,10 +59,10 @@ public void disabledInit() {} @Override public void disabledPeriodic() { - if (RobotController.getUserButton() && m_buttonTimer.get() > 1) { - m_climberSubsystem.toggleCompressor(); - m_buttonTimer.reset(); - } + // if (RobotController.getUserButton() && m_buttonTimer.get() > 1) { + // m_climberSubsystem.toggleCompressor(); + // m_buttonTimer.reset(); + // } } /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e0da380..5845dbe 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.XboxController.Button; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -28,10 +29,12 @@ import frc.robot.commands.IntakeArmPositionCommand; import frc.robot.commands.NoteIntakeCommand; import frc.robot.commands.NoteOuttakeCommand; +import frc.robot.commands.ShooterSetSpeedCommand; import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeSubsystem.ArmPosition; +import frc.robot.subsystems.ShooterSubsystem.ShootSpeed; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.VisionSubsystem; @@ -46,7 +49,7 @@ public class RobotContainer { private final DriveSubsystem m_robotDrive = new DriveSubsystem(); private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); - private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); + // private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); private final VisionSubsystem m_visionSubsystem = new VisionSubsystem(); private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); @@ -118,17 +121,14 @@ private void configureBindings() { // DriveConstants.kMaxSpeedMetersPerSecond - 1, 5, // DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5))); - new JoystickButton(m_operatorController, Button.kX.value) - .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem)) - .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); - new JoystickButton(m_operatorController, Button.kY.value) - .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem)) - .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); - - new JoystickButton(m_operatorController, Button.kA.value) - .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_climberSubsystem)); - new JoystickButton(m_operatorController, Button.kB.value) - .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_climberSubsystem)); + new JoystickButton(m_driverController, Button.kX.value) + .onTrue(new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting), new NoteOuttakeCommand(m_intakeSubsystem))) + .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off)); + + // new JoystickButton(m_operatorController, Button.kA.value) + // .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_climberSubsystem)); + // new JoystickButton(m_operatorController, Button.kB.value) + // .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_climberSubsystem)); new Trigger(() -> { return m_driverController.getLeftTriggerAxis() > 0.5; diff --git a/src/main/java/frc/robot/commands/NoteIntakeCommand.java b/src/main/java/frc/robot/commands/NoteIntakeCommand.java index 18611ce..1dfbc0c 100644 --- a/src/main/java/frc/robot/commands/NoteIntakeCommand.java +++ b/src/main/java/frc/robot/commands/NoteIntakeCommand.java @@ -5,6 +5,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.IntakeConstants; import frc.robot.subsystems.IntakeSubsystem; public class NoteIntakeCommand extends Command { @@ -39,6 +40,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return m_intakeSubsystem.haveNote(); + return m_intakeSubsystem.haveNote() || m_intakeSubsystem.getArmPosition() != IntakeConstants.kIntakeLoweredAngle; } } diff --git a/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java b/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java new file mode 100644 index 0000000..4eea10c --- /dev/null +++ b/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java @@ -0,0 +1,51 @@ +// 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 frc.robot.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.ShooterConstants; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.ShooterSubsystem.ShootSpeed; + +public class ShooterSetSpeedCommand extends Command { + ShooterSubsystem m_ShooterSubsystem; + double m_shooterSpeed = 0; + + Timer m_timer = new Timer(); + + ShootSpeed m_shootSpeed; + + /** Creates a new ShootCommand. */ + public ShooterSetSpeedCommand(ShooterSubsystem shooterSubsystem, ShootSpeed shootSpeed) { + m_ShooterSubsystem = shooterSubsystem; + addRequirements(m_ShooterSubsystem); + + m_shootSpeed = shootSpeed; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_ShooterSubsystem.setShootingSpeed(m_shootSpeed); + + m_timer.reset(); + m_timer.start(); + } + + // 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) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return m_timer.get() > 1; + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index b37d481..70600a7 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -23,7 +23,7 @@ public class IntakeSubsystem extends SubsystemBase { private CANSparkFlex m_intakeMotor = new CANSparkFlex(IntakeConstants.kIntakeMotorID, MotorType.kBrushless); private CANSparkFlex m_armMotor = new CANSparkFlex(IntakeConstants.kArmMotorID, MotorType.kBrushless); - private PIDController m_armPID = new PIDController(0.0015, 0, 0); + private PIDController m_armPID = new PIDController(0.002, 0, 0); private DutyCycleEncoder m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderChannel); @@ -34,17 +34,18 @@ public class IntakeSubsystem extends SubsystemBase { /** Creates a new IntakeSubsystem */ public IntakeSubsystem() { - // m_armMotor.setInverted(IntakeConstants.kArmMotorInverted); - // m_intakeMotor.setInverted(IntakeConstants.kIntakeMotorInverted); + m_distanceSensor.setAutomaticMode(true); m_armEncoder.setPositionOffset(IntakeConstants.kArmEncoderOffset); + SmartDashboard.putNumber("arm", m_armEncoder.getAbsolutePosition()); m_armEncoder.setDistancePerRotation(360); - m_intakeMotor.setIdleMode(IdleMode.kCoast); + m_intakeMotor.setIdleMode(IdleMode.kBrake); m_armMotor.setIdleMode(IdleMode.kCoast); - // m_armPID.disableContinuousInput(); - m_armPID.setTolerance(0.05); + m_armPID.setTolerance(10); + + m_armSetpoint = m_armEncoder.getDistance(); } public void setArmPosition(ArmPosition position) { @@ -53,13 +54,19 @@ public void setArmPosition(ArmPosition position) { m_armSetpoint = IntakeConstants.kIntakeAmpScoringAngle; break; case Extended: - m_armSetpoint = IntakeConstants.kIntakeRaisedAngle; + m_armSetpoint = IntakeConstants.kIntakeLoweredAngle; break; case Retracted: - m_armSetpoint = IntakeConstants.kIntakeLoweredAngle; + m_armSetpoint = IntakeConstants.kIntakeRaisedAngle; default: break; } + + m_armPID.setSetpoint(m_armSetpoint); + } + + public double getArmPosition(){ + return m_armSetpoint; } public boolean armAtSetpoint() { @@ -71,7 +78,7 @@ public void intake() { } public void outtake() { - m_intakeSpeed = -IntakeConstants.kIntakeSpeed; + m_intakeSpeed = -IntakeConstants.kIntakeSpeed - 0.5; } public void stopIntake() { @@ -89,11 +96,16 @@ public double getDistanceSensor() { public void periodic() { haveNote = getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold; - // m_armMotor.set(MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.3, 0.3)); - // m_intakeMotor.set((m_intakeSpeed >= 0 && haveNote) ? 0 : m_intakeSpeed); + //Note: negative because encoder goes from 0 to -193 cuz weird + double setMotorSpeed = MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.4, 0.4); + m_armMotor.set(setMotorSpeed); + m_intakeMotor.set(m_intakeSpeed); + SmartDashboard.putNumber("intakespeed", m_intakeSpeed); SmartDashboard.putNumber("Arm Angle", m_armEncoder.getDistance()); SmartDashboard.putBoolean("Have Note?", haveNote); + SmartDashboard.putNumber("distance sensor", m_distanceSensor.getRange(Rev2mDistanceSensor.Unit.kInches)); + SmartDashboard.putNumber("pid output", setMotorSpeed); } public boolean haveNote() { diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index afdbd19..c7ab4f5 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -5,8 +5,10 @@ package frc.robot.subsystems; import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ShooterConstants; @@ -15,18 +17,48 @@ public class ShooterSubsystem extends SubsystemBase { CANSparkFlex m_bottom = new CANSparkFlex(ShooterConstants.kBottomShooterMotorPort, MotorType.kBrushless); CANSparkFlex m_top = new CANSparkFlex(ShooterConstants.kTopShooterMotorPort, MotorType.kBrushless); + private double m_topSpeed = 0; + private double m_bottomSpeed = 0; + public ShooterSubsystem() { + m_bottom.setIdleMode(IdleMode.kCoast); + m_top.setIdleMode(IdleMode.kCoast); + + m_bottom.setInverted(true); + m_top.setInverted(true); + } + public void setShootingSpeed(ShootSpeed speed) { + switch (speed){ + case Shooting: + m_topSpeed = ShooterConstants.kShooterSpeed; + m_bottomSpeed = ShooterConstants.kShooterSpeed; + // System.out.println("shoot speed: " + ShooterConstants.kShooterSpeed); + break; + case Off: + m_topSpeed = 0.0; + m_bottomSpeed = 0.0; + // System.out.println("shoot speed: " + 0); + break; + } } - public void spin(double speed) { - m_bottom.set(speed); - m_top.set(speed); + public double returnCurrentSpeed(){ + return m_bottom.getEncoder().getVelocity(); } @Override public void periodic() { // This method will be called once per scheduler run - // SmartDashboard.putNumber("Speed", m_bottom.); + SmartDashboard.putNumber("bottom Speed", m_bottomSpeed); + SmartDashboard.putNumber("top Speed", m_topSpeed); + + m_bottom.set(m_bottomSpeed); + m_top.set(m_topSpeed); } + + public static enum ShootSpeed{ + Shooting, + Off + } }