Comp changes

src/main/java/frc/robot/Robot.java

@@ -36,6 +36,9 @@ public class Robot extends TimedRobot {
 
   @Override
   public void robotInit() {
+
+    m_riologger.initLog();
+    m_BoardButtons.initButtons();
     // Instantiate our RobotContainer. This will perform all our button bindings,
     // and put our
     // autonomous chooser on the dashboard.
@@ -45,8 +48,6 @@ public void robotInit() {
     m_robotContainer = new RobotContainer();
     Ultrasonic.setAutomaticMode(true);
 
-    m_riologger.initLog();
-    m_BoardButtons.initButtons();
 
 
 
@@ -107,17 +108,18 @@ public void teleopInit() {
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove
     // this line or comment it out.
+    m_robotContainer.setButtonStateTrue();
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
+
   }
 
   /** This function is called periodically during operator control. */
   @Override
   public void teleopPeriodic() {
     m_BoardButtons.updateButtons();
     m_riologger.executeLogger();
-
   }
 
   @Override

src/main/java/frc/robot/RobotContainer.java

@@ -4,6 +4,9 @@
 
 package frc.robot;
 
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.commands.Climb.ClimbClose;
@@ -21,6 +24,7 @@
 import frc.robot.commands.Intake.Arm;
 import frc.robot.commands.Intake.ArmForward;
 import frc.robot.commands.Intake.AutoClose;
+import frc.robot.commands.Intake.AutoDelay;
 import frc.robot.commands.Intake.AutoOuttake;
 import frc.robot.commands.Intake.BottomClose;
 import frc.robot.commands.Intake.BottomOpen;
@@ -61,8 +65,10 @@ public class RobotContainer {
   private final TrajectoryCreation m_traj = new TrajectoryCreation();
   private final SendableChooser<Command> m_chooser = new SendableChooser<>();
 
+  //Auto Shuffleboard
   private final SequentialCommandGroup dumpGetOut = new SequentialCommandGroup(
-    new AutoClose(m_intake)
+    new AutoDelay(ShuffleBoardButtons.m_autoDelay.getDouble(0.0))
+    .andThen(new AutoClose(m_intake))
     .andThen(new AutoOuttake(m_intake))
     .andThen(m_follower.generateTrajectory(m_drivetrain, m_traj.getOutOfTarmac))
   );
@@ -87,11 +93,16 @@ private void openCimb(){
     m_climb.servoOpen();
   }
 
+  public void setButtonStateTrue(){
+    m_intake.setInputState(true);
+  } 
+
   public RobotContainer() { 
     // Configure the button bindings
     configureButtonBindings();
     servoInit();
     openCimb();
+    setButtonStateTrue();
     configureDefaultCommands();
   }
 

src/main/java/frc/robot/ShuffleBoardButtons.java

@@ -15,7 +15,7 @@ public class ShuffleBoardButtons {
     ShuffleboardTab m_ultraSonicTab;
     ShuffleboardTab m_compressorTab;
     ShuffleboardTab m_smartdashboard;
-
+    
     //NetworkTables
     NetworkTableEntry pivotEntry;
     NetworkTableEntry intakeEntry;
@@ -25,9 +25,14 @@ public class ShuffleBoardButtons {
     NetworkTableEntry searchingInput;
     NetworkTableEntry servoOpened;
     NetworkTableEntry autoState;
-
     NetworkTableEntry isClimbOpened;
     NetworkTableEntry intakeButton;
+    NetworkTableEntry isServoNeutral;
+
+//     //Auto Shuffleboard
+//   ShuffleboardTab m_autoTab = Shuffleboard.getTab("autonomous");
+    public static NetworkTableEntry m_autoDelay;
+
 
 
     public void initButtons(){
@@ -48,8 +53,12 @@ public void initButtons(){
         autoState = m_smartdashboard.add("autoState", false).getEntry();
         isClimbOpened = m_smartdashboard.add("climb opened", false).getEntry();
         intakeButton = m_smartdashboard.add("intake button", false).getEntry();
+        isServoNeutral = m_smartdashboard.add("Is Servo Neutral?", false).getEntry();
+        m_autoDelay  = m_smartdashboard.add("delay amount", 0).getEntry();
     }
 
+
+
     public void updateButtons(){
         pivotEntry.setDouble(PivotMotor.getInstance().getBoreEncoder());
         intakeEntry.setDouble(Intake.getInstance().getBoreEncoder());
@@ -61,6 +70,7 @@ public void updateButtons(){
         autoState.setBoolean(Intake.getInstance().getInputState());
         isClimbOpened.setBoolean(Climb.getInstance().getClimbOpened());
         intakeButton.setBoolean(Intake.getInstance().getButtonVal());
+        isServoNeutral.setBoolean(Climb.getInstance().getServoNeutral());
+
     }
-
 }

src/main/java/frc/robot/commands/Intake/AutoDelay.java

@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.Intake;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class AutoDelay extends CommandBase {
+  private final double delay;
+  /** Creates a new AutoDelay. */
+  public AutoDelay(double t) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    delay = t;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    Timer.delay(delay);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}

src/main/java/frc/robot/controls/ControlMap.java

@@ -36,6 +36,5 @@ public class ControlMap{
     public static JoystickButton DRIVER_LJ_BUTTON = new JoystickButton(driver, 9); //
     public static JoystickButton DRIVER_RJ_BUTTON = new JoystickButton(driver, 10); //
     public static POVButton DRIVER_DUP = new POVButton(driver, 0);
-
 
 }

src/main/java/frc/robot/subsystems/Climb.java

@@ -22,6 +22,7 @@ public class Climb extends SubsystemBase {
 
   private Compressor compressor;
   private boolean toggle_compressor = false;
+  private boolean isServoNeutral = false;
 
   private Climb() {
     rightServo = new Servo(Constants.climb_servo[1]);
@@ -68,6 +69,7 @@ public void setServosNeutral() {
     // System.out.println("Climb.setServosNeutral() start");
     leftServo.setDisabled();
     rightServo.setDisabled();
+    isServoNeutral = true;
     // System.out.println("Climb.setServosNeutral() end");
   }
 
@@ -78,6 +80,7 @@ public void servoClose() {
     leftServo.setAngle(180);
     rightServo.setAngle(0);
     climbOpened = false;
+    isServoNeutral = false;
     // System.out.println("Climb.servoClose() end");
   }
 
@@ -87,9 +90,14 @@ public void servoOpen() {
     leftServo.setAngle(90);
     rightServo.setAngle(90);
     climbOpened = true;
+    isServoNeutral = false;
     // System.out.println("Climb.servoOpen() end");
   }
 
+  public boolean getServoNeutral(){
+    return isServoNeutral;
+  }
+
   public boolean getClimbOpened(){
     return climbOpened;
   }

src/main/java/frc/robot/subsystems/Intake.java

@@ -30,15 +30,15 @@ public class Intake extends SubsystemBase {
 
   private final DigitalInput m_intakeButton;
   static Intake instance = null;
-  private boolean buttonstate = false;
-  
+  private boolean buttonstate;
+
   private Intake() {
     boreEncoderIntake = new DutyCycleEncoder(Constants.boreEncoderIntake);
     shoulder = new WPI_TalonSRX(Constants.Talon_arm);
     shoulder.setNeutralMode(NeutralMode.Brake);
     bottomLeft = new Servo(Constants.bottom_servos[0]);
     bottomRight = new Servo(Constants.bottom_servos[1]);
-
+    buttonstate = true;
     // m_ultrasonicIntake = new Ultrasonic(Constants.ULTRASONIC_INTAKE[0], Constants.ULTRASONIC_INTAKE[1]);
     m_ultrasonicOutake = new Ultrasonic(Constants.ULTRASONIC_OUTAKE[0], Constants.ULTRASONIC_OUTAKE[1]);
     m_intakeButton = new DigitalInput(Constants.IntakeButton);