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

Commit

Permalink
cleanup
Browse files Browse the repository at this point in the history
  • Loading branch information
@PascalSkylake
PascalSkylake committed Mar 29, 2024
1 parent a804f8e commit 1ebfe34
Show file tree
Hide file tree
Showing 6 changed files with 39 additions and 200 deletions.
65 changes: 18 additions & 47 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,11 +5,8 @@

package frc.robot;

import java.util.ArrayList;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.InstantCommand;
Expand All @@ -36,15 +33,6 @@ public class Robot extends TimedRobot {
@Override
public void robotInit() {
DriverStation.silenceJoystickConnectionWarning(true);

// Starts recording to data log
// DataLogManager.start();
// Record both DS control and joystick data
// DriverStation.startDataLog(DataLogManager.getLog());

// Instantiate our RobotContainer. This will perform all our button bindings,
// and put our autonomous chooser on the dashboard.
robotContainer = RobotContainer.getInstance();

// for debugging
CommandScheduler.getInstance().onCommandInitialize((command) -> {
Expand All @@ -59,6 +47,10 @@ public void robotInit() {
var cmdName = command.getName();
System.out.println(cmdName + " ended.");
});

// Instantiate our RobotContainer. This will perform all our button bindings,
// and put our autonomous chooser on the dashboard.
robotContainer = RobotContainer.getInstance();
}

/**
Expand All @@ -79,7 +71,6 @@ public void robotPeriodic() {
// and running subsystem periodic() methods. This must be called from the
// robot's periodic
// block in order for anything in the Command-based framework to work.
SmartDashboard.getEntry("LED State").getInstance().flush();
CommandScheduler.getInstance().run();
}

Expand All @@ -96,9 +87,9 @@ public void disabledInit() {
public void disabledPeriodic() {
RobotContainer.intake.setSetpoint(RobotContainer.intake.getPosition());
RobotContainer.arm.setSetpoint(RobotContainer.arm.getPosition());

SwerveDrive.kMaxAngularSpeedRadiansPerSecond = 3 * Math.PI;
SwerveDrive.kMaxSpeedMetersPerSecond = 4.6;
// RobotContainer.shooter.setSetpoint(RobotContainer.shooter.getPosition());
}

/**
Expand All @@ -107,42 +98,22 @@ public void disabledPeriodic() {
*/
@Override
public void autonomousInit() {
autonomousRoutine = new ProxyCommand(() -> robotContainer.getAutonomousCommand()).andThen(new InstantCommand(() -> {
RobotContainer.swerveDrive.stop();
RobotContainer.shooter.stopIndexer();
RobotContainer.shooter.stopShooting();
RobotContainer.intake.stop();
}, RobotContainer.swerveDrive, RobotContainer.shooter, RobotContainer.intake));

// if autonomousRoutine is custom use RobotContainer.fullAuto(start, note...)
// somehow get and parse a string from glass for start and note...
if (autonomousRoutine.getName().equals("Dynamic")) {
String commandString = SmartDashboard.getString("DynamicAutoString", "");
String[] split = commandString.split(" ");
ArrayList<String> splitList = new ArrayList<>();
for (String s : split) {
splitList.add(s);
}
ArrayList<String> notePoseStrings = new ArrayList<>();

for (int i = 1; i < split.length; i++) {
notePoseStrings.add(splitList.get(i));
}

String[] notePoseStringsArray = new String[notePoseStrings.size()];
for (int i = 0; i < notePoseStrings.size(); i++) {
notePoseStringsArray[i] = notePoseStrings.get(i);
}
autonomousRoutine = robotContainer.getAutonomousCommand();

if (commandString.equals("")) {
autonomousRoutine = robotContainer.fullAuto("center");
} else {
autonomousRoutine = robotContainer.fullAuto(splitList.get(0), notePoseStringsArray);
}
if (autonomousRoutine != null) {
// for the end of the auto routine
var stopAllCmd = new InstantCommand(() -> {
RobotContainer.swerveDrive.stop();
RobotContainer.shooter.stopIndexer();
RobotContainer.shooter.stopShooting();
RobotContainer.intake.stop();
}, RobotContainer.swerveDrive, RobotContainer.shooter, RobotContainer.intake);

}

// get the auto routine as a proxy command so we are free to compose a sequential command group
// using it, run the auto routine then stop all motors
new ProxyCommand(() ->autonomousRoutine).andThen(stopAllCmd);

if (autonomousRoutine != null) {
autonomousRoutine.schedule();
}
}
Expand Down
110 changes: 13 additions & 97 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@
import com.kauailabs.navx.frc.AHRS;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import com.pathplanner.lib.path.PathPlannerPath;
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
import com.pathplanner.lib.util.PIDConstants;
import com.pathplanner.lib.util.PathPlannerLogging;
Expand All @@ -27,15 +26,13 @@
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
import edu.wpi.first.wpilibj2.command.ProxyCommand;
import edu.wpi.first.wpilibj2.command.RepeatCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.commands.Dynamic;
import frc.robot.commands.arm.SetArmState;
import frc.robot.commands.drive.AutoIntake;
import frc.robot.commands.drive.AutoPickupNote;
Expand Down Expand Up @@ -81,30 +78,6 @@ public AutoBuilder getAutoBuilder() {

public static final AHRS navx = new AHRS(Port.kUSB);

/**
* Returns the angle or "yaw" of the robot in degrees. CW positive ranging from
* [-180, 180].
*/
public static double getRobotYaw() {
return navx.getYaw();
}

/**
* Returns the pitch angle of the robot in degrees. This tracks the
* forwards/backwards tilt of the robot.
*/
public static double getRobotPitch() {
return navx.getPitch();
}

/**
* Returns the roll angle of the robot in degrees. This tracks the
* left/right tilt of the robot.
*/
public static double getRobotRoll() {
return navx.getRoll();
}

public static final Field2d field = new Field2d();

// Human Interface Devices (HIDs)
Expand All @@ -117,7 +90,6 @@ public static double getRobotRoll() {
public static final Shooter shooter = new Shooter();
public static final Arm arm = new Arm();
public static final Climber climber = new Climber();
// public static final NewLEDStrip ledStrip = new NewLEDStrip();
public static final LEDStrip leds = new LEDStrip();

// Cameras
Expand Down Expand Up @@ -368,9 +340,7 @@ private void configureAutonmous() {

autoBuilder = new AutoBuilder();
autoChooser = AutoBuilder.buildAutoChooser();
autoChooser.addOption("Dynamic", new Dynamic());
SmartDashboard.putData("Auto Chooser", autoChooser);
SmartDashboard.putString("DynamicAutoString", "");

configurePathPlannerLogging();
}
Expand Down Expand Up @@ -413,7 +383,7 @@ private void registerNamedCommands() {
// General
NamedCommands.registerCommand("ShootSubwooferSequence", shootSubwooferSequence());
NamedCommands.registerCommand("InitSequence", initSequence());

NamedCommands.registerCommand("StopAll", stopAllCommand());
}

private Command stopAllCommand() {
Expand Down Expand Up @@ -444,79 +414,25 @@ private Command straightenSwervesCommand() {
}

private Command intakeSetpointAndRun() {
return (

new SetArmState(ArmState.kIntake)
.andThen(
new InstantCommand(() -> {
intake.intake();
shooter.indexAuto();
}, intake, shooter)));
return new SetArmState(ArmState.kIntake)
.andThen(new InstantCommand(() -> {
intake.intake();
shooter.indexAuto();
}, intake, shooter));
}

private Command initSequence() {
return new ParallelDeadlineGroup(new SetIntakeState(IntakeState.kDown),
new SetArmState(ArmState.kDeployDemonHorns).alongWith(
new RampShooter(() -> 15)));//.alongWith(straightenSwervesCommand()));
return new ParallelDeadlineGroup(
new SetIntakeState(IntakeState.kDown),
new SetArmState(ArmState.kDeployDemonHorns).alongWith(new RampShooter(() -> 15)));
}

private Command shootSubwooferSequence() {
return new SequentialCommandGroup(new RampShooter(() -> 15)
.alongWith(new SetArmState(ArmState.kShootFromSubwooferAuto)), new WaitCommand(0.1), feedCommand(),
intakeSetpointAndRun());
}

private Command getPathCommand(String name) {
PathPlannerPath path = PathPlannerPath.fromPathFile(name);
return AutoBuilder.followPath(path);
}

private PathPlannerPath getPath(String name) {
return PathPlannerPath.fromPathFile(name);
}

private Pose2d pathStartPose(String name) {
return getPath(name).getPreviewStartingHolonomicPose();
}

// this will get a string or something from glass idk yet
private Command getNoteSequence(String startPosition, String notePosition) {
String getNotePathName = startPosition + " get " + notePosition;
String getReturnPathName = "return " + startPosition + " get " + notePosition;
return new SequentialCommandGroup(
new ParallelCommandGroup(
getPathCommand(getNotePathName).andThen(new InstantCommand(() -> swerveDrive.stop(), swerveDrive)),
new WaitForNote().withTimeout(3)),
getPathCommand(getReturnPathName),
new InstantCommand(() -> swerveDrive.stop(), swerveDrive),
shootSubwooferSequence());
}

// THIS EXPECTS YOU TO PUT THE NOTE POSITIONS IN THE RIGHT ORDER!! (I don't want
// to sort them based on startPosition because it's 3am and I'm sleepy)
public Command fullAuto(String startPosition, String... notePositions) {
// if no note positions are passed shoot in place and stop everything
if (notePositions.length == 0) {
return initSequence().andThen(shootSubwooferSequence()).andThen(stopAllCommand());
}

// set all to lowercase to match the path names
for (String s : notePositions) {
s = s.toLowerCase();
}
startPosition = startPosition.toLowerCase();

// get each note sequence, each noteSequence includes path to get, grab, if
// missed timeout after 3 secs, return path, shoot
Command[] noteSequences = new Command[notePositions.length];
for (int i = 0; i < notePositions.length; i++) {
System.out.println(notePositions[i]);
noteSequences[i] = getNoteSequence(startPosition, notePositions[i]);
}

// combine init and all noteSequence
System.out.println(startPosition + notePositions);
return new SequentialCommandGroup(initSequence(), new SequentialCommandGroup(noteSequences));
new RampShooter(() -> 15).alongWith(new SetArmState(ArmState.kShootFromSubwooferAuto)),
new WaitCommand(0.1),
feedCommand(),
intakeSetpointAndRun());
}

private void configurePathPlannerLogging() {
Expand Down
33 changes: 0 additions & 33 deletions src/main/java/frc/robot/commands/Dynamic.java
View file

This file was deleted.

8 changes: 3 additions & 5 deletions src/main/java/frc/robot/commands/drive/DriveSwerveWithXbox.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,11 @@

import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.RobotContainer;
import frc.robot.subsystems.Arm.ArmState;
import frc.robot.subsystems.drive.SwerveDrive;

public class DriveSwerveWithXbox extends Command {
private boolean fieldRelative = false;
private final boolean fieldRelative;

public DriveSwerveWithXbox(boolean fod) {
this.addRequirements(RobotContainer.swerveDrive);
Expand All @@ -27,9 +28,6 @@ public void initialize() {
// Called repeatedly when this Command is scheduled to run
@Override
public void execute() {

// fieldRelative = RobotContainer.driverController.getHID().getRightTriggerAxis() > 0.5;

// Get the x speed. We are inverting this because Xbox controllers return
// negative values when we push forward.
var xSpeed = 0.0;
Expand Down Expand Up @@ -57,7 +55,7 @@ public void execute() {
* SwerveDrive.kMaxAngularSpeedRadiansPerSecond;
}

var sign = fieldRelative ? 1 : -1;
var sign = fieldRelative || RobotContainer.arm.isAtState(ArmState.kAmp) ? 1 : -1;
RobotContainer.swerveDrive.drive(-xSpeed*sign, -ySpeed*sign, rot, fieldRelative);
}

Expand Down
Loading

0 comments on commit 1ebfe34

Please sign in to comment.