diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d6d290e..4a20ca8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -183,6 +183,10 @@ public void placeGamePiecesOnField() {} SimulatedArena.overrideInstance(new EvergreenArena()); } + Indexer indexer = null; + Intake intake = null; + Shooter shooter = null; + // this is here because it doesn't like that the power distribution logger is never closed @SuppressWarnings("resource") public Robot() { @@ -235,11 +239,11 @@ public Robot() { shooter = new ShooterSubsystem( ROBOT_MODE == RobotMode.REAL - ? new HoodIO(HoodIO.getHoodConfiguration(), canivore) - : new HoodIOSim(canivore), + ? new HoodIO(HoodIO.getHoodAlphaConfiguration(), canivore) + : new HoodIOSim(canivore,HoodIO.getHoodAlphaConfiguration(),ShooterSubsystem.HOOD_GEAR_RATIO_A), ROBOT_MODE == RobotMode.REAL - ? new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore) - : new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(), canivore)); + ? new FlywheelIO(FlywheelIO.getFlywheelAlphaConfiguration(), canivore) + : new FlywheelIOSim(FlywheelIO.getFlywheelAlphaConfiguration(), canivore, ShooterSubsystem.FLYWHEEL_GEAR_RATIO)); intake = new FintakeSubsystem( @@ -259,7 +263,15 @@ public Robot() { case COMP: indexer = new SpindexerSubsystem(); intake = new LintakeSubsystem(); - shooter = new TurretSubsystem(); + shooter = + new TurretSubsystem( + ROBOT_MODE == RobotMode.REAL + ? new FlywheelIO(FlywheelIO.getFlywheelCompConfiguration(), canivore) + : new FlywheelIOSim(FlywheelIO.getFlywheelCompConfiguration(), canivore, TurretSubsystem.FLYWHEEL_GEAR_RATIO_C), + ROBOT_MODE == RobotMode.REAL + ? new HoodIO(HoodIO.getHoodCompConfiguration(), canivore) + : new HoodIOSim(canivore, HoodIO.getHoodCompConfiguration(),TurretSubsystem.HOOD_GEAR_RATIO_C)); + climber = new ClimberSubsystem(); // TODO climber break; } @@ -288,6 +300,10 @@ public Robot() { // log if we have uncommitted changes switch (BuildConstants.DIRTY) { case 0: + + + + Logger.recordMetadata("GitDirty", "All changes committed"); break; case 1: @@ -461,6 +477,11 @@ private void addAutos() { // autoChooser.addOption("Index Roller Sysid", indexer.runRollerSysId()); // autoChooser.addOption("Intake Roller Sysid", intake.runRollerSysid()); // autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid()); + autoChooser.addOption("Pitcheck/Intake ", Commands.sequence( + intake.intake().withTimeout(1), + intake.rest().withTimeout(1), + intake.outtake().withTimeout(1)) + ); haveAutosGenerated = true; System.out.println("Done generating autos"); } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem new file mode 100644 index 0000000..be9467d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem @@ -0,0 +1,24 @@ +package frc.robot.subsystems.climber; + +import static edu.wpi.first.units.Units.Volts; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; +import frc.robot.utils.autoaim.AutoAim; +import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; + + + +public class ClimberSubsystem extends SubsystemBase{ + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index 304e65b..7cdabaa 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java @@ -113,7 +113,7 @@ public FlywheelIO(TalonFXConfiguration config, CANBus canbus) { flywheelFollower.optimizeBusUtilization(); } - public static TalonFXConfiguration getFlywheelConfiguration() { + public static TalonFXConfiguration getFlywheelAlphaConfiguration() { TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.NeutralMode = NeutralModeValue.Coast; @@ -136,6 +136,29 @@ public static TalonFXConfiguration getFlywheelConfiguration() { return config; } + public static TalonFXConfiguration getFlywheelCompConfiguration() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + config.Feedback.SensorToMechanismRatio = TurretSubsystem.FLYWHEEL_GEAR_RATIO_C; + + config.Slot0.kS = 0; + config.Slot0.kV = 0; + config.Slot0.kA = 0; + config.Slot0.kP = 0; + config.Slot0.kD = 0; + + config.CurrentLimits.StatorCurrentLimit = 120.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = 80.0; + + config.MotionMagic.MotionMagicAcceleration = 100.0; + + return config; + } + public void setFlywheelVoltage(double volts) { flywheelLeader.setControl(voltageOut.withOutput(volts)); } diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java index 82910fb..828b57c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java @@ -22,18 +22,21 @@ /** Add your docs here. */ public class FlywheelIOSim extends FlywheelIO { TalonFXSimState leaderFxSimState; - DCMotorSim physicsSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX60Foc(2), 0.0136, ShooterSubsystem.FLYWHEEL_GEAR_RATIO), - DCMotor.getKrakenX60Foc(2)); + DCMotorSim physicsSim; + private final double simLoopPeriod = 0.002; private Notifier simNotifier; private double lastSimTime = 0.0; - public FlywheelIOSim(TalonFXConfiguration config, CANBus canbus) { + public FlywheelIOSim(TalonFXConfiguration config, CANBus canbus, double gearRatio) { + super(config, canbus); + physicsSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX60Foc(2), 0.0136, gearRatio), + DCMotor.getKrakenX60Foc(2)); leaderFxSimState = flywheelLeader.getSimState(); leaderFxSimState.setMotorType(MotorType.KrakenX60); leaderFxSimState.Orientation = ChassisReference.CounterClockwise_Positive; @@ -51,9 +54,9 @@ public FlywheelIOSim(TalonFXConfiguration config, CANBus canbus) { physicsSim.update(deltaTime); leaderFxSimState.setRawRotorPosition( - physicsSim.getAngularPosition().in(Rotations) * physicsSim.getGearing()); + physicsSim.getAngularPosition().in(Rotations) * gearRatio); leaderFxSimState.setRotorVelocity( - physicsSim.getAngularVelocity().in(RotationsPerSecond) * physicsSim.getGearing()); + physicsSim.getAngularVelocity().in(RotationsPerSecond) * gearRatio); }); simNotifier.startPeriodic(simLoopPeriod); diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java index 3c38920..5383d91 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java @@ -55,7 +55,7 @@ public static class HoodIOInputs { public HoodIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) { hoodMotor = new TalonFX(11, canbus); - hoodMotor.getConfigurator().apply(HoodIO.getHoodConfiguration()); + hoodMotor.getConfigurator().apply(talonFXConfiguration); hoodPositionRotations = hoodMotor.getPosition(); hoodAngularVelocity = hoodMotor.getVelocity(); @@ -75,7 +75,7 @@ public HoodIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) { hoodMotor.optimizeBusUtilization(); } - public static TalonFXConfiguration getHoodConfiguration() { + public static TalonFXConfiguration getHoodAlphaConfiguration() { TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -86,7 +86,7 @@ public static TalonFXConfiguration getHoodConfiguration() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - config.Feedback.SensorToMechanismRatio = ShooterSubsystem.HOOD_GEAR_RATIO; + config.Feedback.SensorToMechanismRatio = ShooterSubsystem.HOOD_GEAR_RATIO_A; config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; @@ -103,6 +103,35 @@ public static TalonFXConfiguration getHoodConfiguration() { return config; } + public static TalonFXConfiguration getHoodCompConfiguration(){ + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + config.Feedback.SensorToMechanismRatio = TurretSubsystem.HOOD_GEAR_RATIO_C; + + config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; + + config.Slot0.kS = 0; + config.Slot0.kG = 0; + config.Slot0.kV = 0; + config.Slot0.kP = 0; + config.Slot0.kD = 0; + + config.CurrentLimits.StatorCurrentLimit = 80.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = 60.0; + + return config; + + } + public void setHoodVoltage(double hoodVoltage) { hoodMotor.setControl(voltageOut.withOutput(hoodVoltage)); } diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java b/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java index 7aafe9e..4f814e2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java @@ -2,6 +2,7 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.sim.ChassisReference; import com.ctre.phoenix6.sim.TalonFXSimState; import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; @@ -14,20 +15,34 @@ public class HoodIOSim extends HoodIO { TalonFXSimState hoodMotorSim; - private final DCMotorSim hoodPhysicsSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX44Foc(1), 0.01, ShooterSubsystem.HOOD_GEAR_RATIO), - DCMotor.getKrakenX44Foc(1)); - + private final DCMotorSim hoodPhysicsSim; + // will get updated when i get specs private final double simLoopPeriod = 0.002; // 2 ms private Notifier simNotifier = null; private double lastSimTime = 0.0; - public HoodIOSim(CANBus canbus) { - super(HoodIO.getHoodConfiguration(), canbus); + + + + + + + + + + + + + public HoodIOSim(CANBus canbus, TalonFXConfiguration config, double gearRatio ) { + super(config, canbus); + hoodPhysicsSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX44Foc(1), 0.01,gearRatio), + DCMotor.getKrakenX44Foc(1)); + hoodMotorSim = hoodMotor.getSimState(); hoodMotorSim.setMotorType(MotorType.KrakenX44); hoodMotorSim.Orientation = ChassisReference.Clockwise_Positive; @@ -45,12 +60,18 @@ public HoodIOSim(CANBus canbus) { hoodPhysicsSim.update(deltaTime); // rotor position stuff added later when i have access to onshape - - hoodMotorSim.setRawRotorPosition( - hoodPhysicsSim.getAngularPositionRad() * (ShooterSubsystem.HOOD_GEAR_RATIO)); - hoodMotorSim.setRotorVelocity( - hoodPhysicsSim.getAngularVelocityRPM() / 60.0 * ShooterSubsystem.HOOD_GEAR_RATIO); + + hoodMotorSim.setRawRotorPosition( + hoodPhysicsSim.getAngularPositionRad() * (gearRatio)); + hoodMotorSim.setRotorVelocity( + hoodPhysicsSim.getAngularVelocityRPM() / 60.0 * gearRatio); + + + + }); + simNotifier.startPeriodic(simLoopPeriod); } } + diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index cd4ab04..b7feb93 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -25,7 +25,8 @@ /** Fixed shooter. !! ALPHA !! */ public class ShooterSubsystem extends SubsystemBase implements Shooter { - public static double HOOD_GEAR_RATIO = 24.230769; + public static double HOOD_GEAR_RATIO_A = 24.230769; + public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40); public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(2); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 685b95a..af4bde0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -4,66 +4,125 @@ package frc.robot.subsystems.shooter; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.utils.LoggedTunableNumber; +import frc.robot.utils.autoaim.AutoAim; +import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; import java.util.function.Supplier; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; /** Pivoting hooded shooter (turret). !! COMP !! */ public class TurretSubsystem extends SubsystemBase implements Shooter { /** Creates a new TurretSubsystem. */ - public TurretSubsystem() {} + public static double HOOD_GEAR_RATIO_C = 1; + public static double FLYWHEEL_GEAR_RATIO_C = 1; - @Override - public void periodic() { - // This method will be called once per scheduler run + public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40); + public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(2); + + public static double FLYWHEEL_GEAR_RATIO = 28.0 / 24.0; + + public static double FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND = 5.0; + + HoodIO hoodIO; + HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); + + FlywheelIO flywheelIO; + FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); + + public TurretSubsystem(FlywheelIO flywheelIO, HoodIO hoodIO) { + this.flywheelIO = flywheelIO; + this.hoodIO = hoodIO; } + private LoggedTunableNumber testDegrees = new LoggedTunableNumber("Shooter/Test Degrees", 10.0); + private LoggedTunableNumber testVelocity = new LoggedTunableNumber("Shooter/Test Velocity", 30.0); + @Override - public Command shoot(Supplier robotPoseSupplier) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'shoot'"); + public void periodic() { + flywheelIO.updateInputs(flywheelInputs); + Logger.processInputs("Shooter/Flywheel", flywheelInputs); + hoodIO.updateInputs(hoodInputs); + Logger.processInputs("Shooter/Hood", hoodInputs); + } @Override public Command feed(Supplier robotPoseSupplier, Supplier feedTarget) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'feed'"); + return this.run( + () -> { + ShotData shotData = + AutoAim.FEED_SHOT_TREE.get( + robotPoseSupplier + .get() + .getTranslation() + .getDistance(feedTarget.get().getTranslation())); + hoodIO.setHoodPosition(shotData.hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); + }); } @Override public Command rest() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'rest'"); + return this.run( + () -> { + hoodIO.setHoodPosition(HOOD_MIN_ROTATION); // TODO: TUNE TUCKED POSITION IF NEEDED + flywheelIO.setFlywheelVoltage(0.0); + }); } @Override public Command spit() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'spit'"); + return this.run( + () -> { + hoodIO.setHoodPosition(Rotation2d.kZero); + flywheelIO.setMotionProfiledFlywheelVelocity(20); + }); // TODO: TUNE HOOD POS AND FLYWHEEL VELOCITY } @Override + @AutoLogOutput(key = "Shooter/At Vel Setpoint") public boolean atFlywheelVelocitySetpoint() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'atFlywheelVelocitySetpoint'"); + return MathUtil.isNear( + flywheelInputs.flywheelLeaderVelocityRotationsPerSecond, + flywheelIO.getSetpointRotPerSec(), + FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND); } @Override + @AutoLogOutput(key = "Shooter/Hood/At Setpoint") public boolean atHoodSetpoint() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'atHoodSetpoint'"); + return MathUtil.isNear( + hoodInputs.hoodPositionRotations.getDegrees(), hoodIO.getHoodSetpoint().getDegrees(), 1); } @Override public Command zeroHood() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'zeroHood'"); + return this.runOnce(() -> hoodIO.resetEncoder(HOOD_MIN_ROTATION)); } @Override public Command testShoot() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'testShoot'"); + return this.run( + () -> { + hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get())); + flywheelIO.setMotionProfiledFlywheelVelocity(testVelocity.get()); + }); + } + + @Override + public Command shoot(Supplier robotPoseSupplier) { + return this.run( + () -> { + ShotData shotData = + AutoAim.HUB_SHOT_TREE.get(AutoAim.distanceToHub(robotPoseSupplier.get())); + hoodIO.setHoodPosition(shotData.hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); + }); } }