Skip to content
This repository has been archived by the owner on May 19, 2024. It is now read-only.

Commit

Permalink
wip
Browse files Browse the repository at this point in the history
  • Loading branch information
@haydenheroux
haydenheroux committed Apr 2, 2024
1 parent c15df18 commit ea96265
Show file tree
Hide file tree
Showing 5 changed files with 74 additions and 128 deletions.
6 changes: 5 additions & 1 deletion simgui.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,10 @@
"/Shuffleboard/Dashboard/Auto/Auto Chooser": "String Chooser",
"/Shuffleboard/Dashboard/Auto/SendableChooser[0]": "String Chooser",
"/Shuffleboard/Odometry/Field/Field": "Field2d",
"/Shuffleboard/Swerve/Module 0/Sync Steer": "Command",
"/Shuffleboard/Swerve/Module 1/Sync Steer": "Command",
"/Shuffleboard/Swerve/Module 2/Sync Steer": "Command",
"/Shuffleboard/Swerve/Module 3/Sync Steer": "Command",
"/Shuffleboard/Vision/Field": "Field2d",
"/SmartDashboard/Arm Mechanism": "Mechanism2d",
"/SmartDashboard/Mechanism": "Mechanism2d",
Expand Down Expand Up @@ -188,7 +192,7 @@
0.0,
0.8500000238418579
],
"height": 0,
"height": 338,
"series": [
{
"color": [
Expand Down
7 changes: 4 additions & 3 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,13 +78,14 @@ private void configureDefaultCommands() {
private void configureBindings() {
driverController.y().onTrue(odometry.tare());

operatorController.leftBumper().onTrue(superstructure.stow());
operatorController.leftTrigger().onTrue(superstructure.intake());

operatorController.rightBumper().onTrue(superstructure.spin());
operatorController.rightBumper().onTrue(superstructure.idle());
operatorController.rightTrigger().onTrue(superstructure.shoot());

operatorController.a().onTrue(superstructure.amp());
operatorController.x().onTrue(superstructure.stow());
operatorController.a().onTrue(superstructure.ampPosition());
operatorController.b().onTrue(superstructure.ampShoot());
}

/**
Expand Down
15 changes: 10 additions & 5 deletions src/main/java/frc/robot/shooter/ShooterConstants.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
package frc.robot.shooter;

import edu.wpi.first.math.filter.SlewRateLimiter;

/** Constants for the shooter subsystem. */
public class ShooterConstants {

Expand All @@ -8,11 +10,8 @@ public static class SerializerConstants {
/** Velocity to apply while intaking in rotations per second. */
public static final double INTAKE_VELOCITY = 4.75;

/** Velocity to apply while serializing for a speaker shot in rotations per second. */
public static final double SPEAKER_SERIALIZER_VELOCITY = 4.75;

/** Velocity to apply while serializing for an amp shot in rotations per second. */
public static final double AMP_SERIALIZE_VELOCITY = 2.5;
/** Velocity to apply while serializing in rotations per second. */
public static final double SERIALIZE_VELOCITY = 2.5;

/** Maximum speed in rotations per second. */
public static final double MAXIMUM_SPEED = 4.75;
Expand All @@ -23,6 +22,9 @@ public static class SerializerConstants {

/** Constants for the flywheel motor used in the shooter subsystem. */
public static class FlywheelConstants {
/** Velocity to apply while idly spinning up in rotations per second. */
public static final double IDLE_VELOCITY = 2.5;

/** Velocity to apply while shooting into the speaker in rotations per second. */
public static final double SPEAKER_VELOCITY = 5.65;

Expand All @@ -34,5 +36,8 @@ public static class FlywheelConstants {

/** Speed tolerance in rotations per second. */
public static final double SPEED_TOLERANCE = 0.25;

/** Acceleration limiter. */
public static final SlewRateLimiter ACCELERATION_LIMITER = new SlewRateLimiter(1.0);
}
}
40 changes: 17 additions & 23 deletions src/main/java/frc/robot/superstructure/Superstructure.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,7 @@
import frc.lib.Telemetry;
import frc.robot.arm.Arm;
import frc.robot.intake.Intake;
import frc.robot.intake.IntakeConstants.RollerConstants;
import frc.robot.shooter.Shooter;
import frc.robot.shooter.ShooterConstants.FlywheelConstants;
import frc.robot.shooter.ShooterConstants.SerializerConstants;

/** Subsystem class for the superstructure subsystem. */
public class Superstructure extends Subsystem {
Expand All @@ -37,10 +34,10 @@ private Superstructure() {
intake = Intake.getInstance();
shooter = Shooter.getInstance();

setPosition(SuperstructureState.INITIAL);
setPosition(SuperstructureState.STOW);

setpoint = SuperstructureState.INITIAL;
goal = SuperstructureState.INITIAL;
setpoint = SuperstructureState.STOW;
goal = SuperstructureState.STOW;
}

/**
Expand Down Expand Up @@ -130,6 +127,7 @@ private void updateMeasurement() {
measuredShoulderState,
measuredIntakeRollerVelocity,
measuredShooterFlywheelVelocity,
false,
measuredShooterSerializerVelocity);

SuperstructureMechanism.getInstance().updateSuperstructure(measurement);
Expand Down Expand Up @@ -165,7 +163,7 @@ public void setGoal(SuperstructureState goal) {
public boolean at(SuperstructureState goal) {
updateMeasurement();

return measurement.at(goal);
return measurement.atGoal(goal);
}

private Command to(SuperstructureState goal) {
Expand All @@ -177,30 +175,26 @@ public Command stow() {
}

public Command intake() {
return to(SuperstructureState.INTAKE)
return to(SuperstructureState.INTAKE_POSITION)
.andThen(
to(
SuperstructureState.INTAKE
.withSerializerVelocity(SerializerConstants.INTAKE_VELOCITY)
.withRollerVelocity(RollerConstants.INTAKE_VELOCITY)));
to(SuperstructureState.INTAKE));
}

public Command spin() {
return to(SuperstructureState.SHOOT.withFlywheelVelocity(FlywheelConstants.SPEAKER_VELOCITY));
public Command idle() {
return to(SuperstructureState.IDLE);
}

public Command shoot() {
return to(SuperstructureState.SHOOT.withFlywheelVelocity(FlywheelConstants.SPEAKER_VELOCITY))
return to(SuperstructureState.SPEAKER_SPIN)
.andThen(
to(
SuperstructureState.SHOOT
.withFlywheelVelocity(FlywheelConstants.SPEAKER_VELOCITY)
.withSerializerVelocity(SerializerConstants.SPEAKER_SERIALIZER_VELOCITY)));
to(SuperstructureState.SPEAKER_SHOOT));
}

public Command amp() {
return to(SuperstructureState.AMP)
.andThen(to(SuperstructureState.AMP_SHOOT).withTimeout(1.0))
.andThen(to(SuperstructureState.AMP_SERIALIZE));
public Command ampPosition() {
return to(SuperstructureState.AMP_POSITION);
}

public Command ampShoot() {
return ampPosition().andThen(to(SuperstructureState.AMP_SHOOT));
}
}
134 changes: 38 additions & 96 deletions src/main/java/frc/robot/superstructure/SuperstructureState.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,139 +15,70 @@ public record SuperstructureState(
State shoulderAngleRotations,
double rollerVelocityRotationsPerSecond,
double flywheelVelocityRotationsPerSecond,
boolean rampFlywheelVelocity,
double serializerVelocityRotationsPerSecond) {

public static final SuperstructureState INITIAL =
new SuperstructureState(ShoulderAngleConstants.INITIAL);

public static final SuperstructureState STOW =
new SuperstructureState(ShoulderAngleConstants.STOW);
new SuperstructureState(ShoulderAngleConstants.STOW, 0, 0, false, 0);

public static final SuperstructureState INTAKE_POSITION = new SuperstructureState(ShoulderAngleConstants.STOW, 0, 0, false, 0);

public static final SuperstructureState INTAKE =
new SuperstructureState(ShoulderAngleConstants.STOW);
new SuperstructureState(ShoulderAngleConstants.STOW, RollerConstants.INTAKE_VELOCITY, 0, false, SerializerConstants.INTAKE_VELOCITY);

public static final SuperstructureState IDLE =
new SuperstructureState(ShoulderAngleConstants.STOW, 0, FlywheelConstants.IDLE_VELOCITY, true, 0);

public static final SuperstructureState SPEAKER_SPIN = new SuperstructureState(ShoulderAngleConstants.STOW, 0, FlywheelConstants.SPEAKER_VELOCITY, false, 0);

public static final SuperstructureState SHOOT =
new SuperstructureState(ShoulderAngleConstants.STOW);
public static final SuperstructureState SPEAKER_SHOOT = new SuperstructureState(ShoulderAngleConstants.STOW, 0, FlywheelConstants.SPEAKER_VELOCITY, false, SerializerConstants.SERIALIZE_VELOCITY);

public static final SuperstructureState AMP = new SuperstructureState(ShoulderAngleConstants.AMP);
public static final SuperstructureState AMP_POSITION = new SuperstructureState(ShoulderAngleConstants.AMP, 0, 0, false, 0);

public static final SuperstructureState AMP_SHOOT =
AMP.withFlywheelVelocity(FlywheelConstants.AMP_VELOCITY);
public static final SuperstructureState AMP_SPIN = new SuperstructureState(ShoulderAngleConstants.AMP, 0, FlywheelConstants.AMP_VELOCITY, false, 0);

public static final SuperstructureState AMP_SERIALIZE =
AMP_SHOOT.withSerializerVelocity(SerializerConstants.AMP_SERIALIZE_VELOCITY);
public static final SuperstructureState AMP_SHOOT = new SuperstructureState(ShoulderAngleConstants.AMP, 0, FlywheelConstants.AMP_VELOCITY, false, SerializerConstants.SERIALIZE_VELOCITY);

/**
* Creates a new superstructure state.
*
* @param shoulderAngleRotations
* @param intakeRollerVelocityRotationsPerSecond
* @param rollerVelocityRotationsPerSecond
* @param flywheelVelocityRotationsPerSecond
* @param rampFlywheelVelocity
* @param serializerVelocityRotationsPerSecond
*/
public SuperstructureState {
Objects.requireNonNull(shoulderAngleRotations);
Objects.requireNonNull(rollerVelocityRotationsPerSecond);
Objects.requireNonNull(flywheelVelocityRotationsPerSecond);
Objects.requireNonNull(rampFlywheelVelocity);
Objects.requireNonNull(serializerVelocityRotationsPerSecond);
}

/**
* Creates a new superstructure state.
*
* @param shoulderAngle
* @param wristAngle
* @param rollerVelocityRotationsPerSecond
* @param flywheelVelocityRotationsPerSecond
* @param rampFlywheelVelocity
* @param serializerVelocityRotationsPerSecond
*/
public SuperstructureState(
Rotation2d shoulderAngle,
double rollerVelocityRotationsPerSecond,
double flywheelVelocityRotationsPerSecond,
boolean rampFlywheelVelocity,
double serializerVelocityRotationsPerSecond) {
this(
new State(shoulderAngle.getRotations(), 0.0),
rollerVelocityRotationsPerSecond,
flywheelVelocityRotationsPerSecond,
serializerVelocityRotationsPerSecond);
}

/**
* Creates a new superstructure state.
*
* @param shoulderAngle
* @param wristAngle
*/
public SuperstructureState(Rotation2d shoulderAngle) {
this(shoulderAngle, 0.0, 0.0, 0.0);
}

public SuperstructureState withShoulderAngle(Rotation2d newShoulderAngle) {
return new SuperstructureState(
new State(newShoulderAngle.getRotations(), 0.0),
new State(shoulderAngle.getRotations(), 0),
rollerVelocityRotationsPerSecond,
flywheelVelocityRotationsPerSecond,
rampFlywheelVelocity,
serializerVelocityRotationsPerSecond);
}

public SuperstructureState withShoulderAngleOf(SuperstructureState other) {
return new SuperstructureState(
other.shoulderAngleRotations,
rollerVelocityRotationsPerSecond,
flywheelVelocityRotationsPerSecond,
serializerVelocityRotationsPerSecond);
}

public SuperstructureState withRollerVelocity(double newRollerVelocityRotationsPerSecond) {
return new SuperstructureState(
shoulderAngleRotations,
newRollerVelocityRotationsPerSecond,
flywheelVelocityRotationsPerSecond,
serializerVelocityRotationsPerSecond);
}

public SuperstructureState withRollerVelocityOf(SuperstructureState other) {
return new SuperstructureState(
shoulderAngleRotations,
other.rollerVelocityRotationsPerSecond,
flywheelVelocityRotationsPerSecond,
serializerVelocityRotationsPerSecond);
}

public SuperstructureState withFlywheelVelocity(double newFlywheelVelocityRotationsPerSecond) {
return new SuperstructureState(
shoulderAngleRotations,
rollerVelocityRotationsPerSecond,
newFlywheelVelocityRotationsPerSecond,
serializerVelocityRotationsPerSecond);
}

public SuperstructureState withFlywheelVelocityOf(SuperstructureState other) {
return new SuperstructureState(
shoulderAngleRotations,
rollerVelocityRotationsPerSecond,
other.flywheelVelocityRotationsPerSecond,
serializerVelocityRotationsPerSecond);
}

public SuperstructureState withSerializerVelocity(
double newSerializerVelocityRotationsPerSecond) {
return new SuperstructureState(
shoulderAngleRotations,
rollerVelocityRotationsPerSecond,
flywheelVelocityRotationsPerSecond,
newSerializerVelocityRotationsPerSecond);
}

public SuperstructureState withSerializerVelocityOf(SuperstructureState other) {
return new SuperstructureState(
shoulderAngleRotations,
rollerVelocityRotationsPerSecond,
flywheelVelocityRotationsPerSecond,
other.serializerVelocityRotationsPerSecond);
}

/**
* Returns true if at the shoulder angle goal.
*
Expand Down Expand Up @@ -200,11 +131,17 @@ public boolean atSerializerVelocityGoal(SuperstructureState goal) {
SerializerConstants.SPEED_TOLERANCE);
}

public boolean at(SuperstructureState other) {
return atShoulderAngleGoal(other)
&& atRollerVelocityGoal(other)
&& atFlywheelVelocityGoal(other)
&& atSerializerVelocityGoal(other);
/**
* Returns true if at the superstructure goal.
*
* @param goal
* @return true if at the superstructure goal.
*/
public boolean atGoal(SuperstructureState goal) {
return atShoulderAngleGoal(goal)
&& atRollerVelocityGoal(goal)
&& atFlywheelVelocityGoal(goal)
&& atSerializerVelocityGoal(goal);
}

/**
Expand All @@ -222,10 +159,15 @@ public static SuperstructureState nextSetpoint(
setpoint.shoulderAngleRotations(),
goal.shoulderAngleRotations());

double accelerationLimitedFlywheelVelocity = FlywheelConstants.ACCELERATION_LIMITER.calculate(goal.flywheelVelocityRotationsPerSecond());

double nextFlywheelVelocitySetpoint = goal.rampFlywheelVelocity() ? accelerationLimitedFlywheelVelocity : goal.flywheelVelocityRotationsPerSecond();

return new SuperstructureState(
nextShoulderSetpoint,
goal.rollerVelocityRotationsPerSecond(),
goal.flywheelVelocityRotationsPerSecond(),
nextFlywheelVelocitySetpoint,
goal.rampFlywheelVelocity(),
goal.serializerVelocityRotationsPerSecond());
}
}

0 comments on commit ea96265

Please sign in to comment.