Skip to content
This repository was archived by the owner on Dec 25, 2024. It is now read-only.

Work from today #564

Merged
Coolearthsky merged 3 commits into from
Dec 12, 2024
Merged

Work from today #564

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
ebc5708
Work from today
Coolearthsky Dec 3, 2024
22f228b
Merge branch 'main' into visualServoing
Coolearthsky Dec 3, 2024
ae75bfc
Fix to codebase for 2025
Coolearthsky Dec 12, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
201 changes: 5 additions & 196 deletions comp/swerve100/src/main/java/org/team100/frc2024/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,26 +1,10 @@
package org.team100.frc2024;

import java.io.IOException;
import java.util.Map;
import java.util.function.BooleanSupplier;

import org.team100.frc2024.commands.AutonCommand;
import org.team100.frc2024.commands.Feed;
import org.team100.frc2024.commands.Lob;
import org.team100.frc2024.commands.drivetrain.manual.ManualWithAmpLock;
import org.team100.frc2024.commands.drivetrain.manual.ManualWithShooterLock;
import org.team100.frc2024.config.AutonChooser;
import org.team100.frc2024.motion.AutoMaker;
import org.team100.frc2024.motion.FeederSubsystem;
import org.team100.frc2024.motion.ShootSmartWithRotation;
import org.team100.frc2024.motion.drivetrain.manual.AmpLockCommand;
import org.team100.frc2024.motion.intake.Intake;
import org.team100.frc2024.motion.shooter.DrumShooter;
import org.team100.frc2024.motion.shooter.Ramp;
import org.team100.frc2024.motion.shooter.TestShoot;
import org.team100.lib.async.Async;
import org.team100.lib.async.AsyncFactory;
import org.team100.lib.commands.AllianceCommand;
import org.team100.lib.commands.drivetrain.DriveToPoseSimple;
import org.team100.lib.commands.drivetrain.DriveWithProfileRotation;
import org.team100.lib.commands.drivetrain.FancyTrajectory;
Expand All @@ -34,7 +18,6 @@
import org.team100.lib.commands.drivetrain.manual.ManualWithFullStateHeading;
import org.team100.lib.commands.drivetrain.manual.ManualWithNoteRotation;
import org.team100.lib.commands.drivetrain.manual.ManualWithProfiledHeading;
import org.team100.lib.commands.drivetrain.manual.ManualWithTargetLock;
import org.team100.lib.commands.drivetrain.manual.SimpleManualModuleStates;
import org.team100.lib.config.Identity;
import org.team100.lib.controller.drivetrain.FullStateDriveController;
Expand All @@ -52,7 +35,6 @@
import org.team100.lib.hid.DriverControlProxy;
import org.team100.lib.hid.OperatorControl;
import org.team100.lib.hid.OperatorControlProxy;
import org.team100.lib.indicator.LEDIndicator;
import org.team100.lib.localization.AprilTagFieldLayoutWithCorrectOrientation;
import org.team100.lib.localization.NotePosition24ArrayListener;
import org.team100.lib.localization.SwerveDrivePoseEstimator100;
Expand All @@ -77,12 +59,9 @@
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.RepeatCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.Trigger;
Expand All @@ -99,8 +78,7 @@ public class RobotContainer implements Glassy {
private static final double kDriveStatorLimit = 100;

private final SwerveModuleCollection m_modules;
private final Command m_auton;
private final DrumShooter m_shooter;
private final Command m_auton = null;
final SwerveDriveSubsystem m_drive;

public RobotContainer(TimedRobot100 robot) throws IOException {
Expand Down Expand Up @@ -184,30 +162,6 @@ public RobotContainer(TimedRobot100 robot) throws IOException {

final LoggerFactory sysLog = logger.child("Subsystems");

final FeederSubsystem feeder = new FeederSubsystem(sysLog, m_sensors);

final Intake intake = new Intake(sysLog, m_sensors);

m_shooter = new DrumShooter(sysLog, 3, 13, 27, 58, 100);

// final ClimberSubsystem climber = new ClimberSubsystem(sysLog, 60, 61);

// final AmpFeeder m_ampFeeder = new AmpFeeder(sysLog);
// final AmpPivot m_ampPivot = new AmpPivot(sysLog);

///////////////////////////
//
// LEDS
//

final LEDIndicator ledIndicator = new LEDIndicator(0);
// has no default command, registers its own periodic.
new LEDSubsystem(
ledIndicator,
m_sensors,
m_shooter,
visionDataProvider);

// m_ampFeeder = new AmpFeeder(sysLog);
// m_ampPivot = new AmpPivot(sysLog);

Expand Down Expand Up @@ -253,11 +207,12 @@ public RobotContainer(TimedRobot100 robot) throws IOException {
final DriveTrajectoryFollower drivePID = driveControllerFactory.goodPIDF(PIDFlog);

whileTrue(driverControl::driveToNote,
new ParallelDeadlineGroup(new DriveWithProfileRotation(
new DriveWithProfileRotation(
fieldLog,
noteListener::getClosestTranslation2d,
m_drive,
holonomicController,
swerveKinodynamics), intake.run(intake::intakeSmart)));
swerveKinodynamics));
// try the new mintime controller
final HolonomicFieldRelativeController minTimeController = new MinTimeDriveController(comLog, hlog);
whileTrue(driverControl::actualCircle,
Expand All @@ -269,18 +224,6 @@ public RobotContainer(TimedRobot100 robot) throws IOException {
// whileTrue(driverControl::actualCircle,
// new DriveInACircle(comLog, m_drive, controller, -1, viz));

// whileTrue(driverControl::driveToAmp,
// new DriveToAmp(
// m_drive,
// holonomicController,
// swerveKinodynamics,
// m_ampPivot,
// m_ampFeeder,
// intake,
// m_shooter,
// feeder));


///////////////////////
//
// for testing odometry
Expand All @@ -306,44 +249,6 @@ public RobotContainer(TimedRobot100 robot) throws IOException {
new OscillateProfile(m_drive, hp, hcontroller, 1),
new OscillateProfile(m_drive, hp, hcontroller, -1))));

// new RepeatCommand(
// new SequentialCommandGroup(
// new OscillatePosition(driveLog, m_drive, maker, controller, 1, viz),
// new OscillatePosition(driveLog, m_drive, maker, controller, -1, viz))));

// whileTrue(driverControl::fullCycle, new OscillateDirect(comLog, m_drive));
// new Oscillate(comLog, m_drive));
// new RepeatCommand(
// new FullCycle(comLog, m_drive, controller, viz)));

// whileTrue(operatorControl::intake,
// new RunIntakeAndAmpFeeder(intake, feeder, m_ampFeeder));

// whileTrue(operatorControl::outtake,
// new OuttakeCommand(intake, m_shooter, m_ampFeeder, feeder));

whileTrue(operatorControl::ramp, new Ramp(m_shooter, m_drive));

whileTrue(operatorControl::feed, new Feed(intake, feeder));

// fast, then slow.
// whileTrue(operatorControl::pivotToAmpPosition,
// new AmpFastThenSlow(m_ampPivot, 1.7, 1.8));

// whileTrue(operatorControl::feedToAmp,
// new FeedToAmp(intake, m_shooter, m_ampFeeder, feeder));

whileTrue(operatorControl::testShoot, new TestShoot(m_shooter));

// whileTrue(operatorControl::outtakeFromAmp, m_ampFeeder.run(m_ampFeeder::outtake));

whileTrue(operatorControl::never, new Lob(m_shooter, intake));

// whileTrue(operatorControl::homeClimber, new HomeClimber(comLog, climber));

// whileTrue(operatorControl::climbUpPosition, climber.upPosition());
// whileTrue(operatorControl::climbDownPosition, climber.downPosition());

///////////////////////////
//
// DRIVE
Expand Down Expand Up @@ -412,117 +317,21 @@ public RobotContainer(TimedRobot100 robot) throws IOException {
omegaController,
driverControl::trigger));

driveManually.register("LOCKED", false,
new ManualWithTargetLock(
fieldLog,
manLog,
swerveKinodynamics,
driverControl::target,
thetaController,
omegaController,
driverControl::trigger));

driveManually.register("SHOOTER_LOCK", false,
new ManualWithShooterLock(
fieldLog,
manLog,
swerveKinodynamics,
thetaController,
omegaController));

final PIDController omega2Controller = new PIDController(0.5, 0, 0); // .5

final ManualWithShooterLock shooterLock = new ManualWithShooterLock(
fieldLog,
comLog.child("ShooterLock"),
swerveKinodynamics,
thetaController,
omega2Controller);

final ManualWithAmpLock ampLock = new ManualWithAmpLock(
fieldLog,
comLog,
swerveKinodynamics,
thetaController,
omega2Controller);

final AutoMaker m_AutoMaker = new AutoMaker(
logger,
m_drive,
driveControllerFactory,
drivePID,
0,
feeder,
m_shooter,
intake,
m_sensors,
swerveKinodynamics,
viz);

// whileTrue(driverControl::test, m_AutoMaker.citrus(Alliance.Blue));
whileTrue(driverControl::test, m_AutoMaker.fourNoteAuto(Alliance.Blue, m_sensors));

whileTrue(driverControl::ampLock,
new AmpLockCommand(ampLock, driverControl::velocity, m_drive));

whileTrue(driverControl::shooterLock,
new ShootSmartWithRotation(comLog, m_drive, m_shooter, feeder, intake, shooterLock,
driverControl::velocity));

//////////////////
//
// DEFAULT COMMANDS
//

m_drive.setDefaultCommand(driveManually);
m_shooter.setDefaultCommand(m_shooter.run(m_shooter::stop));
feeder.setDefaultCommand(feeder.run(feeder::stop));
intake.setDefaultCommand(intake.run(intake::stop));
// climber.setDefaultCommand(new ClimberDefault(
// if far from the goal, go fast. if near, go slow.
// m_ampPivot.setDefaultCommand(new AmpFastThenSlow(m_ampPivot, 0.1, 0));

////////////////////
//
// AUTONOMOUS
//

// this illustrates how to use AutonCommand together with AllianceCommand
m_auton = new AutonCommand(
Map.of(
AutonChooser.Routine.FIVE_NOTE, new AllianceCommand(
m_AutoMaker.fourNoteAuto(
Alliance.Red, m_sensors),
m_AutoMaker.fourNoteAuto(
Alliance.Blue, m_sensors)),
AutonChooser.Routine.COMPLEMENTARY, new AllianceCommand(
m_AutoMaker.citrus(
Alliance.Red),
m_AutoMaker.citrus(
Alliance.Blue)),
AutonChooser.Routine.COMPLEMENTARY2, new AllianceCommand(
m_AutoMaker.citrusv2(
Alliance.Red),
m_AutoMaker.citrusv2(
Alliance.Blue)),
AutonChooser.Routine.SIBLING, new AllianceCommand(
m_AutoMaker.sibling(
Alliance.Red),
m_AutoMaker.sibling(
Alliance.Blue)),
AutonChooser.Routine.NOTHING, new AllianceCommand(
new PrintCommand("nothing red goes here"),
new PrintCommand("nothing blue goes here"))),
AutonChooser::routine);
}

public void beforeCommandCycle() {
// ModeSelector.selectMode(operatorControl::pov);
}

public void onTeleop() {
m_shooter.reset();
}
public void onTeleop() {}

public void onInit() {
// m_drive.resetPose()
Expand Down
4 changes: 4 additions & 0 deletions comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/DriveToAmp.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
import org.team100.lib.commands.drivetrain.DriveWithProfile2;
import org.team100.lib.controller.drivetrain.HolonomicFieldRelativeController;
import org.team100.lib.geometry.GeometryUtil;
import org.team100.lib.logging.FieldLogger;
import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem;
import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics;

Expand All @@ -33,6 +34,7 @@ public class DriveToAmp extends SequentialCommandGroup {
kAmpXM, kFieldWidthM - kCloseToAmpYM, GeometryUtil.kRotation90);

public DriveToAmp(
FieldLogger.Log fieldLogger,
SwerveDriveSubsystem drive,
HolonomicFieldRelativeController controller,
SwerveKinodynamics limits,
Expand All @@ -44,6 +46,7 @@ public DriveToAmp(
addCommands(
new ParallelDeadlineGroup(
new DriveWithProfile2(
fieldLogger,
() -> DriverStation.getAlliance().map(
x -> switch (x) {
case Red -> kRedNearAmp;
Expand All @@ -57,6 +60,7 @@ public DriveToAmp(
new AmpFastThenSlow(amp, 1.7, 1.8),
new SequentialCommandGroup(
new DriveWithProfile2(
fieldLogger,
() -> DriverStation.getAlliance().map(
x -> switch (x) {
case Red -> kRedCloseToAmp;
Expand Down
9 changes: 9 additions & 0 deletions lib/src/main/java/org/team100/lib/commands/drivetrain/DriveWithProfile2.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
import org.team100.lib.controller.drivetrain.HolonomicFieldRelativeController;
import org.team100.lib.dashboard.Glassy;
import org.team100.lib.framework.TimedRobot100;
import org.team100.lib.logging.FieldLogger;
import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem;
import org.team100.lib.motion.drivetrain.SwerveModel;
import org.team100.lib.motion.drivetrain.kinodynamics.FieldRelativeVelocity;
Expand Down Expand Up @@ -44,6 +45,8 @@ public class DriveWithProfile2 extends Command implements Glassy {

private Pose2d m_fieldRelativeGoal = null;

private final FieldLogger.Log m_field_log;

private Control100 xSetpoint;
private Control100 ySetpoint;
private Control100 thetaSetpoint;
Expand All @@ -53,10 +56,12 @@ public class DriveWithProfile2 extends Command implements Glassy {
private Model100 m_thetaGoalRaw;

public DriveWithProfile2(
FieldLogger.Log fieldLogger,
Supplier<Optional<Pose2d>> fieldRelativeGoal,
SwerveDriveSubsystem drivetrain,
HolonomicFieldRelativeController controller,
SwerveKinodynamics limits) {
m_field_log = fieldLogger;
m_fieldRelativeGoalSupplier = fieldRelativeGoal;
m_swerve = drivetrain;
m_controller = controller;
Expand Down Expand Up @@ -136,6 +141,10 @@ public void execute() {

m_yGoalRaw = new Model100(m_fieldRelativeGoal.getY(), 0);
ySetpoint = yProfile.calculate(TimedRobot100.LOOP_PERIOD_S, ySetpoint.model(), m_yGoalRaw);
m_field_log.m_log_target.log(() -> new double[] {
m_fieldRelativeGoal.getX(),
m_fieldRelativeGoal.getY(),
m_fieldRelativeGoal.getRotation().getRadians() });

thetaSetpoint = thetaProfile.calculate(TimedRobot100.LOOP_PERIOD_S, thetaSetpoint.model(), m_thetaGoalRaw);
SwerveModel goalState = new SwerveModel(xSetpoint.model(), ySetpoint.model(), thetaSetpoint.model());
Expand Down
Loading
Loading