Skip to content
This repository has been archived by the owner on Jan 10, 2025. It is now read-only.

Commit

Permalink
auto intake
Browse files Browse the repository at this point in the history
fcuellar13 committed Jan 5, 2025
1 parent d780beb commit 67c4eaf
Showing 3 changed files with 27 additions and 4 deletions.
3 changes: 1 addition & 2 deletions src/main/java/frc/robot/Robot.java
Original file line number Diff line number Diff line change
@@ -51,9 +51,8 @@ public class Robot extends TimedRobot {
private final LocalizationSubsystem localization = new LocalizationSubsystem(imu, vision, swerve);
private final RobotManager robotManager =
new RobotManager(arm, shooter, localization, vision, imu, intake, queuer, swerve);

private final RobotCommands robotCommands = new RobotCommands(robotManager);
private final Trailblazer trailblazer = new Trailblazer(swerve, localization);
private final RobotCommands robotCommands = new RobotCommands(robotManager, trailblazer);
private final Autos autos = new Autos(robotManager, trailblazer);

private final LightsSubsystem lights = new LightsSubsystem(robotManager, hardware.candle);
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/autos/BaseAuto.java
Original file line number Diff line number Diff line change
@@ -16,7 +16,7 @@ public abstract class BaseAuto {
protected BaseAuto(RobotManager robotManager, Trailblazer trailblazer) {
this.robotManager = robotManager;
this.trailblazer = trailblazer;
this.actions = new RobotCommands(robotManager);
this.actions = new RobotCommands(robotManager, trailblazer);
this.autoCommands = new AutoCommands(actions, robotManager);
}

26 changes: 25 additions & 1 deletion src/main/java/frc/robot/robot_manager/RobotCommands.java
Original file line number Diff line number Diff line change
@@ -1,16 +1,23 @@
package frc.robot.robot_manager;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.Subsystem;
import frc.robot.autos.trailblazer.AutoPoint;
import frc.robot.autos.trailblazer.AutoSegment;
import frc.robot.autos.trailblazer.Trailblazer;
import java.util.List;

public class RobotCommands {
private final RobotManager robot;
private final Subsystem[] requirements;
private final Trailblazer trailblazer;

public RobotCommands(RobotManager robot) {
public RobotCommands(RobotManager robot, Trailblazer trailblazer) {
this.robot = robot;
this.trailblazer = trailblazer;
var requirementsList = List.of(robot.arm, robot.intake, robot.queuer, robot.shooter);
requirements = requirementsList.toArray(Subsystem[]::new);
}
@@ -118,4 +125,21 @@ public Command unjamCommand() {
return Commands.runOnce(robot::unjamRequest, requirements)
.andThen(robot.waitForState(RobotState.IDLE_NO_GP));
}

public Command redAutoIntake() {
return Commands.sequence(
trailblazer.followSegment(
new AutoSegment(
new AutoPoint(new Pose2d(15.14, 6.02, Rotation2d.fromDegrees(49.1))),
new AutoPoint(new Pose2d(15.629, 6.610, Rotation2d.fromDegrees(49.1))),
new AutoPoint(new Pose2d(16.079, 7.219, Rotation2d.fromDegrees(51.7))))),
Commands.print("Raise elevator, intake coral --> finished"),
trailblazer.followSegment(
new AutoSegment(
new AutoPoint(new Pose2d(15.14, 6.801, Rotation2d.fromDegrees(51.7))))));
}

public Command blueAutoIntake() {
return Commands.sequence(null);
}
}

0 comments on commit 67c4eaf

Please sign in to comment.