replacement basic auto (#54)

* replacement basic auto * edited auto, removed smartdashboard drivetime
team401 · Oct 27, 2023 · 20ec770 · 20ec770
1 parent 0663fba
commit 20ec770
Show file tree

Hide file tree

Showing 4 changed files with 25 additions and 14 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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)
+        );  
     }
-
-
 }