Skip to content

did a cool charge station auto with gyros #63

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
siIverfish wants to merge 3 commits into
base: main
Choose a base branch
from
Open

did a cool charge station auto with gyros #63

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
96b361c
added over charge station & back and balance using gyro auto
siIverfish Mar 18, 2023
2edf233
Merge branch 'main' of https://github.com/frcteam3952/FRC2023
siIverfish Mar 18, 2023
6f2574c
Merge branch 'main' of https://github.com/frcteam3952/FRC2023
siIverfish Mar 18, 2023
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
4 changes: 4 additions & 0 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -118,6 +118,7 @@ public class RobotContainer {
private Command doublePlacementAutoPW;
private Command placeCubeThenBalanceAutoPW;
private Command doublePlacementThenBalanceAutoPW;
private Command overChargeStationAndBackGyroAuto;

private Command m_autonomousCommand;
private final SendableChooser<Command> m_chooser = new SendableChooser<>();
Expand Down Expand Up @@ -208,12 +209,14 @@ public void onRobotInit() {
balanceAuto = Autos.balanceAuto(this);
placeCubeThenBalanceAuto = Autos.placeCubeThenBalanceAuto(this);
doublePlacementAuto = Autos.doublePlacementAuto(this);
overChargeStationAndBackGyroAuto = Autos.overChargeStationAndBackGyroAuto(this);

// All below autos use Pathweaver trajectories and probably don't work right now
balanceAutoPW = Autos.balanceAutoPW(this);
doublePlacementAutoPW = Autos.doublePlacementAutoPW(this);
placeCubeThenBalanceAutoPW = Autos.placeCubeThenBalanceAutoPW(this);
doublePlacementThenBalanceAutoPW = Autos.doublePlacementThenBalanceAutoPW(this);



// Adds autonomous options to dashboard
Expand All @@ -228,6 +231,7 @@ public void onRobotInit() {
m_chooser.addOption("Double Placement Auto Using Raw Trajectories (coded at 3am)", SimpleAutos.doublePlacementAuto(this));
m_chooser.addOption("Move one meter using raw trajectories", SimpleAutos.moveOneMeter(this));
m_chooser.addOption("Go to estimated cone position using raw trajectories", SimpleAutos.goToEstimatedConeLocation(this));
m_chooser.addOption("new untested Over Charge Station and Back Auto Then Balance using Gyro", overChargeStationAndBackGyroAuto);

// These autons use Pathweaver
m_chooser.addOption("Balance Auto (PathWeaver)", balanceAutoPW);
Expand Down
43 changes: 43 additions & 0 deletions src/main/java/frc/robot/commands/autocommands/Autos.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import frc.robot.Robot;
import frc.robot.RobotContainer;
import frc.robot.commands.drivecommands.BalanceChargeStationCommand;
import frc.robot.subsystems.staticsubsystems.RobotGyro;
Expand Down Expand Up @@ -188,6 +189,48 @@ public static CommandBase dynamicTaxiForBalanceAuto(RobotContainer robot) {
);
}

public static CommandBase overChargeStationAndBackGyroAuto(RobotContainer robot) {
return Commands.run(() -> {
robot.driveTrain.tankDrive(0.25, 0);
}, robot.driveTrain)
.until(
() -> (RobotGyro.getGyroAngleDegreesPitch() < -5)
)
.andThen(Commands.run(() -> {
robot.driveTrain.tankDrive(0.5, 0);
}, robot.driveTrain))
.until(
() -> (Math.abs(RobotGyro.getGyroAngleDegreesPitch()) < 2)
)
.andThen(Commands.run(() -> {
robot.driveTrain.tankDrive(0.25, 0);
}, robot.driveTrain))
.until(
() -> (RobotGyro.getGyroAngleDegreesPitch() > 5)
)
.andThen(Commands.run(() -> {
robot.driveTrain.tankDrive(0.25, 0);
}, robot.driveTrain))
.until(
() -> (Math.abs(RobotGyro.getGyroAngleDegreesPitch()) < 2)
)
.andThen(Commands.run(() -> {
robot.driveTrain.tankDrive(-0.25, 0);
}, robot.driveTrain))
.until(
() -> (RobotGyro.getGyroAngleDegreesPitch() > 5)
)
.andThen(Commands.run(() -> {
robot.driveTrain.tankDrive(-0.5, 0);
}, robot.driveTrain))
.until(
() -> (Math.abs(RobotGyro.getGyroAngleDegreesPitch()) < 2)
)
.andThen(
balanceAuto(robot)
);
}

// Gets taxi points and balances charge station
public static CommandBase balanceAuto(RobotContainer robot) {
return Commands.runOnce(
Expand Down