Skip to content

Commit

Permalink
More progress
Browse files Browse the repository at this point in the history
  • Loading branch information
@viggy96
viggy96 committed Apr 2, 2024
1 parent b3d3ae5 commit a427a03
Show file tree
Hide file tree
Showing 2 changed files with 27 additions and 22 deletions.
30 changes: 15 additions & 15 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -149,7 +149,7 @@ public static class Shooter {
),
false,
true,
Units.Degrees.of(0.5).in(Units.Radians),
Units.Degrees.of(0.3).in(Units.Radians),
0.40,
1.04,
true
Expand All @@ -159,19 +159,20 @@ public static class Shooter {
Units.DegreesPerSecond.of(360.0 * 10).per(Units.Second)
);
public static final List<Entry<Measure<Distance>,State>> SHOOTER_MAP = Arrays.asList(
Map.entry(Units.Meters.of(0.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(56.0))),
Map.entry(Units.Meters.of(1.50), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(56.0))),
Map.entry(Units.Meters.of(2.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(47.0))),
Map.entry(Units.Meters.of(2.50), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(40.0))),
Map.entry(Units.Meters.of(2.80), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(38.0))),
Map.entry(Units.Meters.of(3.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(37.5))),
Map.entry(Units.Meters.of(3.20), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(36.5))),
Map.entry(Units.Meters.of(3.50), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(35.0))),
Map.entry(Units.Meters.of(3.75), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(35.0))),
Map.entry(Units.Meters.of(4.00), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(32.0))),
Map.entry(Units.Meters.of(4.30), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(31.0))),
Map.entry(Units.Meters.of(4.50), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(30.5))),
Map.entry(Units.Meters.of(5.00), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(30.0)))
Map.entry(Units.Meters.of(0.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(53.0))),
Map.entry(Units.Meters.of(1.50), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(53.0))),
Map.entry(Units.Meters.of(2.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(44.0))),
Map.entry(Units.Meters.of(2.20), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(42.0))),
Map.entry(Units.Meters.of(2.50), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(37.0))),
Map.entry(Units.Meters.of(2.80), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(36.5))),
Map.entry(Units.Meters.of(3.00), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(34.0))),
Map.entry(Units.Meters.of(3.20), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(32.5))),
Map.entry(Units.Meters.of(3.50), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(30.5))),
Map.entry(Units.Meters.of(3.75), new State(Units.MetersPerSecond.of(15.0), Units.Degrees.of(31.0))),
Map.entry(Units.Meters.of(4.00), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(28.0))),
Map.entry(Units.Meters.of(4.30), new State(Units.MetersPerSecond.of(17.0), Units.Degrees.of(27.0))),
Map.entry(Units.Meters.of(4.50), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(26.5))),
Map.entry(Units.Meters.of(5.00), new State(Units.MetersPerSecond.of(17.5), Units.Degrees.of(26.0)))
);
}

Expand Down Expand Up @@ -205,7 +206,6 @@ public static class ShooterHardware {
public static final Spark.ID ANGLE_MOTOR_ID = new Spark.ID("ShooterHardware/Angle", 13);
public static final Spark.ID INDEXER_MOTOR_ID = new Spark.ID("ShooterHardware/Indexer", 14);
public static final LEDStrip.ID LED_STRIP_ID = new LEDStrip.ID("ShooterHardware/LEDStrip", 0, 200);

}

public static class VisionHardware {
Expand Down
19 changes: 12 additions & 7 deletions src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,8 +77,8 @@ public static class State {

public static final State AMP_PREP_STATE = new State(ZERO_FLYWHEEL_SPEED, Units.Degrees.of(56.0));
public static final State AMP_SCORE_STATE = new State(Units.MetersPerSecond.of(+3.1), Units.Degrees.of(56.0));
public static final State SPEAKER_PREP_STATE = new State(ZERO_FLYWHEEL_SPEED, Units.Degrees.of(56.0));
public static final State SPEAKER_SCORE_STATE = new State(Units.MetersPerSecond.of(+15.0), Units.Degrees.of(56.0));
public static final State SPEAKER_PREP_STATE = new State(ZERO_FLYWHEEL_SPEED, Units.Degrees.of(53.0));
public static final State SPEAKER_SCORE_STATE = new State(Units.MetersPerSecond.of(+15.0), Units.Degrees.of(53.0));
public static final State SOURCE_PREP_STATE = new State(ZERO_FLYWHEEL_SPEED, Units.Degrees.of(55.0));
public static final State SOURCE_INTAKE_STATE = new State(Units.MetersPerSecond.of(-10.0), Units.Degrees.of(55.0));
public static final State PASSING_STATE = new State(Units.MetersPerSecond.of(+15.0), Units.Degrees.of(45.0));
Expand Down Expand Up @@ -136,6 +136,7 @@ public State(Measure<Velocity<Distance>> speed, Measure<Angle> angle) {
private MechanismLigament2d m_simShooterJoint;
private TrapezoidProfile m_simShooterAngleMotionProfile;
private TrapezoidProfile.State m_simShooterAngleState;
private Measure<Distance> m_targetDistance;

/**
* Create an instance of ShooterSubsystem
Expand Down Expand Up @@ -170,6 +171,7 @@ public ShooterSubsystem(Hardware shooterHardware, SparkPIDConfig flywheelConfig,
this.m_poseSupplier = poseSupplier;
this.m_targetSupplier = targetSupplier;


// Initialize PID
m_topFlywheelMotor.initializeSparkPID(m_flywheelConfig, FeedbackSensor.NEO_ENCODER);
m_bottomFlywheelMotor.initializeSparkPID(m_flywheelConfig, FeedbackSensor.NEO_ENCODER);
Expand Down Expand Up @@ -346,11 +348,7 @@ private State getAutomaticState() {
* @return Distance to target
*/
private Measure<Distance> getTargetDistance() {
return Units.Meters.of(MathUtil.clamp(
m_poseSupplier.get().getTranslation().getDistance(m_targetSupplier.get().pose.toPose2d().getTranslation()),
MIN_SHOOTING_DISTANCE.in(Units.Meters),
MAX_SHOOTING_DISTANCE.in(Units.Meters)
));
return m_targetDistance;
}

/**
Expand Down Expand Up @@ -410,6 +408,13 @@ public void periodic() {
// Put note indicator on SmartDashboard
SmartDashboard.putBoolean(SHOOTER_NOTE_INSIDE_INDICATOR, isObjectPresent());

// Update target distance
m_targetDistance = Units.Meters.of(MathUtil.clamp(
m_poseSupplier.get().getTranslation().getDistance(m_targetSupplier.get().pose.toPose2d().getTranslation()),
MIN_SHOOTING_DISTANCE.in(Units.Meters),
MAX_SHOOTING_DISTANCE.in(Units.Meters)
));

// Log outputs
var currentState = getCurrentState();
Logger.recordOutput(getName() + MECHANISM_2D_LOG_ENTRY, m_mechanism2d);
Expand Down

0 comments on commit a427a03

Please sign in to comment.