diff --git a/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java b/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java index 83c744e..2bbd047 100644 --- a/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java +++ b/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java @@ -75,9 +75,9 @@ public record TalonFXFollower(Device.CAN id, MotorAlignmentValue opposesMain) {} // Preconfigured control objects reused for efficiency protected final CoastOut coastControl = new CoastOut(); protected final StaticBrake brakeControl = new StaticBrake(); - protected final VoltageOut voltageControl = new VoltageOut(0).withEnableFOC(true); + protected final VoltageOut voltageControl = new VoltageOut(0); protected final TorqueCurrentFOC currentControl = new TorqueCurrentFOC(0); - protected final DutyCycleOut dutyCycleControl = new DutyCycleOut(0).withEnableFOC(true); + protected final DutyCycleOut dutyCycleControl = new DutyCycleOut(0); protected final DynamicMotionMagicTorqueCurrentFOC positionControl = new DynamicMotionMagicTorqueCurrentFOC(0, 0, 0); protected final VelocityTorqueCurrentFOC velocityControl = new VelocityTorqueCurrentFOC(0); @@ -166,7 +166,7 @@ protected boolean isRunningPositionControl() { return (control instanceof PositionTorqueCurrentFOC) || (control instanceof PositionVoltage) || (control instanceof MotionMagicTorqueCurrentFOC) - || (control instanceof DynamicMotionMagicTorqueCurrentFOC) + || (control instanceof MotionMagicDutyCycle) || (control instanceof MotionMagicVoltage); } @@ -181,7 +181,7 @@ protected boolean isRunningVelocityControl() { var control = motor.getAppliedControl(); return (control instanceof VelocityTorqueCurrentFOC) || (control instanceof VelocityVoltage) - || (control instanceof MotionMagicVelocityTorqueCurrentFOC) + || (control instanceof MotionMagicVelocityDutyCycle) || (control instanceof MotionMagicVelocityVoltage); } @@ -196,8 +196,8 @@ protected boolean isRunningVelocityControl() { protected boolean isRunningMotionMagic() { var control = motor.getAppliedControl(); return (control instanceof MotionMagicTorqueCurrentFOC) - || (control instanceof DynamicMotionMagicTorqueCurrentFOC) - || (control instanceof MotionMagicVelocityTorqueCurrentFOC) + || (control instanceof MotionMagicDutyCycle) + || (control instanceof MotionMagicVelocityDutyCycle) || (control instanceof MotionMagicVoltage) || (control instanceof MotionMagicVelocityVoltage); } @@ -395,13 +395,7 @@ public void runPosition( Velocity maxJerk, PIDSlot slot) { this.goalPosition = position; - motor.setControl( - positionControl - .withPosition(position) - .withVelocity(cruiseVelocity) - .withAcceleration(acceleration) - .withJerk(maxJerk) - .withSlot(slot.getNum())); + motor.setControl(positionControl.withPosition(position).withSlot(slot.getNum())); } /** diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c23c8bb..406dbf6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -266,10 +266,10 @@ public final class ShooterConstants { public static final Angle STARTING_ANGLE = Rotations.of(0.0); public static final Distance WHEEL_RADIUS = Meters.of(0.5); - public static final double IDLE_SPEED_RPM = (1.0); - public static final double HUB_SPEED_RPM = (1.0); - public static final double TOWER_SPEED_RPM = (1.0); - public static final double DEFAULT_SPEED_RPM = (1.0); + public static final AngularVelocity IDLE_SPEED_RPM = RotationsPerSecond.of(1.0); + public static final AngularVelocity HUB_SPEED_RPM = RotationsPerSecond.of(1.0); + public static final AngularVelocity TOWER_SPEED_RPM = RotationsPerSecond.of(1.0); + public static final AngularVelocity DEFAULT_SPEED_RPM = RotationsPerSecond.of(1.0); public static final double FLYWHEEL_VELOCITY_TOLERANCE = 1.0; // Hood Constants @@ -286,9 +286,9 @@ public final class ShooterConstants { new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); } - public class ShooterFlywheelConstants { - - public static String NAME = "ShooterFlywheel"; + // Needs tuned + public class ShooterFeederConstants { + public static String NAME = "ShooterFeederFlywheel"; public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI); public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second); @@ -340,6 +340,103 @@ public static TalonFXConfiguration getFXConfig(boolean invert) { } } + // Needs tuned + public class ShooterTowerConstants { + public static String NAME = "ShooterTowerFlywheel"; + + public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI); + public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second); + + private static final double GEARING = (2.0 / 1.0); + + public static final AngularVelocity TOLERANCE = MAX_VELOCITY.times(0.1); + + public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); + public static final MomentOfInertia MOI = KilogramSquareMeters.of(1.0); + + // Velocity PID + private static Slot0Configs SLOT0CONFIG = + new Slot0Configs().withKP(1000.0).withKI(0.0).withKD(0.0); + + public static TalonFXConfiguration getFXConfig() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.CurrentLimits.SupplyCurrentLimitEnable = Robot.isReal(); + config.CurrentLimits.SupplyCurrentLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerTime = 0.1; + + config.CurrentLimits.StatorCurrentLimitEnable = Robot.isReal(); + config.CurrentLimits.StatorCurrentLimit = 80.0; + + config.Voltage.PeakForwardVoltage = 12.0; + config.Voltage.PeakReverseVoltage = -12.0; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; + + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; + + config.Feedback.RotorToSensorRatio = 1.0; + + config.Feedback.SensorToMechanismRatio = GEARING; + + config.Slot0 = SLOT0CONFIG; + + return config; + } + } + + public class ShooterFlywheelConstants { + public static String NAME = "ShooterFlywheel"; + + public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI); + public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second); + + private static final double GEARING = (5.0 / 1.0); + + public static final AngularVelocity TOLERANCE = MAX_VELOCITY.times(0.1); + + public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); + public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.2); + + // Velocity PID + private static Slot0Configs SLOT0CONFIG = + new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0); + + public static TalonFXConfiguration getFXConfig() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.CurrentLimits.SupplyCurrentLimitEnable = Robot.isReal(); + config.CurrentLimits.SupplyCurrentLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerTime = 0.1; + + config.CurrentLimits.StatorCurrentLimitEnable = Robot.isReal(); + config.CurrentLimits.StatorCurrentLimit = 80.0; + + config.Voltage.PeakForwardVoltage = 12.0; + config.Voltage.PeakReverseVoltage = -12.0; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; + + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; + + config.Feedback.RotorToSensorRatio = 1.0; + + config.Feedback.SensorToMechanismRatio = GEARING; + + config.Slot0 = SLOT0CONFIG; + + return config; + } + } + public class ShooterRotaryConstants { public static String NAME = "ShooterRotary"; @@ -350,13 +447,13 @@ public class ShooterRotaryConstants { // CRUISE_VELOCITY.div(0.1).per(Units.Second); // public static final Velocity JERK = ACCELERATION.per(Second); - private static final double ROTOR_TO_SENSOR = (2.0 / 1.0); - private static final double SENSOR_TO_MECHANISM = (2.0 / 1.0); + private static final double ROTOR_TO_SENSOR = (1.0 / 1.0); + private static final double SENSOR_TO_MECHANISM = (50.0 / 1.0); public static final Translation3d OFFSET = Translation3d.kZero; public static final Angle MIN_ANGLE = Degrees.of(0.0); - public static final Angle MAX_ANGLE = Rotations.of(45.0); + public static final Angle MAX_ANGLE = Degrees.of(45.0); public static final Angle STARTING_ANGLE = Rotations.of(0.0); public static final Distance ARM_LENGTH = Meters.of(1.0); @@ -395,12 +492,12 @@ public class ShooterRotaryConstants { public static TalonFXConfiguration getFXConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); - config.CurrentLimits.SupplyCurrentLimitEnable = false; + config.CurrentLimits.SupplyCurrentLimitEnable = Robot.isReal(); config.CurrentLimits.SupplyCurrentLimit = 40.0; config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; config.CurrentLimits.SupplyCurrentLowerTime = 0.1; - config.CurrentLimits.StatorCurrentLimitEnable = false; + config.CurrentLimits.StatorCurrentLimitEnable = Robot.isReal(); config.CurrentLimits.StatorCurrentLimit = 80.0; config.Voltage.PeakForwardVoltage = 12.0; @@ -418,8 +515,6 @@ public static TalonFXConfiguration getFXConfig() { config.Feedback.RotorToSensorRatio = ROTOR_TO_SENSOR; config.Feedback.SensorToMechanismRatio = SENSOR_TO_MECHANISM; - config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; - config.Slot0 = SLOT0CONFIG; return config; @@ -452,10 +547,10 @@ public class IntakeFlywheelConstants { public static final Angle MIN_ANGLE = Rotations.of(0.0); public static final Angle MAX_ANGLE = Rotations.of(1); public static final Angle STARTING_ANGLE = Rotations.of(0.0); - public static final double PICKUP_SPEED = 0.0; + public static final double PICKUP_SPEED = 10.0; public static final Distance WHEEL_RADIUS = Meters.of(0.05); public static final Translation3d OFFSET = Translation3d.kZero; - public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.0028125); + public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.2); public static final RotaryMechCharacteristics CONSTANTS = new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); public static final String MOTOR_NAME = "Intake Flywheel"; @@ -584,7 +679,10 @@ public static TalonFXConfiguration getFXConfig() { public class IntakePivotConstants { public static final String NAME = "Intake"; - public static final Angle PICKUP_ANGLE = Rotations.of(0.0); + public static final Angle MIN_ANGLE = Degrees.of(0.0); + public static final Angle MAX_ANGLE = Degrees.of(130.0); + + public static final Angle PICKUP_ANGLE = MAX_ANGLE; public static final Angle STOW_ANGLE = Rotations.of(0.0); public static final Angle TOLERANCE = Degrees.of(1.0); @@ -594,11 +692,8 @@ public class IntakePivotConstants { public static final Velocity JERK = RadiansPerSecondPerSecond.per(Second).of(0.1); - private static final double ROTOR_TO_SENSOR = (50.0 / 1.0); - private static final double SENSOR_TO_MECHANISM = 1.0; - - public static final Angle MIN_ANGLE = Degrees.of(0.0); - public static final Angle MAX_ANGLE = Degrees.of(130.0); + private static final double ROTOR_TO_SENSOR = (1.0 / 1.0); + private static final double SENSOR_TO_MECHANISM = (50.0 / 1.0); public static final Angle STARTING_ANGLE = Radians.zero(); public static final Distance ARM_LENGTH = Foot.one(); @@ -607,7 +702,7 @@ public class IntakePivotConstants { new Translation3d(), ARM_LENGTH, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); - public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.25); + public static final MomentOfInertia MOI = KilogramSquareMeters.of(1); // Positional PID public static final Slot0Configs SLOT_0_CONFIG = @@ -652,11 +747,6 @@ public static TalonFXConfiguration getFXConfig() { } } - public class FeederConstants { - public static final AngularVelocity FEED_SPEED = RotationsPerSecond.of(0.0); - public static final AngularAcceleration FEED_ACCELERATION = RotationsPerSecondPerSecond.of(0.0); - } - public class ClimberConstants { public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); public static final Mass CARRIAGE_MASS = Kilograms.of(2.5); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 70a5332..26f864a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,22 +24,30 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.lib.W8.io.motor.*; +import frc.lib.W8.io.vision.VisionIO; import frc.lib.W8.io.vision.VisionIOPhotonVision; import frc.lib.W8.io.vision.VisionIOPhotonVisionSim; import frc.lib.W8.mechanisms.flywheel.*; +import frc.lib.W8.mechanisms.linear.LinearMechanism; +import frc.lib.W8.mechanisms.linear.LinearMechanismReal; +import frc.lib.W8.mechanisms.linear.LinearMechanismSim; import frc.lib.W8.mechanisms.rotary.RotaryMechanism; import frc.lib.W8.mechanisms.rotary.RotaryMechanismReal; import frc.lib.W8.mechanisms.rotary.RotaryMechanismSim; +import frc.robot.Constants.ClimberConstants; import frc.robot.Constants.HopperConstants; import frc.robot.Constants.IntakeFlywheelConstants; import frc.robot.Constants.IntakeFlywheelConstants.VisionConstants; import frc.robot.Constants.IntakePivotConstants; import frc.robot.Constants.Ports; +import frc.robot.Constants.ShooterFeederConstants; import frc.robot.Constants.ShooterFlywheelConstants; import frc.robot.Constants.ShooterRotaryConstants; +import frc.robot.Constants.ShooterTowerConstants; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.BallCounter; +import frc.robot.subsystems.Climber; import frc.robot.subsystems.Hopper; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; @@ -65,6 +73,7 @@ public class RobotContainer { // Subsystems public final Drive drive; private final Hopper hopper; + private final Climber climber; private final Shooter shooter; public final Intake intake; private final BallCounter ballCounter; @@ -104,17 +113,22 @@ public RobotContainer() { new FlywheelMechanismReal( new MotorIOTalonFX( ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(false), + ShooterFlywheelConstants.getFXConfig(), Ports.LeftFlywheel)), new FlywheelMechanismReal( new MotorIOTalonFX( ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(true), + ShooterFlywheelConstants.getFXConfig(), Ports.RightFlywheel)), new FlywheelMechanismReal( new MotorIOTalonFX( - ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(false), + ShooterFeederConstants.NAME, + ShooterFeederConstants.getFXConfig(false), + Ports.Spindexer)), + new FlywheelMechanismReal( + new MotorIOTalonFX( + ShooterTowerConstants.NAME, + ShooterTowerConstants.getFXConfig(), Ports.TowerRoller)), new RotaryMechanismReal( new MotorIOTalonFX( @@ -145,6 +159,15 @@ public RobotContainer() { VisionConstants.robotToCamera0, VisionConstants.aprilTagLayout, PoseStrategy.CONSTRAINED_SOLVEPNP)); + climber = + new Climber( + new LinearMechanismReal( + new MotorIOTalonFX( + ClimberConstants.MOTOR_NAME, + ClimberConstants.getFXConfig(), + Ports.ClimberMotor), + ClimberConstants.CHARACTERISTICS)); + break; case SIM: @@ -170,7 +193,7 @@ public RobotContainer() { new FlywheelMechanismSim( new MotorIOTalonFXSim( ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(false), + ShooterFlywheelConstants.getFXConfig(), Ports.LeftFlywheel), ShooterFlywheelConstants.DCMOTOR, ShooterFlywheelConstants.MOI, @@ -178,19 +201,27 @@ public RobotContainer() { new FlywheelMechanismSim( new MotorIOTalonFXSim( ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(true), - Ports.LeftFlywheel), + ShooterFlywheelConstants.getFXConfig(), + Ports.RightFlywheel), ShooterFlywheelConstants.DCMOTOR, ShooterFlywheelConstants.MOI, ShooterFlywheelConstants.TOLERANCE), new FlywheelMechanismSim( new MotorIOTalonFXSim( - ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(false), + ShooterFeederConstants.NAME, + ShooterFeederConstants.getFXConfig(false), + Ports.Spindexer), + ShooterFeederConstants.DCMOTOR, + ShooterFeederConstants.MOI, + ShooterFeederConstants.TOLERANCE), + new FlywheelMechanismSim( + new MotorIOTalonFXSim( + ShooterTowerConstants.NAME, + ShooterTowerConstants.getFXConfig(), Ports.TowerRoller), - ShooterFlywheelConstants.DCMOTOR, - ShooterFlywheelConstants.MOI, - ShooterFlywheelConstants.TOLERANCE), + ShooterTowerConstants.DCMOTOR, + ShooterTowerConstants.MOI, + ShooterTowerConstants.TOLERANCE), new RotaryMechanismSim( new MotorIOTalonFXSim( ShooterRotaryConstants.NAME, @@ -198,7 +229,7 @@ public RobotContainer() { Ports.HoodMotor), ShooterRotaryConstants.DCMOTOR, ShooterRotaryConstants.MOI, - true, + false, ShooterRotaryConstants.CONSTANTS, java.util.Optional.empty())); @@ -216,7 +247,7 @@ public RobotContainer() { new MotorIOTalonFXSim( IntakePivotConstants.NAME, IntakePivotConstants.getFXConfig(), - Ports.IntakeRoller), + Ports.IntakePivot), IntakePivotConstants.DCMOTOR, IntakePivotConstants.MOI, false, @@ -231,6 +262,18 @@ public RobotContainer() { VisionConstants.aprilTagLayout, PoseStrategy.CONSTRAINED_SOLVEPNP, VisionConstants.getSystemSim())); + + climber = + new Climber( + new LinearMechanismSim( + new MotorIOTalonFXSim( + ClimberConstants.MOTOR_NAME, + ClimberConstants.getFXConfig(), + Ports.ClimberMotor), + ClimberConstants.DCMOTOR, + ClimberConstants.CARRIAGE_MASS, + ClimberConstants.CHARACTERISTICS, + false)); break; default: @@ -250,13 +293,18 @@ public RobotContainer() { new FlywheelMechanism() {}, new FlywheelMechanism() {}, new FlywheelMechanism() {}, + new FlywheelMechanism() {}, new RotaryMechanism(null, null) {}); intake = new Intake( new FlywheelMechanism() {}, new RotaryMechanism(IntakePivotConstants.NAME, IntakePivotConstants.CONSTANTS) {}); - vision = new Vision(null); + vision = new Vision(new VisionIO() {}); + climber = + new Climber( + new LinearMechanism( + ClimberConstants.MOTOR_NAME, ClimberConstants.CHARACTERISTICS) {}); break; } @@ -326,18 +374,19 @@ private void configureButtonBindings() { .x() .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); - controller.rightTrigger().onTrue(Commands.runOnce(() -> shooter.simShoot())); + // Commands for sim testing! + // controller.rightTrigger().onTrue(Commands.runOnce(() -> shooter.simShoot())); + + // controller + // .leftTrigger() + // .onTrue( + // Commands.runOnce( + // () -> { + // Robot.fuelSim.clearFuel(); + // Robot.fuelSim.spawnStartingFuel(); + // intake.simBalls = 0; + // })); - controller - .leftTrigger() - .onTrue( - Commands.runOnce( - () -> { - Robot.fuelSim.clearFuel(); - Robot.fuelSim.spawnStartingFuel(); - intake.simBalls = 0; - })); - controller.y().onTrue(Commands.runOnce(() -> intake.setVelocity(1))); controller.leftBumper().onTrue(intake.intake()); controller.rightBumper().onTrue(intake.stowAndStopRollers()); controller.a().onTrue(shooter.runFlywheel()); diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 4aade98..7354a3a 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -2,13 +2,11 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; @@ -43,21 +41,6 @@ public boolean isAboveCurrentLimit() { } } - @Override - public void periodic() { - _io.periodic(); - - double z = Math.abs(Math.sin(Timer.getFPGATimestamp()) * 0.33); // Placeholder for position - - // The z of the Translation3D should be - // 'ClimberConstants.CONVERTER.toDistance(_io.getPosition()).in(Meters)', change after fixing - // motor configs. - Logger.recordOutput( - "3DField/4_Climber", new Pose3d(new Translation3d(0, 0, z), new Rotation3d(0, 0, 0))); - - _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); - } - public Command runClimber() { return this.runOnce( () -> @@ -79,4 +62,19 @@ public boolean nearGoalposition() { return false; } } + + @Override + public void periodic() { + _io.periodic(); + + Logger.recordOutput( + "3DField/4_Climber", + new Pose3d( + new Translation3d( + 0, 0, ClimberConstants.CONVERTER.toDistance(_io.getPosition()).in(Meters)), + new Rotation3d(0, 0, 0))); + + // _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); // For testing climber + // motion! + } } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 414cebc..1961f6c 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -3,7 +3,6 @@ import static edu.wpi.first.units.Units.Degree; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; @@ -16,6 +15,7 @@ import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; import frc.lib.W8.mechanisms.rotary.RotaryMechanism; +import frc.lib.W8.util.LoggerHelper; import frc.robot.Constants.IntakeFlywheelConstants; import frc.robot.Constants.IntakePivotConstants; import org.littletonrobotics.junction.Logger; @@ -42,16 +42,13 @@ public void setVelocity(double velocity) { _rollerIO.runVelocity(angVelo, IntakeFlywheelConstants.ACCELERATION, PIDSlot.SLOT_0); } - public Command setPivotAngle(Angle pivotAngle) { - return this.runOnce( - () -> - _pivotIO.runPosition( - pivotAngle, - IntakeFlywheelConstants.CRUISE_VELOCITY, - IntakeFlywheelConstants.ACCELERATION, - IntakeFlywheelConstants.JERK, - PIDSlot.SLOT_0)); - // .withName("Go To " + setpoint.toString() + " Setpoint"); + public void setPivotAngle(Angle pivotAngle) { + _pivotIO.runPosition( + pivotAngle, + IntakeFlywheelConstants.CRUISE_VELOCITY, + IntakeFlywheelConstants.ACCELERATION, + IntakeFlywheelConstants.JERK, + PIDSlot.SLOT_0); } public AngularVelocity getVelocity() { @@ -68,8 +65,8 @@ public void stop() { public Command intake() { return Commands.sequence( - Commands.run(() -> setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)), - setPivotAngle(IntakePivotConstants.PICKUP_ANGLE)); + Commands.runOnce(() -> this.setPivotAngle(IntakePivotConstants.PICKUP_ANGLE)), + Commands.runOnce(() -> this.setVelocity(IntakeFlywheelConstants.PICKUP_SPEED))); } public boolean isIntendedAngle() { @@ -102,10 +99,11 @@ private Command setStowAngle(Angle stowAngle) { @Override public void periodic() { - if (_pivotIO.getPosition().in(Degree) < IntakePivotConstants.MAX_ANGLE.in(Degree)) - _pivotIO.runVoltage(Volts.of(0.25)); + LoggerHelper.recordCurrentCommand("1_Intake", this); _pivotIO.periodic(); + _rollerIO.periodic(); + Logger.recordOutput( "3DField/1_Intake", new Pose3d( @@ -117,6 +115,11 @@ public void periodic() { new Translation3d(Math.sin(_pivotIO.getPosition().in(Radians) * 0.1055), 0, 0), new Rotation3d(0, 0, 0))); - // _pivotIO.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*0.25)); //--- Tests the pivot + // For testing purposes, opens hopper in sim + // if (_pivotIO.getPosition().in(Degrees) < IntakePivotConstants.MAX_ANGLE.in(Degrees) - 10) + // _pivotIO.runVoltage(Volts.of(7)); + + // _pivotIO.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*5.0)); //--- Tests the pivot + // _rollerIO.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*5.0)); //--- Tests the pivot } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 2cc0c7a..0d8931a 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -3,26 +3,24 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; import frc.lib.W8.mechanisms.rotary.RotaryMechanism; -import frc.robot.Constants.FeederConstants; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.ShooterFeederConstants; +import frc.robot.Constants.ShooterTowerConstants; import frc.robot.Robot; import org.littletonrobotics.junction.Logger; @@ -31,6 +29,7 @@ public class Shooter extends SubsystemBase { private final FlywheelMechanism _rflywheel; private final FlywheelMechanism _lflywheel; private final FlywheelMechanism _feeder; + private final FlywheelMechanism _tower; private final RotaryMechanism _hood; // desired target values @@ -41,17 +40,27 @@ public Shooter( FlywheelMechanism lflywheel, FlywheelMechanism rflywheel, FlywheelMechanism feeder, + FlywheelMechanism tower, RotaryMechanism hood) { _lflywheel = lflywheel; _rflywheel = rflywheel; _feeder = feeder; + _tower = tower; _hood = hood; } // Sets feeder motor speed public void runFeeder() { _feeder.runVelocity( - FeederConstants.FEED_SPEED, FeederConstants.FEED_ACCELERATION, PIDSlot.SLOT_2); + ShooterFeederConstants.MAX_VELOCITY, + ShooterFeederConstants.MAX_ACCELERATION, + PIDSlot.SLOT_2); + } + + // Sets tower motor speed + public void runTower() { + _feeder.runVelocity( + ShooterTowerConstants.MAX_VELOCITY, ShooterTowerConstants.MAX_ACCELERATION, PIDSlot.SLOT_2); } // Sets the flywheel velocity based on an input. @@ -64,20 +73,21 @@ public void setFlywheelVelocity(double velocity) { _rflywheel.runVelocity(negangVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); } - public enum State { - OFF(Units.RevolutionsPerSecond.of(0.0)), - IDLE(Units.RevolutionsPerSecond.of(ShooterConstants.IDLE_SPEED_RPM / 60)), - SHOOT_FROM_HUB(Units.RevolutionsPerSecond.of(ShooterConstants.HUB_SPEED_RPM / 60)), - SHOOT_FROM_TOWER(Units.RevolutionsPerSecond.of(ShooterConstants.TOWER_SPEED_RPM / 60)), - SHOOT(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)), - SHOOT_ON_MOVE(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)); + // // Broken aha !! + // public enum State { + // OFF(Units.RevolutionsPerSecond.of(0.0)), + // IDLE(Units.RevolutionsPerSecond.of(ShooterConstants.IDLE_SPEED_RPM / 60)), + // SHOOT_FROM_HUB(Units.RevolutionsPerSecond.of(ShooterConstants.HUB_SPEED_RPM / 60)), + // SHOOT_FROM_TOWER(Units.RevolutionsPerSecond.of(ShooterConstants.TOWER_SPEED_RPM / 60)), + // SHOOT(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)), + // SHOOT_ON_MOVE(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)); - private final AngularVelocity stateVelocity; + // private final AngularVelocity stateVelocity; - State(AngularVelocity stateVelocity) { - this.stateVelocity = stateVelocity; - } - } + // State(AngularVelocity stateVelocity) { + // this.stateVelocity = stateVelocity; + // } + // } // Checks if the flywheel is at speed and returns a boolean public boolean flyAtVelocity() { @@ -88,8 +98,7 @@ public boolean flyAtVelocity() { } public double getHoodAngleDegrees(Translation2d robotPos) { - - // TODO: Replace with HUB later once it gets added. + // Replace with HUB when it is added double distance = robotPos.getDistance(FieldConstants.FIELDCENTER); double check = @@ -134,6 +143,7 @@ public Command shoot(double velocity) { Commands.run(() -> setHoodAngle(hoodAngle)).until(this::hoodAtAngle)), // feed once ready Commands.runOnce(() -> runFeeder()), + Commands.runOnce(() -> runTower()), // stop flywheel when finished Commands.runOnce(() -> setFlywheelVelocity(0))); } @@ -145,9 +155,7 @@ public Command runFlywheel() { public void simShoot() { if (Robot.robotContainer.intake.simBalls <= 0) return; - double flywheelSpeed = 6; Translation2d robotPose2d = Robot.robotContainer.drive.getPose().getTranslation(); - double Yaw = Robot.robotContainer.drive.getPose().getRotation().getRadians(); Pose3d robotPose3d = new Pose3d( new Translation3d(robotPose2d.getX(), robotPose2d.getY(), 0), @@ -157,10 +165,21 @@ public void simShoot() { new Translation3d(-0.0075, 0.0, 0.523), new Rotation3d(0, _hood.getPosition().in(Radians), 0)); + double flywheelSpeed = + _lflywheel.getVelocity().magnitude() + _rflywheel.getVelocity().magnitude(); + + double Yaw = Robot.robotContainer.drive.getPose().getRotation().getRadians(); double V_xy = Math.sin(Math.PI / 2 - (_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) * flywheelSpeed; + // ChassisSpeeds driveChassisSpeeds = Robot.robotContainer.drive.getChassisSpeeds(); + // Translation3d driveSpeed3d = new Translation3d( + // 0.0, + // 0.0, + // 0.0 + // ); + Robot.fuelSim.spawnFuel( robotPose3d .plus( @@ -175,24 +194,30 @@ public void simShoot() { V_xy * Math.sin(Yaw), Math.sin(Math.PI / 2 - (_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) * flywheelSpeed)); + Robot.robotContainer.intake.simBalls--; } public void periodic() { _hood.periodic(); - // _feeder.periodic(); - // _flywheel.periodic(); - - double pitch = - Math.toRadians( - Math.abs(Math.sin(Timer.getFPGATimestamp()) * 45)); // Placeholder for position + _feeder.periodic(); + _tower.periodic(); + _lflywheel.periodic(); + _rflywheel.periodic(); - // The pitch of the Rotation3D should be '_hood.getPosition().in(Radians)', change after fixing - // motor configs. Logger.recordOutput( "3DField/3_Hood", - new Pose3d(new Translation3d(-0.0075, 0.0, 0.523), new Rotation3d(0, pitch, 0))); + new Pose3d( + new Translation3d(-0.0075, 0.0, 0.523), + new Rotation3d(0, _hood.getPosition().in(Radians), 0))); + + // For testing purposes, raises the hood + // if (_hood.getPosition().in(Degrees) < ShooterRotaryConstants.MAX_ANGLE.in(Degrees) - 10) + // _hood.runVoltage(Volts.of(7)); - _hood.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); + // _hood.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 5)); + // _feeder.runVoltage(Volts.of(5)); + // _tower.runVoltage(Volts.of(5)); + // _flywheel.runVoltage(Volts.of(5)); } }