Skip to content

Commit

Permalink
Add Speaker to constants
Browse files Browse the repository at this point in the history 
Finish devbot superstructure
  • Loading branch information
@suryatho
suryatho committed Feb 10, 2024
1 parent 64cfa03 commit b03c055
Show file tree
Hide file tree
Showing 6 changed files with 52 additions and 28 deletions.
10 changes: 5 additions & 5 deletions src/main/java/org/littletonrobotics/frc2024/FieldConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,11 +79,11 @@ public static final class Speaker {
new Translation3d(0.0, Units.inchesToMeters(197.765), Units.inchesToMeters(78.324));

/** Center of the speaker opening (blue alliance) */
// public static Translation2d centerSpeakerOpening =
// new Translation2d(topLeftSpeaker.getX() / 2.0, Units.inchesToMeters(241.56));

public static Translation2d centerSpeakerOpening =
new Translation2d(topLeftSpeaker.getX() / 2, fieldWidth - Units.inchesToMeters(104.0));
public static Translation3d centerSpeakerOpening =
new Translation3d(
topLeftSpeaker.getX() / 2.0,
fieldWidth - Units.inchesToMeters(104.0),
(bottomLeftSpeaker.getZ() + bottomRightSpeaker.getZ()) / 2.0);
}

public static double aprilTagWidth = Units.inchesToMeters(6.50);
Expand Down
25 changes: 9 additions & 16 deletions src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,9 @@
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIO;
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOKrakenFOC;
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOSim;
import org.littletonrobotics.frc2024.subsystems.superstructure.feeder.Feeder;
import org.littletonrobotics.frc2024.subsystems.superstructure.feeder.FeederIOSim;
import org.littletonrobotics.frc2024.subsystems.superstructure.feeder.FeederIOSparkFlex;
import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.Flywheels;
import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIO;
import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIOSim;
Expand All @@ -61,6 +64,7 @@ public class RobotContainer {
private Drive drive;
private AprilTagVision aprilTagVision;
private Flywheels flywheels;
private Feeder feeder;
private Intake intake;
private Arm arm;
private DevBotSuperstructure superstructure;
Expand All @@ -84,8 +88,8 @@ public RobotContainer() {
new ModuleIOSparkMax(DriveConstants.moduleConfigs[3]));
arm = new Arm(new ArmIOKrakenFOC());
flywheels = new Flywheels(new FlywheelsIOSparkFlex());
// intake = new Intake(new IntakeIOSparkMax());
superstructure = new DevBotSuperstructure(arm, flywheels);
feeder = new Feeder(new FeederIOSparkFlex());
superstructure = new DevBotSuperstructure(arm, flywheels, feeder);
}
case SIMBOT -> {
drive =
Expand All @@ -97,8 +101,9 @@ public RobotContainer() {
new ModuleIOSim(DriveConstants.moduleConfigs[3]));
arm = new Arm(new ArmIOSim());
flywheels = new Flywheels(new FlywheelsIOSim());
feeder = new Feeder(new FeederIOSim());
intake = new Intake(new IntakeIOSim());
superstructure = new DevBotSuperstructure(arm, flywheels);
superstructure = new DevBotSuperstructure(arm, flywheels, feeder);
}
case COMPBOT -> {
// No impl yet
Expand Down Expand Up @@ -208,24 +213,12 @@ private void configureButtonBindings() {

Trigger readyToShootTrigger =
new Trigger(() -> drive.isAutoAimGoalCompleted() && superstructure.atShootingSetpoint());
readyToShootTrigger
.whileTrue(
Commands.run(
() -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1.0)))
.whileFalse(
Commands.run(
() -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0)));
controller
.rightTrigger()
.and(readyToShootTrigger)
.onTrue(
Commands.runOnce(
() -> superstructure.setGoalState(DevBotSuperstructure.SystemState.SHOOT))
.andThen(Commands.waitSeconds(0.5))
.andThen(
Commands.runOnce(
() ->
superstructure.setGoalState(DevBotSuperstructure.SystemState.IDLE))));
() -> superstructure.setGoalState(DevBotSuperstructure.SystemState.SHOOT)));

controller
.leftBumper()
Expand Down
3 changes: 2 additions & 1 deletion src/main/java/org/littletonrobotics/frc2024/RobotState.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -149,8 +149,9 @@ public AimingParameters getAimingParameters() {
Pose2d robot = getEstimatedPose();
Twist2d fieldVelocity = fieldVelocity();

Translation2d originToGoal =
Translation3d originToGoal3d =
AllianceFlipUtil.apply(FieldConstants.Speaker.centerSpeakerOpening);
Translation2d originToGoal = new Translation2d(originToGoal3d.getX(), originToGoal3d.getY());
Translation2d originToRobot = robot.getTranslation();

// Get robot to goal angle but limit to reasonable range
Expand Down
1 change: 0 additions & 1 deletion src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -237,7 +237,6 @@ public void periodic() {
currentSetpoint =
setpointGenerator.generateSetpoint(
currentModuleLimits, currentSetpoint, desiredSpeeds, 0.02);

// run modules
SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4];
for (int i = 0; i < modules.length; i++) {
Expand Down
31 changes: 26 additions & 5 deletions ...in/java/org/littletonrobotics/frc2024/subsystems/superstructure/DevBotSuperstructure.java
View file
Original file line number Diff line number Diff line change
@@ -1,11 +1,15 @@
package org.littletonrobotics.frc2024.subsystems.superstructure;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import lombok.Getter;
import lombok.Setter;
import org.littletonrobotics.frc2024.FieldConstants;
import org.littletonrobotics.frc2024.RobotState;
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm;
import org.littletonrobotics.frc2024.subsystems.superstructure.feeder.Feeder;
import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.Flywheels;
import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
import org.littletonrobotics.junction.AutoLogOutput;
Expand Down Expand Up @@ -40,13 +44,15 @@ public enum SystemState {
@Getter @Setter private SystemState goalState = SystemState.IDLE;
private final Arm arm;
private final Flywheels flywheels;
private final Feeder feeder;

private final Timer followThroughTimer = new Timer();
private double followThroughArmAngle = 0.0;

public DevBotSuperstructure(Arm arm, Flywheels flywheels) {
public DevBotSuperstructure(Arm arm, Flywheels flywheels, Feeder feeder) {
this.arm = arm;
this.flywheels = flywheels;
this.feeder = feeder;
}

@Override
Expand Down Expand Up @@ -80,11 +86,26 @@ public void periodic() {
switch (currentState) {
case IDLE -> {
arm.setSetpoint(Rotation2d.fromDegrees(armIdleSetpointDegrees.get()));
flywheels.runVolts(0.0, 0.0);
flywheels.runVolts(4.0, 4.0);
feeder.stop();
}
case INTAKE -> {
arm.setSetpoint(Rotation2d.fromDegrees(armIntakeSetpointDegrees.get()));
flywheels.runVolts(-2.0, -2.0);
feeder.runVolts(-0.5);
}
case PREPARE_SHOOT -> {
var aimingParams = RobotState.getInstance().getAimingParameters();
arm.setSetpoint(
new Rotation2d(
aimingParams.effectiveDistance(),
FieldConstants.Speaker.centerSpeakerOpening.getZ()
+ Units.inchesToMeters(yCompensation.get())));
flywheels.setSetpointRpm(shootingLeftRPM.get(), shootingRightRPM.get());
}
case SHOOT -> {
feeder.runVolts(2.0);
}
case INTAKE -> {}
case PREPARE_SHOOT -> {}
case SHOOT -> {}
}

Logger.recordOutput("DevBotSuperstructure/GoalState", goalState);
Expand Down
10 changes: 10 additions & 0 deletions src/main/java/org/littletonrobotics/frc2024/util/AllianceFlipUtil.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import org.littletonrobotics.frc2024.FieldConstants;
Expand Down Expand Up @@ -47,6 +48,15 @@ public static Pose2d apply(Pose2d pose) {
}
}

public static Translation3d apply(Translation3d translation3d) {
if (shouldFlip()) {
return new Translation3d(
apply(translation3d.getX()), translation3d.getY(), translation3d.getZ());
} else {
return translation3d;
}
}

/**
* Flips a trajectory state to the correct side of the field based on the current alliance color.
*/
Expand Down

0 comments on commit b03c055

Please sign in to comment.