diff --git a/FRC_2020_Infinite_Recharge_Official/src/main/java/frc/robot/RobotContainer.java b/FRC_2020_Infinite_Recharge_Official/src/main/java/frc/robot/RobotContainer.java index f849ab3..05c86c7 100644 --- a/FRC_2020_Infinite_Recharge_Official/src/main/java/frc/robot/RobotContainer.java +++ b/FRC_2020_Infinite_Recharge_Official/src/main/java/frc/robot/RobotContainer.java @@ -257,6 +257,6 @@ public void disableLimelight() { */ public Command getAutonomousCommand() { // An ExampleCommand will run in autonomous - return new AutoMain(driveSubsystem, shooterSubsystem, indexSubsystem, intakeSubsystem, lifterSubsystem); + return new AutoMain(autoMode.generatorShooter, driveSubsystem, shooterSubsystem, indexSubsystem, intakeSubsystem, lifterSubsystem); } } diff --git a/FRC_2020_Infinite_Recharge_Official/src/main/java/frc/robot/commands/Autonomous/AutoMain.java b/FRC_2020_Infinite_Recharge_Official/src/main/java/frc/robot/commands/Autonomous/AutoMain.java new file mode 100644 index 0000000..c75114a --- /dev/null +++ b/FRC_2020_Infinite_Recharge_Official/src/main/java/frc/robot/commands/Autonomous/AutoMain.java @@ -0,0 +1,47 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.DriveCommands.DriveStraightTime; +import frc.robot.commands.SolenoidSetsAndToggles.ToggleIntake; +import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.IndexSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.LifterSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import GeneratorShooterAuto; +import OpposingTrenchAuto; +import PushTowardWallAuto; + +enum autoMode { + pushTowardWall, + generatorShooter, + opposingTrench +} + +public void AutoMain( + autoMode mode, + DriveSubsystem driveSubsystem, + ShooterSubsystem shooterSubsystem, + IndexSubsystem indexSubsystem, + IntakeSubsystem intakeSubsystem, + LifterSubsystem lifterSubsystem +) { + switch (mode) { + case pushTowardWall: + return PushTowardWallAuto(driveSubsystem); + break; + case generatorShooter: + return GeneratorShooterAuto(driveSubsystem, shooterSubsystem, indexSubsystem, intakeSubsystem, lifterSubsystem); + break; + case OpposingTrenchAuto(driveSubsystem, shooterSubsystem, indexSubsystem, intakeSubsystem, lifterSubsystem); + } +} diff --git a/FRC_2020_Infinite_Recharge_Official/src/main/java/frc/robot/commands/AutoMain.java b/FRC_2020_Infinite_Recharge_Official/src/main/java/frc/robot/commands/Autonomous/GeneratorShooterAuto.java similarity index 100% rename from FRC_2020_Infinite_Recharge_Official/src/main/java/frc/robot/commands/AutoMain.java rename to FRC_2020_Infinite_Recharge_Official/src/main/java/frc/robot/commands/Autonomous/GeneratorShooterAuto.java diff --git a/FRC_2020_Infinite_Recharge_Official/src/main/java/frc/robot/commands/Autonomous/OpposingTrenchAuto.java b/FRC_2020_Infinite_Recharge_Official/src/main/java/frc/robot/commands/Autonomous/OpposingTrenchAuto.java new file mode 100644 index 0000000..8d946b6 --- /dev/null +++ b/FRC_2020_Infinite_Recharge_Official/src/main/java/frc/robot/commands/Autonomous/OpposingTrenchAuto.java @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.DriveCommands.DriveStraightTime; +import frc.robot.commands.SolenoidSetsAndToggles.ToggleIntake; +import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.IndexSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.LifterSubsystem; +import frc.robot.subsystems.ShooterSubsystem; + +// Steals the nearest two power cells from the opposing alliance's trench. +// CAUTION: Avoid running this if an opposing team's short robot lines up for their trench. +// Our robot might not be faster than theirs. If we are not faster, we will incur a penalty. +// Requriements: Robot's front bumper fully on the initialization line (parrallel with it), +// intake pointed toward trench, and robot centered on the two balls in the trench. + +public class OpposingTrenchAuto extends SequentialCommandGroup { + public AutoMain(DriveSubsystem driveSubsystem, ShooterSubsystem shooterSubsystem, + IndexSubsystem indexSubsystem, IntakeSubsystem intakeSubsystem, + LifterSubsystem lifterSubsystem) { + addCommands( + + // Drop/run Intake + // Drive toward the balls in the trench. + // Immediately drive foward (out of the trench) when we think we've driven far enough to the balls. + // Stop when we think we are out of the trench. + // Turn toward the Outer Port (optionally drive closer to the port) + // Fire! + ); + } +} diff --git a/FRC_2020_Infinite_Recharge_Official/src/main/java/frc/robot/commands/Autonomous/PushTowardWallAuto.java b/FRC_2020_Infinite_Recharge_Official/src/main/java/frc/robot/commands/Autonomous/PushTowardWallAuto.java new file mode 100644 index 0000000..c2e226d --- /dev/null +++ b/FRC_2020_Infinite_Recharge_Official/src/main/java/frc/robot/commands/Autonomous/PushTowardWallAuto.java @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.DriveCommands.DriveStraightTime; +import frc.robot.commands.SolenoidSetsAndToggles.ToggleIntake; +import frc.robot.subsystems.DriveSubsystem; + +// Moves off the initialization line toward the Oppsing Alliance's driver station. +// May or may not push another robot (that presumably does not have auto) in the process. +public class PushTowardWallAuto extends SequentialCommandGroup { + double motorSpeed = 0.75; + double driveTime = 3.0; + public AutoPushTowardWall(DriveSubsystem driveSubsystem) { + addCommands( + new DriveStraightTime(driveSpeed, driveSpeed, driveTime, driveSubsystem) + ); + } +}