From f1178a8a2a1ea91fcb7a36a44aa50299c0047708 Mon Sep 17 00:00:00 2001 From: npwtub <145060985+npwtub@users.noreply.github.com> Date: Thu, 25 Jan 2024 18:46:07 -0500 Subject: [PATCH 1/7] basicShooterSubsystemSetup --- .../frc2024/controlBoard/CompControl.java | 75 ++++++++++--------- .../frc2024/controlBoard/ControlBoard.java | 16 ++-- .../subsystems/shooter/ShooterSubsystem.java | 32 ++++++++ .../shooterCommands/ShooterCommand.java | 23 ++++++ 4 files changed, 103 insertions(+), 43 deletions(-) create mode 100644 src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java create mode 100644 src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/shooterCommands/ShooterCommand.java diff --git a/src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java b/src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java index 7a1110f3..717f6408 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java +++ b/src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java @@ -4,41 +4,44 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; public class CompControl implements ControlBoard { - private CommandXboxController driver = new CommandXboxController(0); - private CommandXboxController operator = new CommandXboxController(1); - - // TODO: Implement operator controls in the future - // =============Operator Controls============= - - // =============Driver Controls============= - @Override - public double xInput() { - // TODO Auto-generated method stub - return this.driver.getLeftX(); - } - - @Override - public double yInput() { - // TODO Auto-generated method stub - return this.driver.getLeftY(); - } - - @Override - public double rotationalInput() { - // TODO Auto-generated method stub - return this.driver.getRightX(); - } - - @Override - public Trigger brake() { - // TODO Auto-generated method stub - return this.driver.leftBumper(); - } - - @Override - public Trigger setZeroHeading() { - // TODO Auto-generated method stub - return this.driver.rightBumper(); - } + private CommandXboxController driver = new CommandXboxController(0); + private CommandXboxController operator = new CommandXboxController(1); + + // TODO: Implement operator controls in the future + // =============Operator Controls============= + public double shooterInput() { + return this.operator.getRightTriggerAxis(); + } + + // =============Driver Controls============= + @Override + public double xInput() { + // TODO Auto-generated method stub + return this.driver.getLeftX(); + } + + @Override + public double yInput() { + // TODO Auto-generated method stub + return this.driver.getLeftY(); + } + + @Override + public double rotationalInput() { + // TODO Auto-generated method stub + return this.driver.getRightX(); + } + + @Override + public Trigger brake() { + // TODO Auto-generated method stub + return this.driver.leftBumper(); + } + + @Override + public Trigger setZeroHeading() { + // TODO Auto-generated method stub + return this.driver.rightBumper(); + } } diff --git a/src/main/java/org/jmhsrobotics/frc2024/controlBoard/ControlBoard.java b/src/main/java/org/jmhsrobotics/frc2024/controlBoard/ControlBoard.java index 2958f026..1350aace 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/controlBoard/ControlBoard.java +++ b/src/main/java/org/jmhsrobotics/frc2024/controlBoard/ControlBoard.java @@ -4,17 +4,19 @@ public interface ControlBoard { - // =============Operator Controls============= + // =============Operator Controls============= - // =============Driver Controls============= - public Trigger brake(); + public double shooterInput(); - public Trigger setZeroHeading(); + // =============Driver Controls============= + public Trigger brake(); - public double xInput(); + public Trigger setZeroHeading(); - public double yInput(); + public double xInput(); - public double rotationalInput(); + public double yInput(); + + public double rotationalInput(); } diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java new file mode 100644 index 00000000..c2534919 --- /dev/null +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java @@ -0,0 +1,32 @@ +package org.jmhsrobotics.frc2024.subsystems.shooter; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkBase.IdleMode; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ShooterSubsystem extends SubsystemBase { + private CANSparkMax motor1; + private CANSparkMax motor2; + + // private double speed; + + public ShooterSubsystem() { + // Initializes motor(s) + initializeMotors(); + } + + private void initializeMotors() { + motor1.setIdleMode(IdleMode.kCoast); + motor1.setSmartCurrentLimit(20); + motor1.setOpenLoopRampRate(20); + motor2.setIdleMode(IdleMode.kCoast); + motor2.setSmartCurrentLimit(20); + motor2.setOpenLoopRampRate(20); + + motor2.follow(motor1); + } + + public void setSpeed(double speed) { + motor1.set(speed); + } +} diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/shooterCommands/ShooterCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/shooterCommands/ShooterCommand.java new file mode 100644 index 00000000..6ae4b777 --- /dev/null +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/shooterCommands/ShooterCommand.java @@ -0,0 +1,23 @@ +package org.jmhsrobotics.frc2024.subsystems.shooter.shooterCommands; + +import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; +import org.jmhsrobotics.frc2024.controlBoard.ControlBoard; +import edu.wpi.first.wpilibj2.command.Command; + +public class ShooterCommand extends Command { + private ShooterSubsystem shooterSubsystem; + private ControlBoard control; + + public ShooterCommand(ShooterSubsystem shooterSubsystem, ControlBoard control) { + this.shooterSubsystem = shooterSubsystem; + this.control = control; + + addRequirements(this.shooterSubsystem); + } + + @Override + public void execute() { + double motorSpeed = control.shooterInput(); + shooterSubsystem.setSpeed(motorSpeed); + } +} From 7dd91e93b9687e7c790c0043f8a1a7c2889e09b8 Mon Sep 17 00:00:00 2001 From: npwtub Date: Fri, 26 Jan 2024 19:18:27 -0500 Subject: [PATCH 2/7] Shooter Subsystem PID Control added basic PID functionality to shooter subsystem --- .../jmhsrobotics/frc2024/RobotContainer.java | 6 +++ .../subsystems/shooter/ShooterSubsystem.java | 13 ++++++- .../shooterCommands/ShooterCommand.java | 37 ++++++++++++++----- 3 files changed, 45 insertions(+), 11 deletions(-) diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java index f587c41d..72b7e97f 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java @@ -10,6 +10,8 @@ import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem; import org.jmhsrobotics.frc2024.subsystems.drive.commands.DriveCommand; import org.jmhsrobotics.frc2024.subsystems.drive.commands.IntakeCommand; +import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; +import org.jmhsrobotics.frc2024.subsystems.shooter.shooterCommands.ShooterCommand; import org.jmhsrobotics.frc2024.subsystems.vision.VisionSubsystem; import com.pathplanner.lib.auto.AutoBuilder; @@ -35,6 +37,8 @@ public class RobotContainer { private final VisionSubsystem vision = new VisionSubsystem(this.driveSubsystem); + private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(); + private final SendableChooser autoChooser; public RobotContainer() { @@ -47,6 +51,8 @@ public RobotContainer() { configurePathPlanner(); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); + ShooterCommand shooterCommand = new ShooterCommand(2000, shooterSubsystem); + SmartDashboard.putData("Shooter Command", shooterCommand); } private void configurePathPlanner() { diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java index c2534919..de878eaf 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java @@ -1,12 +1,16 @@ package org.jmhsrobotics.frc2024.subsystems.shooter; import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; + import edu.wpi.first.wpilibj2.command.SubsystemBase; public class ShooterSubsystem extends SubsystemBase { - private CANSparkMax motor1; - private CANSparkMax motor2; + private CANSparkMax motor1 = new CANSparkMax(7, MotorType.kBrushless); + private CANSparkMax motor2 = new CANSparkMax(8, MotorType.kBrushless);; + private RelativeEncoder encoder; // private double speed; @@ -26,6 +30,11 @@ private void initializeMotors() { motor2.follow(motor1); } + public double getRPM() { + encoder = motor1.getEncoder(); + return encoder.getVelocity(); + } + public void setSpeed(double speed) { motor1.set(speed); } diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/shooterCommands/ShooterCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/shooterCommands/ShooterCommand.java index 6ae4b777..eefd2021 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/shooterCommands/ShooterCommand.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/shooterCommands/ShooterCommand.java @@ -1,23 +1,42 @@ package org.jmhsrobotics.frc2024.subsystems.shooter.shooterCommands; import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; -import org.jmhsrobotics.frc2024.controlBoard.ControlBoard; + +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.Command; public class ShooterCommand extends Command { - private ShooterSubsystem shooterSubsystem; - private ControlBoard control; + private ShooterSubsystem shooter; + private double targetRPM; + private PIDController pid; + + public ShooterCommand(double targetRPM, ShooterSubsystem shooterSubsystem) { + this.shooter = shooterSubsystem; + this.targetRPM = targetRPM; + pid = new PIDController(.1, 0, 0); + + addRequirements(this.shooter); + } + + @Override + public void initialize() { + pid.reset(); + pid.setSetpoint(targetRPM); + } - public ShooterCommand(ShooterSubsystem shooterSubsystem, ControlBoard control) { - this.shooterSubsystem = shooterSubsystem; - this.control = control; + @Override + public boolean isFinished() { + return false; + } - addRequirements(this.shooterSubsystem); + @Override + public void end(boolean interrupted) { + shooter.setSpeed(0); } @Override public void execute() { - double motorSpeed = control.shooterInput(); - shooterSubsystem.setSpeed(motorSpeed); + double motorSpeed = pid.calculate(shooter.getRPM(), targetRPM); + shooter.setSpeed(motorSpeed); } } From 479914e0c4c2a85baa3e46566a905c47d52f6a63 Mon Sep 17 00:00:00 2001 From: m10653 Date: Sat, 27 Jan 2024 01:04:27 +0000 Subject: [PATCH 3/7] Add rough simulation support --- .../subsystems/shooter/ShooterSubsystem.java | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java index de878eaf..0f480b3c 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java @@ -1,10 +1,15 @@ package org.jmhsrobotics.frc2024.subsystems.shooter; +import org.jmhsrobotics.warcore.rev.RevEncoderSimWrapper; + import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class ShooterSubsystem extends SubsystemBase { @@ -17,6 +22,9 @@ public class ShooterSubsystem extends SubsystemBase { public ShooterSubsystem() { // Initializes motor(s) initializeMotors(); + if(RobotBase.isSimulation()){ + initSim(); + } } private void initializeMotors() { @@ -38,4 +46,19 @@ public double getRPM() { public void setSpeed(double speed) { motor1.set(speed); } + + FlywheelSim flywheelSim; + RevEncoderSimWrapper encSim; + public void initSim(){ + flywheelSim = new FlywheelSim(DCMotor.getNEO(1), 1, 1); + encSim = RevEncoderSimWrapper.create(motor1); + } + + @Override + public void simulationPeriodic() { + double motorVolts = motor1.get()*12; + flywheelSim.setInputVoltage(motorVolts); + flywheelSim.update(0.2); + encSim.setVelocity(flywheelSim.getAngularVelocityRPM()); + } } From 150a52b02968cf15a8e45639917b4a0d865b9203 Mon Sep 17 00:00:00 2001 From: m10653 Date: Sat, 27 Jan 2024 01:07:23 +0000 Subject: [PATCH 4/7] Fix Formatting --- .../frc2024/subsystems/shooter/ShooterSubsystem.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java index 0f480b3c..c76c3ed8 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java @@ -22,7 +22,7 @@ public class ShooterSubsystem extends SubsystemBase { public ShooterSubsystem() { // Initializes motor(s) initializeMotors(); - if(RobotBase.isSimulation()){ + if (RobotBase.isSimulation()) { initSim(); } } @@ -49,14 +49,14 @@ public void setSpeed(double speed) { FlywheelSim flywheelSim; RevEncoderSimWrapper encSim; - public void initSim(){ + public void initSim() { flywheelSim = new FlywheelSim(DCMotor.getNEO(1), 1, 1); encSim = RevEncoderSimWrapper.create(motor1); } @Override public void simulationPeriodic() { - double motorVolts = motor1.get()*12; + double motorVolts = motor1.get() * 12; flywheelSim.setInputVoltage(motorVolts); flywheelSim.update(0.2); encSim.setVelocity(flywheelSim.getAngularVelocityRPM()); From 64256a6c8f847413a6dfcc20452959699da672d8 Mon Sep 17 00:00:00 2001 From: m10653 Date: Mon, 29 Jan 2024 13:31:24 +0000 Subject: [PATCH 5/7] Code Cleanup --- .../org/jmhsrobotics/frc2024/Constants.java | 2 + .../jmhsrobotics/frc2024/RobotContainer.java | 2 +- .../subsystems/shooter/ShooterSubsystem.java | 42 ++++++++++++------- .../ShooterCommand.java | 2 +- 4 files changed, 31 insertions(+), 17 deletions(-) rename src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/{shooterCommands => commands}/ShooterCommand.java (92%) diff --git a/src/main/java/org/jmhsrobotics/frc2024/Constants.java b/src/main/java/org/jmhsrobotics/frc2024/Constants.java index 6c5b92a0..65e9c0a6 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/Constants.java +++ b/src/main/java/org/jmhsrobotics/frc2024/Constants.java @@ -13,6 +13,8 @@ public class Constants { */ public static class CAN { public static final int kIntakeId = 32; + public static final int kShooterTopId = 33; + public static final int kShooterBottomId = 34; }; /** diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java index 87d99e0b..b3504f76 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java @@ -11,7 +11,7 @@ import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem; import org.jmhsrobotics.frc2024.subsystems.drive.commands.DriveCommand; import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; -import org.jmhsrobotics.frc2024.subsystems.shooter.shooterCommands.ShooterCommand; +import org.jmhsrobotics.frc2024.subsystems.shooter.commands.ShooterCommand; import org.jmhsrobotics.frc2024.subsystems.drive.commands.LockAprilTag; import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; import org.jmhsrobotics.frc2024.subsystems.vision.VisionSubsystem; diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java index c76c3ed8..33054147 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java @@ -1,5 +1,6 @@ package org.jmhsrobotics.frc2024.subsystems.shooter; +import org.jmhsrobotics.frc2024.Constants; import org.jmhsrobotics.warcore.rev.RevEncoderSimWrapper; import com.revrobotics.CANSparkMax; @@ -10,12 +11,14 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class ShooterSubsystem extends SubsystemBase { - private CANSparkMax motor1 = new CANSparkMax(7, MotorType.kBrushless); - private CANSparkMax motor2 = new CANSparkMax(8, MotorType.kBrushless);; - private RelativeEncoder encoder; + private CANSparkMax topFlywheel = new CANSparkMax(Constants.CAN.kShooterTopId, MotorType.kBrushless); + private CANSparkMax bottomFlywheel = new CANSparkMax(Constants.CAN.kShooterBottomId, MotorType.kBrushless);; + private RelativeEncoder topEncoder; + private RelativeEncoder bottomEncoder; // private double speed; @@ -28,35 +31,44 @@ public ShooterSubsystem() { } private void initializeMotors() { - motor1.setIdleMode(IdleMode.kCoast); - motor1.setSmartCurrentLimit(20); - motor1.setOpenLoopRampRate(20); - motor2.setIdleMode(IdleMode.kCoast); - motor2.setSmartCurrentLimit(20); - motor2.setOpenLoopRampRate(20); + topFlywheel.setIdleMode(IdleMode.kCoast); + topFlywheel.setSmartCurrentLimit(20); + topFlywheel.setOpenLoopRampRate(20); + topEncoder = topFlywheel.getEncoder(); - motor2.follow(motor1); + bottomFlywheel.setIdleMode(IdleMode.kCoast); + bottomFlywheel.setSmartCurrentLimit(20); + bottomFlywheel.setOpenLoopRampRate(20); + bottomEncoder = bottomFlywheel.getEncoder(); + + bottomFlywheel.follow(topFlywheel); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("shooter/topRPM", getRPM()); + SmartDashboard.putNumber("shooter/bottomRPM", bottomEncoder.getVelocity()); } public double getRPM() { - encoder = motor1.getEncoder(); - return encoder.getVelocity(); + + return topEncoder.getVelocity(); } public void setSpeed(double speed) { - motor1.set(speed); + topFlywheel.set(speed); } FlywheelSim flywheelSim; RevEncoderSimWrapper encSim; public void initSim() { flywheelSim = new FlywheelSim(DCMotor.getNEO(1), 1, 1); - encSim = RevEncoderSimWrapper.create(motor1); + encSim = RevEncoderSimWrapper.create(topFlywheel); } @Override public void simulationPeriodic() { - double motorVolts = motor1.get() * 12; + double motorVolts = topFlywheel.get() * 12; flywheelSim.setInputVoltage(motorVolts); flywheelSim.update(0.2); encSim.setVelocity(flywheelSim.getAngularVelocityRPM()); diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/shooterCommands/ShooterCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/commands/ShooterCommand.java similarity index 92% rename from src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/shooterCommands/ShooterCommand.java rename to src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/commands/ShooterCommand.java index eefd2021..2920f658 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/shooterCommands/ShooterCommand.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/commands/ShooterCommand.java @@ -1,4 +1,4 @@ -package org.jmhsrobotics.frc2024.subsystems.shooter.shooterCommands; +package org.jmhsrobotics.frc2024.subsystems.shooter.commands; import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; From a7648f2d056c62280bf911d7ed21ea91e38c797d Mon Sep 17 00:00:00 2001 From: Luhan Wang <117863446+Lu-han-wang@users.noreply.github.com> Date: Mon, 29 Jan 2024 17:44:58 -0500 Subject: [PATCH 6/7] add some this. --- .../subsystems/shooter/ShooterSubsystem.java | 20 +++++++++---------- .../shooter/commands/ShooterCommand.java | 10 +++++----- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java index 33054147..7fc0435c 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java @@ -31,17 +31,17 @@ public ShooterSubsystem() { } private void initializeMotors() { - topFlywheel.setIdleMode(IdleMode.kCoast); - topFlywheel.setSmartCurrentLimit(20); - topFlywheel.setOpenLoopRampRate(20); - topEncoder = topFlywheel.getEncoder(); + this.topFlywheel.setIdleMode(IdleMode.kCoast); + this.topFlywheel.setSmartCurrentLimit(20); + this.topFlywheel.setOpenLoopRampRate(20); + this.topEncoder = topFlywheel.getEncoder(); - bottomFlywheel.setIdleMode(IdleMode.kCoast); - bottomFlywheel.setSmartCurrentLimit(20); - bottomFlywheel.setOpenLoopRampRate(20); - bottomEncoder = bottomFlywheel.getEncoder(); + this.bottomFlywheel.setIdleMode(IdleMode.kCoast); + this.bottomFlywheel.setSmartCurrentLimit(20); + this.bottomFlywheel.setOpenLoopRampRate(20); + this.bottomEncoder = bottomFlywheel.getEncoder(); - bottomFlywheel.follow(topFlywheel); + this.bottomFlywheel.follow(topFlywheel); } @Override @@ -56,7 +56,7 @@ public double getRPM() { } public void setSpeed(double speed) { - topFlywheel.set(speed); + this.topFlywheel.set(speed); } FlywheelSim flywheelSim; diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/commands/ShooterCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/commands/ShooterCommand.java index 2920f658..c1d5ebeb 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/commands/ShooterCommand.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/commands/ShooterCommand.java @@ -13,15 +13,15 @@ public class ShooterCommand extends Command { public ShooterCommand(double targetRPM, ShooterSubsystem shooterSubsystem) { this.shooter = shooterSubsystem; this.targetRPM = targetRPM; - pid = new PIDController(.1, 0, 0); + this.pid = new PIDController(.1, 0, 0); addRequirements(this.shooter); } @Override public void initialize() { - pid.reset(); - pid.setSetpoint(targetRPM); + this.pid.reset(); + this.pid.setSetpoint(targetRPM); } @Override @@ -31,12 +31,12 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - shooter.setSpeed(0); + this.shooter.setSpeed(0); } @Override public void execute() { double motorSpeed = pid.calculate(shooter.getRPM(), targetRPM); - shooter.setSpeed(motorSpeed); + this.shooter.setSpeed(motorSpeed); } } From b0fd2125140120e66da36467e8532171b7dda2e5 Mon Sep 17 00:00:00 2001 From: m10653 Date: Thu, 1 Feb 2024 23:14:05 +0000 Subject: [PATCH 7/7] Fix Formatting --- .../frc2024/subsystems/shooter/ShooterSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java index a922bc16..1d6a90f3 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java @@ -69,7 +69,7 @@ public void initSim() { @Override public void simulationPeriodic() { - double motorVolts = MathUtil.clamp(topFlywheel.get() * 12,-12,12); + double motorVolts = MathUtil.clamp(topFlywheel.get() * 12, -12, 12); flywheelSim.setInputVoltage(motorVolts); flywheelSim.update(Constants.ksimDtSec); encSim.setVelocity(flywheelSim.getAngularVelocityRPM());