-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* added a bang bang controller to the shooter wheels, ShooterPIDSubsystem UNUSED * add driving practice fixes --------- Co-authored-by: Luhan Wang <[email protected]> Co-authored-by: Connor Murphy <[email protected]>
- Loading branch information
3 people
authored
Feb 22, 2024
1 parent
62750fd
commit 076789c
Showing
6 changed files
with
153 additions
and
23 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
33 changes: 17 additions & 16 deletions
33
src/main/java/org/jmhsrobotics/frc2024/autoCommands/FireCommand.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,22 +1,23 @@ | ||
| package org.jmhsrobotics.frc2024.autoCommands; | ||
| // package org.jmhsrobotics.frc2024.autoCommands; | ||
|
|
||
| import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; | ||
| import org.jmhsrobotics.frc2024.subsystems.intake.commands.IntakeCommand; | ||
| import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; | ||
| import org.jmhsrobotics.frc2024.subsystems.shooter.commands.ShooterCommand; | ||
| // import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem; | ||
| // import org.jmhsrobotics.frc2024.subsystems.intake.commands.IntakeCommand; | ||
| // import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem; | ||
| // import org.jmhsrobotics.frc2024.subsystems.shooter.commands.ShooterCommand; | ||
|
|
||
| import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; | ||
| // import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; | ||
|
|
||
| public class FireCommand extends SequentialCommandGroup { | ||
| private IntakeSubsystem intakeSubsystem; | ||
| private ShooterSubsystem shooterSubsystem; | ||
| // public class FireCommand extends SequentialCommandGroup { | ||
| // private IntakeSubsystem intakeSubsystem; | ||
| // private ShooterSubsystem shooterSubsystem; | ||
|
|
||
| public FireCommand(IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem) { | ||
| this.intakeSubsystem = intakeSubsystem; | ||
| this.shooterSubsystem = shooterSubsystem; | ||
| // public FireCommand(IntakeSubsystem intakeSubsystem, ShooterSubsystem | ||
| // shooterSubsystem) { | ||
| // this.intakeSubsystem = intakeSubsystem; | ||
| // this.shooterSubsystem = shooterSubsystem; | ||
|
|
||
| addCommands(new ShooterCommand(1, this.shooterSubsystem), | ||
| new IntakeCommand(1, this.intakeSubsystem).withTimeout(1)); | ||
| } | ||
| // addCommands(new ShooterCommand(1, this.shooterSubsystem), | ||
| // new IntakeCommand(1, this.intakeSubsystem).withTimeout(1)); | ||
| // } | ||
|
|
||
| } | ||
| // } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
109 changes: 109 additions & 0 deletions
109
src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterPIDSubsystem.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,109 @@ | ||
| package org.jmhsrobotics.frc2024.subsystems.shooter; | ||
|
|
||
| import org.jmhsrobotics.frc2024.Constants; | ||
| 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.MathUtil; | ||
| import edu.wpi.first.math.controller.PIDController; | ||
| 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 ShooterPIDSubsystem extends SubsystemBase { | ||
| 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 PIDController shooterPID; | ||
| private double speed; | ||
|
|
||
| // private double speed; | ||
|
|
||
| public ShooterPIDSubsystem() { | ||
| // Initializes motor(s) | ||
| initializeMotors(); | ||
| if (RobotBase.isSimulation()) { | ||
| initSim(); | ||
| } | ||
| } | ||
|
|
||
| private void initializeMotors() { | ||
| this.topFlywheel.setIdleMode(IdleMode.kCoast); | ||
| this.topFlywheel.setSmartCurrentLimit(20); | ||
| this.topFlywheel.setOpenLoopRampRate(.5); | ||
| this.topEncoder = topFlywheel.getEncoder(); | ||
|
|
||
| this.bottomFlywheel.setIdleMode(IdleMode.kCoast); | ||
| this.bottomFlywheel.setSmartCurrentLimit(20); | ||
| this.bottomFlywheel.setOpenLoopRampRate(.5); | ||
| this.bottomEncoder = bottomFlywheel.getEncoder(); | ||
|
|
||
| this.bottomFlywheel.follow(topFlywheel); | ||
|
|
||
| shooterPID = new PIDController(.004, 0, 0); | ||
| shooterPID.setSetpoint(speed); | ||
| SmartDashboard.putNumber("shooter/goal", 0); | ||
| SmartDashboard.putNumber("shooter/P", shooterPID.getP()); | ||
| SmartDashboard.putNumber("shooter/I", shooterPID.getI()); | ||
| SmartDashboard.putNumber("shooter/D", shooterPID.getD()); | ||
|
|
||
| } | ||
|
|
||
| @Override | ||
| public void periodic() { | ||
| SmartDashboard.putNumber("shooter/topRPM", getRPM()); | ||
| SmartDashboard.putNumber("shooter/bottomRPM", bottomEncoder.getVelocity()); | ||
| double PIDOut = shooterPID.calculate(getRPM()); | ||
| PIDOut = MathUtil.clamp(PIDOut, -.1, 6); | ||
| setVolt(PIDOut); | ||
| SmartDashboard.putNumber("shooter/PIDOut", PIDOut); | ||
| setGoal(SmartDashboard.getNumber("shooter/goal", 0)); | ||
| shooterPID.setP(SmartDashboard.getNumber("shooter/P", 0)); | ||
| shooterPID.setI(SmartDashboard.getNumber("shooter/I", 0)); | ||
| shooterPID.setD(SmartDashboard.getNumber("shooter/D", 0)); | ||
|
|
||
| } | ||
|
|
||
| public double getRPM() { | ||
|
|
||
| return topEncoder.getVelocity(); | ||
| } | ||
|
|
||
| public void setGoal(double goal) { | ||
| shooterPID.setSetpoint(goal); | ||
| } | ||
|
|
||
| public boolean atGoal() { | ||
| return shooterPID.atSetpoint(); | ||
| } | ||
|
|
||
| public void setSpeed(double speed) { | ||
| this.topFlywheel.set(speed); | ||
| } | ||
|
|
||
| public void setVolt(double amount) { | ||
| this.topFlywheel.setVoltage(amount); | ||
| } | ||
|
|
||
| FlywheelSim flywheelSim; | ||
| RevEncoderSimWrapper encSim; | ||
| public void initSim() { | ||
| flywheelSim = new FlywheelSim(DCMotor.getNEO(1), 1, 1); | ||
| encSim = RevEncoderSimWrapper.create(topFlywheel); | ||
| } | ||
|
|
||
| @Override | ||
| public void simulationPeriodic() { | ||
| double motorVolts = MathUtil.clamp(topFlywheel.get() * 12, -12, 12); | ||
| flywheelSim.setInputVoltage(motorVolts); | ||
| flywheelSim.update(Constants.ksimDtSec); | ||
| encSim.setVelocity(flywheelSim.getAngularVelocityRPM()); | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters