Merge pull request #238 from CyberCoyotes/SZ-3-14-Schizo

Sz 3 14 schizo; fix to the auto-intake and auto-index

src/main/java/frc/robot/ChargeIntakeCommand.java.txt

@@ -0,0 +1,50 @@
+package frc.robot;
+
+import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.IntakeSubsystem;
+
+public class ChargeIntakeCommand extends Command {
+
+    CommandSwerveDrivetrain m_driveTrain;
+    IntakeSubsystem intake;
+    SwerveRequest newRequest;
+    SwerveRequest oldRequest;
+
+    @Override
+    public void execute() {
+
+        super.execute();
+        m_driveTrain.applyRequest(() -> newRequest);
+        intake.Run(Constants.IntakeConstants.INTAKE_POWER);
+    }
+
+    public ChargeIntakeCommand(CommandSwerveDrivetrain dt, IntakeSubsystem intake,
+            SwerveRequest.FieldCentric oldRequest) {
+        this.oldRequest = oldRequest;
+        this.m_driveTrain = dt;
+        this.intake = intake;
+        addRequirements(intake, m_driveTrain);
+
+    }
+
+    @Override
+    public void initialize() {
+        super.initialize();
+
+        double velo = TunerConstants.kSpeedAt12VoltsMps;
+        newRequest = new SwerveRequest.RobotCentric().withVelocityY(-velo);
+    }
+
+    public ChargeIntakeCommand() {
+        this.addRequirements(m_driveTrain);
+
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        m_driveTrain.applyRequest(() -> oldRequest);
+    }
+}

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

@@ -4,7 +4,15 @@
 
 package frc.robot;
 
+import com.ctre.phoenix6.SignalLogger;
+
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.cscore.CvSink;
+import edu.wpi.first.cscore.CvSource;
+import edu.wpi.first.cscore.VideoSource;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -16,21 +24,33 @@ public class Robot extends TimedRobot {
   @Override
   public void robotInit() {
     m_robotContainer = new RobotContainer();
+    SignalLogger.setPath("/media/sda1/");
+    // SignalLogger.start();
+    CameraServer.startAutomaticCapture();
+    // Creates the CvSink and connects it to the UsbCamera
+    CvSink cvSink = CameraServer.getVideo();
+        NetworkTableInstance.getDefault().getTable("limelight-speedy").getEntry("stream").setNumber(2);
+
+    // Creates the CvSource and MjpegServer [2] and connects them
+
   }
 
   @Override
   public void robotPeriodic() {
-    CommandScheduler.getInstance().run(); 
+    CommandScheduler.getInstance().run();
   }
 
   @Override
-  public void disabledInit() {}
+  public void disabledInit() {
+  }
 
   @Override
-  public void disabledPeriodic() {}
+  public void disabledPeriodic() {
+  }
 
   @Override
-  public void disabledExit() {}
+  public void disabledExit() {
+  }
 
   @Override
   public void autonomousInit() {
@@ -42,10 +62,12 @@ public void autonomousInit() {
   }
 
   @Override
-  public void autonomousPeriodic() {}
+  public void autonomousPeriodic() {
+  }
 
   @Override
-  public void autonomousExit() {}
+  public void autonomousExit() {
+  }
 
   @Override
   public void teleopInit() {
@@ -55,22 +77,27 @@ public void teleopInit() {
   }
 
   @Override
-  public void teleopPeriodic() {}
+  public void teleopPeriodic() {
+  }
 
   @Override
-  public void teleopExit() {}
+  public void teleopExit() {
+  }
 
   @Override
   public void testInit() {
     CommandScheduler.getInstance().cancelAll();
   }
 
   @Override
-  public void testPeriodic() {}
+  public void testPeriodic() {
+  }
 
   @Override
-  public void testExit() {}
+  public void testExit() {
+  }
 
   @Override
-  public void simulationPeriodic() {}
+  public void simulationPeriodic() {
+  }
 }

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

@@ -9,6 +9,8 @@
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.cscore.VideoSource;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@@ -30,6 +32,7 @@
 import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.IndexSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.Limelight;
 import frc.robot.subsystems.ShooterSubsystemVelocity;
 import frc.robot.subsystems.WinchSubsystem2;
 
@@ -38,7 +41,7 @@
 public class RobotContainer {
 
   RunShooter shooterRun;
-
+  SwerveRequest.FieldCentric driveRequest;
   // private final Telemetry logger = new
   // Telemetry(Constants.SystemConstants.MAX_SPEED);
   // #endregion
@@ -57,7 +60,7 @@ public class RobotContainer {
 
   // #region commands
 
-  // #endregion 
+  // #endregion
 
   private double MaxSpeed = TunerConstants.kSpeedAt12VoltsMps; // kSpeedAt12VoltsMps desired top speed
   private double MaxAngularRate = 1.5 * Math.PI; // 3/4 of a rotation per second max angular velocity
@@ -83,21 +86,27 @@ public class RobotContainer {
 
   private final IntakeCommandGroup intakeGroup = new IntakeCommandGroup(index, intake);
   private final IntakeRevCommandGroup intakeRevGroup = new IntakeRevCommandGroup(index, intake);
+  // ChargeIntakeCommand chargeIntake = new ChargeIntakeCommand(drivetrain, intake, driveRequest);
 
-  /* Autonomous Chooser*/
+  /* Autonomous Chooser */
   SendableChooser<Command> autoChooser;
 
   // Constructor of the class
   public RobotContainer() {
+    Limelight lime = new Limelight();
 
-    /*Pathplanner Named Commands*/
+    /* Pathplanner Named Commands */
     NamedCommands.registerCommand("ShootClose", new ShootClose(arm, index, intake, shooter));
     NamedCommands.registerCommand("Intake", new IntakeIndex(index, intake));
-    // NamedCommands.registerCommand("ArmMid", new SetArmPosition(arm, Constants.ArmConstants.ARM_MID_POSE));
-
-    /* Auto Chooser
-    * Unintended side effect is this will create EVERY auton file from the RIO deploy folder.
-    * FTP into the the RIO to delete old auton options */
+    // NamedCommands.registerCommand("ArmMid", new SetArmPosition(arm,
+    // Constants.ArmConstants.ARM_MID_POSE));
+
+    /*
+     * Auto Chooser
+     * Unintended side effect is this will create EVERY auton file from the RIO
+     * deploy folder.
+     * FTP into the the RIO to delete old auton options
+     */
     autoChooser = AutoBuilder.buildAutoChooser();
     SmartDashboard.putData("Auto Chooser", autoChooser);
     // Shuffleboard.getTab("Auton").add("Auto Chooser", autoChooser);
@@ -111,77 +120,85 @@ public RobotContainer() {
 
   }
 
-  /* Method to check our Alliance status for Red or Blue
-   * This is essential for the autonomous mode to know which side of the field we are on */
+  /*
+   * Method to check our Alliance status for Red or Blue
+   * This is essential for the autonomous mode to know which side of the field we
+   * are on
+   */
   public static boolean isAllianceRed() {
     return DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red;
   }
 
   /* Method to configure the button bindings */
   private void configureBindings() {
-
-    drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically
-      // Drive forward with negative Y (forward)
-        drivetrain.applyRequest(() -> drive.withVelocityX(-m_driverController.getLeftY() * MaxSpeed) 
+    index.setDefaultCommand(index.run(() -> index.SetPower(0)));
+            driveRequest = drive.withVelocityX(-m_driverController.getLeftY() * MaxSpeed)
             .withVelocityY(-m_driverController.getLeftX() * MaxSpeed) // Drive left with negative X (left)
             .withRotationalRate(-m_driverController.getRightX() * MaxAngularRate) // Drive counterclockwise with
                                                                                   // negative X (left)
-        ));
+        ;
+    drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically
+        // Drive forward with negative Y (forward)
+
+        drivetrain.applyRequest(() -> driveRequest));
 
     // m_driverController.a().whileTrue(drivetrain.applyRequest(() -> brake));
-    /* m_driverController.b().whileTrue(drivetrain
-        .applyRequest(() -> point
-            .withModuleDirection(new Rotation2d(-m_driverController.getLeftY(), -m_driverController.getLeftX()))));
-    */
-
+    /*
+     * m_driverController.b().whileTrue(drivetrain
+     * .applyRequest(() -> point
+     * .withModuleDirection(new Rotation2d(-m_driverController.getLeftY(),
+     * -m_driverController.getLeftX()))));
+     */
+
     // reset the field-centric heading
-    /* 
-    m_driverController.b().whileTrue(drivetrain.applyRequest(() -> point
-    .withModuleDirection(new Rotation2d(-m_driverController.getLeftY(),
-    -m_driverController.getLeftX()))));
-    */
+    /*
+     * m_driverController.b().whileTrue(drivetrain.applyRequest(() -> point
+     * .withModuleDirection(new Rotation2d(-m_driverController.getLeftY(),
+     * -m_driverController.getLeftX()))));
+     */
 
     m_driverController.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative()));
     m_driverController.a().whileTrue(new InstantCommand(() -> arm.setArmPose(Constants.ArmConstants.ARM_HOME_POSE)));
     m_driverController.b().whileTrue(new InstantCommand(() -> arm.setArmPose(Constants.ArmConstants.ARM_MID_POSE)));
     m_driverController.x().whileTrue(new InstantCommand(() -> arm.setArmPose(Constants.ArmConstants.ARM_AMP_POSE)));
     m_driverController.y().whileTrue(new InstantCommand(() -> arm.setArmPose(Constants.ArmConstants.ARM_MID_POSE)));
-    
+
     m_driverController.rightBumper().whileTrue(new IntakeCommandGroup(index, intake));
     m_driverController.leftBumper().whileTrue(new IntakeRevCommandGroup(index, intake));
     m_driverController.rightTrigger().whileTrue(new RevAndShootCommand(index, shooter));
     m_driverController.rightTrigger().whileFalse(new InstantCommand(() -> shooter.SetOutput(0)));
-    m_driverController.leftTrigger().whileTrue(new SetIndex(index,-0.75));
+    m_driverController.leftTrigger().whileTrue(new SetIndex(index, -0.75));
 
     // m_operatorController.a().whileTrue (new));
-    m_operatorController.b().whileTrue(new SetArmClimb(arm,Constants.ArmConstants.ARM_MANUAL_POWER));
+    m_operatorController.b().whileTrue(new SetArmClimb(arm, Constants.ArmConstants.ARM_MANUAL_POWER));
     // m_operatorController.x().whileTrue (new));
     m_operatorController.y().whileTrue(new SetWinch(winch, Constants.WinchConstants.WINCH_POWER));
     m_operatorController.back().whileTrue(new SetWinch(winch, Constants.WinchConstants.WINCH_POWER_BOOST));
-
+    // m_operatorController.start().whileTrue();
+
   };
 
   /* Use for Debugging and diagnostics purposes */
   public void DebugMethodSingle() {
     // #region Driving
-
+    var driverDiagnostics = Shuffleboard.getTab("Driver Diagnostics");
     // #endregion Driving
     // #region Testing
 
     // Less useful logs that we still need to see for testing.
 
-    // var driverDiagnostics = Shuffleboard.getTab("Diagnostics");
-
-    // driverDiagnostics.addDouble("Arm Rot", () -> arm.GetArmPos().getValueAsDouble());
+    // driverDiagnostics.addDouble("Arm Rot", () ->
+    // arm.GetArmPos().getValueAsDouble());
     // driverDiagnostics.addDouble("Arm Rot Deg", () -> arm.GetPositionDegrees());
     // arm.showArmTelemetry("Driver Diagnostics");
     // Shuffleboard.getTab("Arm").add("Arm Output", arm);
 
     // #endregion Testing
   }
+
 
   public Command getAutonomousCommand() {
     return autoChooser.getSelected();
-  }
+  }  
 
 } // End of class

src/main/java/frc/robot/commands/IncrementIndex.java

@@ -0,0 +1,27 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.IndexSubsystem;
+
+public class IncrementIndex extends Command {
+
+    IndexSubsystem indexer;
+
+    public IncrementIndex(IndexSubsystem indexer) {
+        this.indexer = indexer;
+
+        addRequirements(indexer);
+    }
+
+    @Override
+    public void execute() {
+        indexer.SetPower(-0.1);
+    }
+
+    @Override
+    public boolean isFinished() {
+        // Check if there is no note loaded. i.e. HasCargo() returns false
+        return !indexer.HasCargo();
+    }
+
+}

src/main/java/frc/robot/commands/RevAndShootCommand.java

@@ -23,14 +23,14 @@ public class RevAndShootCommand extends SequentialCommandGroup {
     IndexSubsystem indexer;
     ShooterSubsystemVelocity shooter;
     // component Commands
-    // private IncrementIndex outtaMyWay;
+    private IncrementIndex outtaMyWay;
     private Command revUpShooter;
     private Command indexCommand;
 
     public RevAndShootCommand(IndexSubsystem indexer) {
         // Set up our subsystems
         this.indexer = indexer;
-        // outtaMyWay = new IncrementIndex(indexer);
+        outtaMyWay = new IncrementIndex(indexer);
     }