Skip to content

Commit

Permalink
more shooter data added
Browse files Browse the repository at this point in the history
  • Loading branch information
@GearBoxFox
GearBoxFox committed Mar 9, 2024
1 parent 93da5fa commit 9120c53
Show file tree
Hide file tree
Showing 6 changed files with 53 additions and 28 deletions.
2 changes: 2 additions & 0 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
16 changes: 7 additions & 9 deletions src/main/java/frc/robot/commands/IntakeControlCommand.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,42 +16,40 @@ 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);
}

@Override
public void initialize() {

hasPieceRising = false;
m_timer.reset();
}

@Override
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);
m_shooterSubsystem.setShooterPowerLeft(0.0);
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);
Expand Down
7 changes: 5 additions & 2 deletions src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
14 changes: 7 additions & 7 deletions src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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) {
Expand Down
20 changes: 14 additions & 6 deletions src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java
View file
Original file line number Diff line number Diff line change
@@ -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;
Expand All @@ -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();
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
}
Expand Down Expand Up @@ -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);
}
}
22 changes: 18 additions & 4 deletions src/main/java/lib/utils/AimbotUtils.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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*/
Expand Down

0 comments on commit 9120c53

Please sign in to comment.