From f45afb90fc1d9b2818604db405b73013543a22ce Mon Sep 17 00:00:00 2001 From: SR1899 Date: Wed, 28 Feb 2024 17:50:15 -0800 Subject: [PATCH 1/6] WIP --- src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/Robot.java | 10 ++-- src/main/java/frc/robot/RobotContainer.java | 22 ++++---- .../commands/ShooterSetSpeedCommand.java | 56 +++++++++++++++++++ .../frc/robot/subsystems/IntakeSubsystem.java | 12 +++- .../robot/subsystems/ShooterSubsystem.java | 25 ++++++++- 6 files changed, 106 insertions(+), 23 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..4f2fe90 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -107,7 +107,7 @@ public static final class IntakeConstants { public static final double kIntakeAmpScoringAngle = 100; /** Encoder offset in rotations */ - public static final double kArmEncoderOffset = 0.6692; + public static final double kArmEncoderOffset = 0.6862; public static final double kIntakeSpeed = 3.0; @@ -118,6 +118,8 @@ public static final class IntakeConstants { public static final class ShooterConstants { public static final int kTopShooterMotorPort = 35; public static final int kBottomShooterMotorPort = 20; + public static final double kShooterSpeed = 3.0; + 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..9506074 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); @@ -119,16 +122,13 @@ private void configureBindings() { // 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)); + .onTrue(new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, true), new NoteOuttakeCommand(m_intakeSubsystem))) + .onFalse(new InstantCommand(() -> m_shooterSubsystem.setShootingSpeed(ShootSpeed.Off), 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 Trigger(() -> { return m_driverController.getLeftTriggerAxis() > 0.5; 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..0530aa1 --- /dev/null +++ b/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java @@ -0,0 +1,56 @@ +// 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(); + + boolean bool = false; + + /** Creates a new ShootCommand. */ + public ShooterSetSpeedCommand(ShooterSubsystem shooterSubsystem, boolean shoot) { + m_ShooterSubsystem = shooterSubsystem; + addRequirements(m_ShooterSubsystem); + + bool = shoot; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + if (bool){ + m_ShooterSubsystem.setShootingSpeed(ShootSpeed.Shooting); + } + else{ + m_ShooterSubsystem.setShootingSpeed(ShootSpeed.Off); + } + + 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() > 0.5; + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index b37d481..d3d5a68 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -38,6 +38,7 @@ public IntakeSubsystem() { // m_intakeMotor.setInverted(IntakeConstants.kIntakeMotorInverted); m_armEncoder.setPositionOffset(IntakeConstants.kArmEncoderOffset); + SmartDashboard.putNumber("arm", m_armEncoder.getAbsolutePosition()); m_armEncoder.setDistancePerRotation(360); m_intakeMotor.setIdleMode(IdleMode.kCoast); @@ -45,6 +46,8 @@ public IntakeSubsystem() { // m_armPID.disableContinuousInput(); m_armPID.setTolerance(0.05); + + m_armSetpoint = m_armEncoder.getDistance(); } public void setArmPosition(ArmPosition position) { @@ -87,13 +90,16 @@ public double getDistanceSensor() { @Override public void periodic() { - haveNote = getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold; + // haveNote = getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold; + haveNote = false; - // 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); + m_armMotor.set(-MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.3, 0.3) / 2); + m_intakeMotor.set((m_intakeSpeed >= 0 && haveNote) ? 0 : 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", -MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.3, 0.3) / 2); } 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..bcf346e 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -5,6 +5,7 @@ package frc.robot.subsystems; import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -15,18 +16,36 @@ 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); } - public void spin(double speed) { - m_bottom.set(speed); - m_top.set(speed); + public void setShootingSpeed(ShootSpeed speed) { + switch (speed){ + case Shooting: + m_topSpeed = ShooterConstants.kShooterSpeed; + m_bottomSpeed = ShooterConstants.kShooterSpeed; + case Off: + m_topSpeed = 0.0; + m_bottomSpeed = 0.0; + } } @Override public void periodic() { // This method will be called once per scheduler run // SmartDashboard.putNumber("Speed", m_bottom.); + m_bottom.set(m_bottomSpeed); + m_top.set(m_topSpeed); } + + public static enum ShootSpeed{ + Shooting, + Off + } } From 421b4a30c5d173603405fedeac5d77b614afb789 Mon Sep 17 00:00:00 2001 From: SR1899 Date: Thu, 29 Feb 2024 18:21:02 -0800 Subject: [PATCH 2/6] 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 --- src/main/java/frc/robot/Constants.java | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4f2fe90..39df74f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -101,15 +101,14 @@ 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.6862; + 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; From 7932f73aee3103207d3bf277faba3910091ba10b Mon Sep 17 00:00:00 2001 From: SR1899 Date: Thu, 29 Feb 2024 18:24:20 -0800 Subject: [PATCH 3/6] 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) --- .../frc/robot/commands/NoteIntakeCommand.java | 3 +- .../frc/robot/subsystems/IntakeSubsystem.java | 33 +++++++++++-------- 2 files changed, 21 insertions(+), 15 deletions(-) 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/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index d3d5a68..4caedfd 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -23,29 +23,27 @@ 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.0014, 0, 0); private DutyCycleEncoder m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderChannel); - private Rev2mDistanceSensor m_distanceSensor = new Rev2mDistanceSensor(Port.kOnboard); // onboard I2C port; + private Rev2mDistanceSensor m_distanceSensor = new Rev2mDistanceSensor(Port.kMXP); // onboard I2C port; private double m_intakeSpeed = 0; private double m_armSetpoint = IntakeConstants.kIntakeRaisedAngle; /** 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(15); m_armSetpoint = m_armEncoder.getDistance(); } @@ -56,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() { @@ -90,15 +94,16 @@ public double getDistanceSensor() { @Override public void periodic() { - // haveNote = getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold; - haveNote = false; + haveNote = getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold; - m_armMotor.set(-MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.3, 0.3) / 2); - m_intakeMotor.set((m_intakeSpeed >= 0 && haveNote) ? 0 : m_intakeSpeed); + //Note: negative because encoder goes from 0 to -193 cuz weird + m_armMotor.set(-MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.4, 0.4)); + 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("distance sensor", m_distanceSensor.getRange(Rev2mDistanceSensor.Unit.kInches)); SmartDashboard.putNumber("pid output", -MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.3, 0.3) / 2); } From 4565404fbac1eb536519a4ae23831ba8b2a268bf Mon Sep 17 00:00:00 2001 From: SR1899 Date: Thu, 29 Feb 2024 18:52:06 -0800 Subject: [PATCH 4/6] Modified smart dashboard to be exact --- src/main/java/frc/robot/subsystems/IntakeSubsystem.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 4caedfd..926ee1c 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -97,14 +97,15 @@ public void periodic() { haveNote = getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold; //Note: negative because encoder goes from 0 to -193 cuz weird - m_armMotor.set(-MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.4, 0.4)); + 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", -MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.3, 0.3) / 2); + SmartDashboard.putNumber("pid output", setMotorSpeed); } public boolean haveNote() { From 563f84c640e8b34f7fa90c5b38f5cfac83896db1 Mon Sep 17 00:00:00 2001 From: SR1899 Date: Thu, 29 Feb 2024 19:08:01 -0800 Subject: [PATCH 5/6] Commit message# t abort qqqt :wqq# Your branch is up to date with 'origin/shoot-command'. --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 39df74f..7dbbcf2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -117,7 +117,7 @@ public static final class IntakeConstants { public static final class ShooterConstants { public static final int kTopShooterMotorPort = 35; public static final int kBottomShooterMotorPort = 20; - public static final double kShooterSpeed = 3.0; + public static final double kShooterSpeed = 0.7; public static final double kShooterOff = 0; } public static class ClimberConstants { From f3188298ac50c99012a6e1aa43b9e6584a8cff15 Mon Sep 17 00:00:00 2001 From: SR1899 Date: Fri, 1 Mar 2024 19:27:07 -0800 Subject: [PATCH 6/6] testing --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 6 +++--- .../robot/commands/ShooterSetSpeedCommand.java | 15 +++++---------- .../frc/robot/subsystems/IntakeSubsystem.java | 10 +++++----- .../frc/robot/subsystems/ShooterSubsystem.java | 15 ++++++++++++++- 5 files changed, 29 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7dbbcf2..59dcb39 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -115,8 +115,8 @@ public static final class IntakeConstants { } 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; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9506074..5845dbe 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -121,9 +121,9 @@ private void configureBindings() { // DriveConstants.kMaxSpeedMetersPerSecond - 1, 5, // DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5))); - new JoystickButton(m_operatorController, Button.kX.value) - .onTrue(new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, true), new NoteOuttakeCommand(m_intakeSubsystem))) - .onFalse(new InstantCommand(() -> m_shooterSubsystem.setShootingSpeed(ShootSpeed.Off), m_shooterSubsystem)); + 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)); diff --git a/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java b/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java index 0530aa1..4eea10c 100644 --- a/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java +++ b/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java @@ -16,25 +16,20 @@ public class ShooterSetSpeedCommand extends Command { Timer m_timer = new Timer(); - boolean bool = false; + ShootSpeed m_shootSpeed; /** Creates a new ShootCommand. */ - public ShooterSetSpeedCommand(ShooterSubsystem shooterSubsystem, boolean shoot) { + public ShooterSetSpeedCommand(ShooterSubsystem shooterSubsystem, ShootSpeed shootSpeed) { m_ShooterSubsystem = shooterSubsystem; addRequirements(m_ShooterSubsystem); - bool = shoot; + m_shootSpeed = shootSpeed; } // Called when the command is initially scheduled. @Override public void initialize() { - if (bool){ - m_ShooterSubsystem.setShootingSpeed(ShootSpeed.Shooting); - } - else{ - m_ShooterSubsystem.setShootingSpeed(ShootSpeed.Off); - } + m_ShooterSubsystem.setShootingSpeed(m_shootSpeed); m_timer.reset(); m_timer.start(); @@ -51,6 +46,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return m_timer.get() > 0.5; + 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 926ee1c..70600a7 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -23,11 +23,11 @@ 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.0014, 0, 0); + private PIDController m_armPID = new PIDController(0.002, 0, 0); private DutyCycleEncoder m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderChannel); - private Rev2mDistanceSensor m_distanceSensor = new Rev2mDistanceSensor(Port.kMXP); // onboard I2C port; + private Rev2mDistanceSensor m_distanceSensor = new Rev2mDistanceSensor(Port.kOnboard); // onboard I2C port; private double m_intakeSpeed = 0; private double m_armSetpoint = IntakeConstants.kIntakeRaisedAngle; @@ -43,7 +43,7 @@ public IntakeSubsystem() { m_intakeMotor.setIdleMode(IdleMode.kBrake); m_armMotor.setIdleMode(IdleMode.kCoast); - m_armPID.setTolerance(15); + m_armPID.setTolerance(10); m_armSetpoint = m_armEncoder.getDistance(); } @@ -78,7 +78,7 @@ public void intake() { } public void outtake() { - m_intakeSpeed = -IntakeConstants.kIntakeSpeed; + m_intakeSpeed = -IntakeConstants.kIntakeSpeed - 0.5; } public void stopIntake() { @@ -97,7 +97,7 @@ public void periodic() { haveNote = getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold; //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); + 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); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index bcf346e..c7ab4f5 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -8,6 +8,7 @@ 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; @@ -23,6 +24,8 @@ 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) { @@ -30,16 +33,26 @@ public void setShootingSpeed(ShootSpeed 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 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); }