diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8281fd9..f21ef73 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,6 +7,8 @@ public class Constants { + public static final double autoDriveTime = 5; + public final class ArmConstants { public static final int wristMotorID = 19; //19? diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1ba0fb8..f83b28a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.commands.auto.Auto; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -23,8 +24,6 @@ */ public class Robot extends TimedRobot { - private final Timer m_timer = new Timer(); - private RobotContainer m_robotContainer; private final Timer loopTimer = new Timer(); @@ -53,9 +52,7 @@ public void robotInit() { /** This function is run once each time the robot enters autonomous mode. */ @Override public void autonomousInit() { - m_timer.reset(); - m_timer.start(); - + m_robotContainer.getAutonomous().schedule(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4233cd0..6828ca8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,12 +3,14 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.ArmMove; import frc.robot.commands.DriveWithJoysticks; +import frc.robot.commands.auto.Auto; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.ArmSubsystem.Mode; import frc.robot.subsystems.drive.DriveSubsystem; @@ -44,6 +46,7 @@ public RobotContainer() { )); configureButtonBindings(); + SmartDashboard.putNumber("Auto Drive Time", 5); } private void configureButtonBindings() { @@ -64,5 +67,9 @@ private void configureButtonBindings() { } + public Command getAutonomous() { + return new Auto(arm, drive, Constants.autoDriveTime); + } + } diff --git a/src/main/java/frc/robot/commands/auto/Auto.java b/src/main/java/frc/robot/commands/auto/Auto.java index def7068..44a8115 100644 --- a/src/main/java/frc/robot/commands/auto/Auto.java +++ b/src/main/java/frc/robot/commands/auto/Auto.java @@ -1,11 +1,14 @@ package frc.robot.commands.auto; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.ArmMove; @@ -17,17 +20,19 @@ public class Auto extends SequentialCommandGroup { public Auto(ArmSubsystem arm, DriveSubsystem drive, double driveTime) { + addRequirements(arm); + addRequirements(drive); addCommands( - new InstantCommand(() -> arm.setMode(Mode.SHOOT_HIGH)), - new WaitCommand(1), - new InstantCommand(() -> arm.setMode(Mode.IDLE)), - new ParallelRaceGroup( + new ArmMove(arm, Mode.SHOOT_HIGH).raceWith(new WaitCommand(1.0)), + new ParallelRaceGroup ( + new ArmMove(arm, Mode.INTAKE), + new RunCommand(() -> drive.setGoalChassisSpeeds(new ChassisSpeeds(0.5, 0, 0))), new WaitCommand(driveTime) - ) - ); - + ), + new InstantCommand(() -> SmartDashboard.putBoolean("drive finished", true)), + new InstantCommand(() -> drive.setGoalChassisSpeeds(new ChassisSpeeds(0, 0, 0))), + new ArmMove(arm, Mode.STOW) + ); } - - }