From f3188298ac50c99012a6e1aa43b9e6584a8cff15 Mon Sep 17 00:00:00 2001 From: SR1899 Date: Fri, 1 Mar 2024 19:27:07 -0800 Subject: [PATCH] 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); }