Skip to content

Commit

Permalink
Shooter Sim not working
Browse files Browse the repository at this point in the history
  • Loading branch information
legoguy1000 committed Oct 1, 2024
1 parent 37b7a05 commit 07d14dd
Show file tree
Hide file tree
Showing 2 changed files with 47 additions and 3 deletions.
7 changes: 4 additions & 3 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@
import frc.robot.subsystems.intake.IntakeIOSim;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.shooter.ShooterIO;
import frc.robot.subsystems.shooter.ShooterSim;
import frc.robot.subsystems.shooter.ShooterVortex;
import frc.robot.subsystems.swerve.Swerve;
import frc.robot.subsystems.swerve.SwerveIO;
Expand Down Expand Up @@ -160,9 +161,9 @@ public RobotContainer(RobotRunType runtimeType) {
break;
case kSimulation:
s_Swerve = new Swerve(new SwerveIO() {}, cameras);
shooter = new Shooter(new ShooterIO() {});
intake = new Intake(new IntakeIOSim() {});
indexer = new Indexer(new IndexerIOSim() {});
shooter = new Shooter(new ShooterSim());
intake = new Intake(new IntakeIOSim());
indexer = new Indexer(new IndexerIOSim());
elevatorWrist = new ElevatorWrist(new ElevatorWristIO() {}, operator);
break;
default:
Expand Down
43 changes: 43 additions & 0 deletions src/main/java/frc/robot/subsystems/shooter/ShooterSim.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
package frc.robot.subsystems.shooter;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.simulation.FlywheelSim;
import frc.robot.Constants;

/**
* Class for ShooterSim
*/
public class ShooterSim implements ShooterIO {

private FlywheelSim topShooterMotor =
new FlywheelSim(DCMotor.getNeoVortex(1), 1 / Constants.ShooterConstants.GEAR_RATIO, 0.025);
private FlywheelSim bottomShooterMotor =
new FlywheelSim(DCMotor.getNeoVortex(1), 1 / Constants.ShooterConstants.GEAR_RATIO, 0.025);

private double topAppliedVolts = 0.0;
private double bottomAppliedVolts = 0.0;


/**
* Constructor Shooter Subsystem - sets motor and encoder preferences
*/
public ShooterSim() {}

public void setTopMotor(double power) {
topAppliedVolts = MathUtil.clamp(power, -12.0, 12.0);
topShooterMotor.setInputVoltage(topAppliedVolts);
}

public void setBottomMotor(double power) {
bottomAppliedVolts = MathUtil.clamp(power, -12.0, 12.0);
bottomShooterMotor.setInputVoltage(bottomAppliedVolts);
}


@Override
public void updateInputs(ShooterIOInputsAutoLogged inputs) {
inputs.topShooterVelocityRotPerMin = topShooterMotor.getAngularVelocityRPM();
inputs.bottomShooterVelocityRotPerMin = bottomShooterMotor.getAngularVelocityRPM();
}
}

0 comments on commit 07d14dd

Please sign in to comment.