diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3ae6b9eb..6627d9fa 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -237,6 +237,8 @@ private ArmSetpoints() { public static final ArmPose AMP_SETPOINT = new ArmPose("ArmPoses/Amp", true, 94.0, 145.0); + public static final ArmPose STATIC_SHOOTER = new ArmPose(0.0, 55.0); + public static final GosDoubleProperty WRIST_ANGLE = new GosDoubleProperty(false, "Wrist Angle", 45.0); public static final Trajectory STOW_AMP_TRAJ; diff --git a/src/main/java/frc/robot/commands/IntakeControlCommand.java b/src/main/java/frc/robot/commands/IntakeControlCommand.java index 2e8e0d67..630aff06 100644 --- a/src/main/java/frc/robot/commands/IntakeControlCommand.java +++ b/src/main/java/frc/robot/commands/IntakeControlCommand.java @@ -16,10 +16,7 @@ public class IntakeControlCommand extends Command { public IntakeControlCommand(ArmSubsystem armSubsystem, ShooterSubsystem shooterSubsystem) { m_armSubsystem = armSubsystem; m_shooterSubsystem = shooterSubsystem; - m_timer = new Timer(); - hasPieceRising = false; - // each subsystem used by the command must be passed into the // addRequirements() method (which takes a vararg of Subsystem) addRequirements(m_armSubsystem, m_shooterSubsystem); @@ -27,7 +24,8 @@ public IntakeControlCommand(ArmSubsystem armSubsystem, ShooterSubsystem shooterS @Override public void initialize() { - + hasPieceRising = false; + m_timer.reset(); } @Override @@ -35,11 +33,11 @@ public void execute() { if(!m_shooterSubsystem.hasPiece() && !hasPieceRising) { // run intake and kicker wheels in m_shooterSubsystem.setIntakePower(0.95); - m_shooterSubsystem.setKickerPower(0.5); + m_shooterSubsystem.setKickerPower(0.35); m_shooterSubsystem.setShooterPowerLeft(-0.1); m_shooterSubsystem.setShooterPowerRight(-0.1); m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.INTAKE); - } else if (m_shooterSubsystem.hasPiece() && !hasPieceRising) { + } else if (m_shooterSubsystem.hasPiece() && !m_timer.hasElapsed(0.0001)) { // stop running intake m_shooterSubsystem.setIntakePower(0.0); m_shooterSubsystem.setKickerPower(0.0); @@ -47,11 +45,11 @@ public void execute() { m_shooterSubsystem.setShooterPowerRight(0.0); // piece detected, mark as we have a piece and start moving up - hasPieceRising = true; + m_timer.start(); m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.STOW); - } else if (hasPieceRising && m_armSubsystem.bothAtSetpoint() && m_shooterSubsystem.hasPiece()) { + } else if (!m_timer.hasElapsed(0.075) && m_armSubsystem.bothAtSetpoint()) { // arm is up, haven't run kickers back yet - m_shooterSubsystem.setKickerPower(-0.25); + m_shooterSubsystem.setKickerPower(-0.2); } else { m_shooterSubsystem.setIntakePower(0.0); m_shooterSubsystem.setKickerPower(0.0); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index 7d87abc7..347b8841 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -127,9 +127,12 @@ public void handleState() { m_wristVelocityMult = 1.0; m_desiredArmPoseDegs = ArmConstants.WRIST_ARM_GAP.getValue() - m_desiredWristPoseDegs; - m_desiredArmPoseDegs = m_desiredArmPoseDegs >= ArmConstants.ARM_LOWER_LIMIT.getValue() ? m_desiredArmPoseDegs - : ArmConstants.ARM_LOWER_LIMIT.getValue(); + m_desiredArmPoseDegs = m_desiredArmPoseDegs >= 0 ? m_desiredArmPoseDegs + : 0; m_desiredWristPoseDegs = ArmSetpoints.WRIST_ANGLE.getValue(); + +// m_desiredArmPoseDegs = ArmSetpoints.STATIC_SHOOTER.armAngle(); +// m_desiredWristPoseDegs = ArmSetpoints.STATIC_SHOOTER.wristAngle(); } case INTAKE -> { m_armVelocityMult = 1.0; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java index 34c05f69..02080b70 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java @@ -21,7 +21,7 @@ public class ShooterIOKraken implements ShooterIO { private final TalonFX m_intake; private final TalonFX m_indexer; -// private final TimeOfFlight m_tof; + private final TimeOfFlight m_tof; private final PidPropertyPublic m_leftProperty; private final PidPropertyPublic m_rightProperty; @@ -57,8 +57,8 @@ public ShooterIOKraken() { m_intake = new TalonFX(ShooterConstants.INTAKE_ID, canbus); m_indexer = new TalonFX(ShooterConstants.INDEXER_ID, canbus); -// m_tof = new TimeOfFlight(28); -// m_tof.setRangingMode(TimeOfFlight.RangingMode.Short, 25); + m_tof = new TimeOfFlight(28); + m_tof.setRangingMode(TimeOfFlight.RangingMode.Short, 25); // general motor configs TalonFXConfiguration shooterConfig = new TalonFXConfiguration(); @@ -194,10 +194,10 @@ public void updateInputs(ShooterIOInputs inputs) { m_rightProperty.updateIfChanged(); } -// @Override -// public boolean hasPiece() { -// return m_tof.getRange() < 60; -// } + @Override + public boolean hasPiece() { + return m_tof.getRange() < 60; + } @Override public void setMotorVoltageTL(double voltage) { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index c3e7ce50..1ac9afd5 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.shooter; +import com.gos.lib.properties.GosDoubleProperty; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -14,6 +15,11 @@ public class ShooterSubsystem extends SubsystemBase { private final DataLogTable m_logTable = DataLogUtil.getTable("Shooter"); + private final GosDoubleProperty m_leftPower = new + GosDoubleProperty(false, "Shooter/Left RPM", 3600); + private final GosDoubleProperty m_rightPower = new + GosDoubleProperty(false, "Shooter/Right RPM", 3600); + public ShooterSubsystem(ShooterIO io) { m_io = io; m_inputs = new ShooterIO.ShooterIOInputs(); @@ -47,11 +53,11 @@ public void setIntakePower(double power) { public Command runShooterVelocity(boolean runKicker) { return runEnd(() -> { - m_io.setLeftVelocityRpm(0.0); - m_io.setRightVelocityRpm(0.0); + m_io.setLeftVelocityRpm(m_leftPower.getValue()); + m_io.setRightVelocityRpm(m_rightPower.getValue()); if (runKicker) { - m_io.setKickerVoltage(3.0); + m_io.setKickerVoltage(12.0); m_io.setIntakeVoltage(0.05); } else { m_io.setKickerVoltage(0.0); @@ -83,11 +89,11 @@ public Command intakeCommand(double intakePower, double kickerPower, double time setKickerPower(kickerPower); timer.restart(); } else if (!timer.hasElapsed(timeout)) { - setShooterPowerLeft(0.0); - setShooterPowerRight(0.0); - setKickerPower(-0.25); + setKickerPower(-0.1); setIntakePower(0.0); } else { + setShooterPowerRight(0.0); + setShooterPowerLeft(0.0); setKickerPower(0.0); setIntakePower(0.0); } @@ -119,5 +125,7 @@ public void setupLogging() { m_logTable.addDouble("LeftTemperature", () -> m_inputs.tlTemperature, false); m_logTable.addDouble("RightTemperature", () -> m_inputs.trTemperature, false); + + m_logTable.addDouble("IntakeCurrent", () -> m_inputs.intakeCurrentDraw, true); } } diff --git a/src/main/java/lib/utils/AimbotUtils.java b/src/main/java/lib/utils/AimbotUtils.java index 44766f2d..1459534b 100644 --- a/src/main/java/lib/utils/AimbotUtils.java +++ b/src/main/java/lib/utils/AimbotUtils.java @@ -9,18 +9,32 @@ public class AimbotUtils { private static final InterpolatingDoubleTreeMap m_angleLerpTable = new InterpolatingDoubleTreeMap(); - private static final InterpolatingDoubleTreeMap m_speedLerpTable = new InterpolatingDoubleTreeMap(); + private static final InterpolatingDoubleTreeMap m_leftSpeedLerpTable = new InterpolatingDoubleTreeMap(); + private static final InterpolatingDoubleTreeMap m_rightSpeedLerpTable = new InterpolatingDoubleTreeMap(); static { // angle measurements, meters -> degrees m_angleLerpTable.put(Units.inchesToMeters(18.0), 50.0); m_angleLerpTable.put(Units.inchesToMeters(46.0), 40.0); m_angleLerpTable.put(Units.inchesToMeters(97.0), 31.5); + m_angleLerpTable.put(Units.inchesToMeters(116.0), 29.75); + m_angleLerpTable.put(Units.inchesToMeters(155.0), 24.65); + m_angleLerpTable.put(Units.inchesToMeters(229.0), 0.0); //flywheel measurements, meters -> RPM - m_speedLerpTable.put(Units.inchesToMeters(18.0), 3600.0); - m_speedLerpTable.put(Units.inchesToMeters(46.0), 3600.0); - m_speedLerpTable.put(Units.inchesToMeters(97.0), 3600.0); + m_leftSpeedLerpTable.put(Units.inchesToMeters(18.0), 3600.0); + m_leftSpeedLerpTable.put(Units.inchesToMeters(46.0), 3600.0); + m_leftSpeedLerpTable.put(Units.inchesToMeters(97.0), 3600.0); + m_leftSpeedLerpTable.put(Units.inchesToMeters(116.0), 3800.0); + m_leftSpeedLerpTable.put(Units.inchesToMeters(155.0), 5000.0); + m_leftSpeedLerpTable.put(Units.inchesToMeters(229.0), 0.0); + + m_rightSpeedLerpTable.put(Units.inchesToMeters(18.0), 3600.0); + m_rightSpeedLerpTable.put(Units.inchesToMeters(46.0), 3600.0); + m_rightSpeedLerpTable.put(Units.inchesToMeters(97.0), 3600.0); + m_rightSpeedLerpTable.put(Units.inchesToMeters(116.0), 3800.0); + m_rightSpeedLerpTable.put(Units.inchesToMeters(155.0), 4600.0); + m_rightSpeedLerpTable.put(Units.inchesToMeters(229.0), 0.0); } /** Gets the top point of the shooter for checking limits*/