Skip to content

Commit

Permalink
Merge from main
Browse files Browse the repository at this point in the history
  • Loading branch information
@manthan-acharya
manthan-acharya committed Feb 13, 2024
2 parents ab8f016 + bc9936d commit eec4384
Show file tree
Hide file tree
Showing 20 changed files with 527 additions and 487 deletions.
190 changes: 75 additions & 115 deletions src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,10 @@
import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIO;
import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIONorthstar;
import org.littletonrobotics.frc2024.subsystems.drive.*;
import org.littletonrobotics.frc2024.subsystems.flywheels.Flywheels;
import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIO;
import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIOSim;
import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIOSparkFlex;
import org.littletonrobotics.frc2024.subsystems.rollers.Rollers;
import org.littletonrobotics.frc2024.subsystems.rollers.RollersSensorsIO;
import org.littletonrobotics.frc2024.subsystems.rollers.RollersSensorsIOReal;
Expand All @@ -48,10 +52,6 @@
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.flywheels.Flywheels;
import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIO;
import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIOSim;
import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIOSparkFlex;
import org.littletonrobotics.frc2024.util.AllianceFlipUtil;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;

Expand Down Expand Up @@ -102,9 +102,8 @@ public RobotContainer() {
intake = new Intake(new IntakeIOKrakenFOC());
rollers = new Rollers(feeder, indexer, intake, new RollersSensorsIOReal());

arm = new Arm(new ArmIOKrakenFOC());
flywheels = new Flywheels(new FlywheelsIOSparkFlex());
superstructure = new Superstructure(arm, flywheels);
arm = new Arm(new ArmIOKrakenFOC());

aprilTagVision =
new AprilTagVision(
Expand All @@ -125,8 +124,6 @@ public RobotContainer() {
new ModuleIOSim(DriveConstants.moduleConfigs[3]));
arm = new Arm(new ArmIOSim());
flywheels = new Flywheels(new FlywheelsIOSim());
// intake = new Intake(new IntakeIOSim());
// feeder = new Feeder(new FeederIOSim());
superstructure = new Superstructure(arm, flywheels);
}
case COMPBOT -> {
Expand Down Expand Up @@ -202,32 +199,11 @@ public RobotContainer() {
flywheels,
flywheels::runRightCharacterizationVolts,
flywheels::getRightCharacterizationVelocity));
// autoChooser.addOption("Arm get static current", arm.getStaticCurrent());
autoChooser.addOption("Arm get static current", arm.getStaticCurrent());

AutoCommands autoCommands = new AutoCommands(drive, superstructure);

autoChooser.addOption("Davis Auto", autoCommands.davisEthicalAuto());

// Testing autos paths
// Function<File, Optional<Command>> trajectoryCommandFactory =
// trajectoryFile -> {
// Optional<HolonomicTrajectory> trajectory =
// ChoreoTrajectoryDeserializer.deserialize(trajectoryFile);
// return trajectory.map(
// traj ->
// Commands.runOnce(
// () ->
// robotState.resetPose(AllianceFlipUtil.apply(traj.getStartPose())))
// .andThen(Commands.runOnce(() ->
// drive.setTrajectoryGoal(traj)))
// .until(drive::isTrajectoryGoalCompleted));
// };
// final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo");
// for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) {
// trajectoryCommandFactory
// .apply(trajectoryFile)
// .ifPresent(command -> autoChooser.addOption(trajectoryFile.getName(), command));
// }
// autoChooser.addOption("Drive Straight", autoCommands.driveStraight());

// Configure the button bindings
configureButtonBindings();
Expand All @@ -239,6 +215,7 @@ public RobotContainer() {
* XboxController}), and then passing it to a {@link JoystickButton}.
*/
private void configureButtonBindings() {
// Drive Commands
drive.setDefaultCommand(
drive.run(
() ->
Expand All @@ -250,90 +227,73 @@ private void configureButtonBindings() {
.onTrue(Commands.runOnce(drive::setAutoAimGoal))
.onFalse(Commands.runOnce(drive::clearAutoAimGoal));

if (superstructure != null) {
controller
.a()
.onTrue(
Commands.runOnce(
() -> superstructure.setGoalState(Superstructure.SystemState.PREPARE_SHOOT)))
.onFalse(
Commands.runOnce(() -> superstructure.setGoalState(Superstructure.SystemState.IDLE)));

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(Superstructure.SystemState.SHOOT);
rollers.setGoal(Rollers.Goal.FEED_SHOOTER);
},
superstructure,
rollers)
.andThen(Commands.waitSeconds(1.0))
.andThen(
Commands.runOnce(
() -> {
superstructure.setGoalState(Superstructure.SystemState.IDLE);
rollers.setGoal(Rollers.Goal.IDLE);
})));

controller
.leftTrigger()
.whileTrue(
Commands.runOnce(
() -> superstructure.setGoalState(Superstructure.SystemState.INTAKE),
superstructure)
.andThen(
Commands.waitSeconds(0.25),
Commands.runOnce(() -> rollers.setGoal(Rollers.Goal.FLOOR_INTAKE), rollers),
Commands.idle())
.finallyDo(
() -> {
rollers.setGoal(Rollers.Goal.IDLE);
superstructure.setGoalState(Superstructure.SystemState.IDLE);
}));

controller
.leftBumper()
.whileTrue(
Commands.runOnce(
() -> superstructure.setGoalState(Superstructure.SystemState.INTAKE),
superstructure)
.andThen(
Commands.waitSeconds(0.25),
Commands.runOnce(() -> rollers.setGoal(Rollers.Goal.EJECT_TO_FLOOR), rollers),
Commands.idle())
.finallyDo(
() -> {
rollers.setGoal(Rollers.Goal.IDLE);
superstructure.setGoalState(Superstructure.SystemState.IDLE);
}));

// controller
// .a()
// .onTrue(
// Commands.runOnce(
// () -> {
// superstructure.setGoalState(Superstructure.SystemState.REVERSE_INTAKE);
// rollers.setGoal(Rollers.Goal.STATION_INTAKE);
// }))
// .onFalse(
// Commands.runOnce(
// () -> {
// superstructure.setGoalState(Superstructure.SystemState.IDLE);
// rollers.setGoal(Rollers.Goal.IDLE);
// }));
}
controller
.a()
.onTrue(
Commands.runOnce(
() -> superstructure.setGoalState(Superstructure.SystemState.PREPARE_SHOOT)))
.onFalse(
Commands.runOnce(() -> superstructure.setGoalState(Superstructure.SystemState.IDLE)));

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(Superstructure.SystemState.SHOOT);
rollers.setGoal(Rollers.Goal.FEED_SHOOTER);
},
superstructure,
rollers)
.andThen(Commands.waitSeconds(1.0))
.andThen(
Commands.runOnce(
() -> {
superstructure.setGoalState(Superstructure.SystemState.IDLE);
rollers.setGoal(Rollers.Goal.IDLE);
})));

controller
.leftTrigger()
.whileTrue(
Commands.runOnce(
() -> superstructure.setGoalState(Superstructure.SystemState.INTAKE),
superstructure)
.andThen(
Commands.waitSeconds(0.25),
Commands.runOnce(() -> rollers.setGoal(Rollers.Goal.FLOOR_INTAKE), rollers),
Commands.idle())
.finallyDo(
() -> {
rollers.setGoal(Rollers.Goal.IDLE);
superstructure.setGoalState(Superstructure.SystemState.IDLE);
}));

controller
.leftBumper()
.whileTrue(
Commands.runOnce(
() -> superstructure.setGoalState(Superstructure.SystemState.INTAKE),
superstructure)
.andThen(
Commands.waitSeconds(0.25),
Commands.runOnce(() -> rollers.setGoal(Rollers.Goal.EJECT_TO_FLOOR), rollers),
Commands.idle())
.finallyDo(
() -> {
rollers.setGoal(Rollers.Goal.IDLE);
superstructure.setGoalState(Superstructure.SystemState.IDLE);
}));

controller
.y()
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/org/littletonrobotics/frc2024/RobotState.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@
import java.util.NoSuchElementException;
import lombok.experimental.ExtensionMethod;
import org.littletonrobotics.frc2024.subsystems.drive.DriveConstants;
import org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants;
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmConstants;
import org.littletonrobotics.frc2024.util.AllianceFlipUtil;
import org.littletonrobotics.frc2024.util.GeomUtil;
import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
Expand Down Expand Up @@ -185,9 +185,9 @@ public AimingParameters getAimingParameters() {
new AimingParameters(
targetVehicleDirection,
new Rotation2d(
targetDistance - SuperstructureConstants.ArmConstants.armOrigin.getX(),
targetDistance - ArmConstants.armOrigin.getX(),
FieldConstants.Speaker.centerSpeakerOpening.getZ()
- SuperstructureConstants.ArmConstants.armOrigin.getY()
- ArmConstants.armOrigin.getY()
+ shotHeightCompensation.get()),
feedVelocity);
Logger.recordOutput("RobotState/AimingParameters/Direction", latestParameters.driveHeading);
Expand Down
23 changes: 23 additions & 0 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,23 @@
package org.littletonrobotics.frc2024.subsystems.flywheels;

import org.littletonrobotics.frc2024.Constants;

public class FlywheelConstants {
public static Config config =
switch (Constants.getRobot()) {
case COMPBOT -> null;
case DEVBOT -> new Config(5, 4, (1.0 / 2.0), 100.0);
case SIMBOT -> new Config(0, 0, (1.0 / 2.0), 100.0);
};

public static Gains gains =
switch (Constants.getRobot()) {
case COMPBOT -> null;
case DEVBOT -> new Gains(0.0006, 0.0, 0.05, 0.33329, 0.00083, 0.0);
case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.09078, 0.00103, 0.0);
};

public record Config(int leftID, int rightID, double reduction, double toleranceRPM) {}

public record Gains(double kP, double kI, double kD, double kS, double kV, double kA) {}
}
Loading

0 comments on commit eec4384

Please sign in to comment.