Button bindings

Charge2023Test/src/main/java/frc/robot/Constants.java

@@ -33,6 +33,10 @@ public static final class ClawConstants {
 
         public static final int kColorSensorPort = 0;
         public static final int kBeamBreakPort = 0;
+        public static final double kCubeCurrent = 0;
+        public static final double kConeCurrent = 0;
+
+
 
 
         public static final int kLeftPistonExtendChannel = 0;

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

@@ -4,24 +4,62 @@
 
 package frc.robot;
 
+
+import java.util.logging.Handler;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.PS4Controller.Axis;
 import edu.wpi.first.wpilibj.XboxController.Button;
-import frc.robot.commands.ChangeArmAngle;
-import frc.robot.commands.ExampleCommand;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.commandgroups.AutoDriveBackwards;
+import frc.robot.commandgroups.ChargeArm;
+import frc.robot.commandgroups.GridConeMid;
+import frc.robot.commandgroups.GridLow;
+import frc.robot.commandgroups.ShelfPickup;
+//import frc.robot.commandgroups.GridConeMid;
+//import frc.robot.commandgroups.GridCubeHigh;
+//import frc.robot.commandgroups.GridCubeMid;
 import frc.robot.subsystems.Arm;
+import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.Claw;
+
+//import frc.robot.commands.ChangeArmAngle;
+import frc.robot.commands.DriveManuallyArcade;
+import frc.robot.commands.ExampleCommand;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj2.command.button.POVButton;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.commands.ClawConePickup;
+import frc.robot.commands.ClawEject;
+import frc.robot.commands.ClawCubePickup;
+import frc.robot.commands.DriveDistance;
+import frc.robot.commands.PointTurnGyroPID;
+import frc.robot.commands.PointTurnGyroTank;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.commandgroups.AutoDriveBackwards;
+import frc.robot.commands.DriveDistance;
+import frc.robot.commands.DriveManuallyArcade;
+import frc.robot.subsystems.Drivetrain;
+import frc.robot.commands.PointTurnGyroTank;
 
 import static frc.robot.Constants.*;
+import static frc.robot.Constants.DriveConstants.*;
+import frc.robot.commands.ChangeLedColor;
+import frc.robot.subsystems.Lights;
+
 
 /**
- * This class is where the bulk of the robot should be declared. Since
- * Command-based is a
- * "declarative" paradigm, very little robot logic should actually be handled in
- * the {@link Robot}
- * periodic methods (other than the scheduler calls). Instead, the structure of
- * the robot (including
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
  * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
@@ -30,54 +68,133 @@ public class RobotContainer {
   private final XboxController m_driver = new XboxController(DriveConstants.kDriverControllerPort);
   private final XboxController m_operator = new XboxController(DriveConstants.kOperatorControllerPort);
 
+
   private final Arm arm;
+  private final Claw claw;
+  private final Drivetrain m_drivetrain;
 
-  /**
-   * The container for the robot. Contains subsystems, OI devices, and commands.
-   */
+
+  private final double straightSpeedFactor = 0.6;
+  private final double turnSpeedFactor = 0.5;
+  private final double straightDecelSpeedFactor = 0.7;
+  private final double turnDecelSpeedFactor = 0.4;
+
+
+
+  // private final Index m_index;
+  // private final Intake m_intake;
+  // private final Shooter m_shooter;
+  // private final Vision m_vision;
+  // private final Compressor m_testCompressor;
+
+  private final SendableChooser<Command> m_autoChooser;
+
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
 
     arm = new Arm();
+    m_drivetrain = new Drivetrain();
+    claw = new Claw();
+
+    m_autoChooser = new SendableChooser<>();
+    SmartDashboard.putData("Autonomous Selector", m_autoChooser);
+
+
+
+
+    m_drivetrain.setDefaultCommand(
+        new DriveManuallyArcade(() -> m_driver.getLeftY(), () -> m_driver.getRightX(), m_drivetrain));
+
+    SmartDashboard.putData("Autonomous Selector", m_autoChooser);
+
+
+
+    m_drivetrain.setDefaultCommand(
+        new DriveManuallyArcade(() -> m_driver.getLeftY(), () -> m_driver.getRightX(), m_drivetrain));
+
 
     // Configure the button bindings
     configureButtonBindings();
+
   }
 
+  //TAHA WUZ HERE!!! 
+
   /**
-   * Use this method to define your button->command mappings. Buttons can be
-   * created by
+   * Use this method to define your button->command mappings. Buttons can be created by
    * instantiating a {@link GenericHID} or one of its subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing
-   * it to a {@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
    * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   * @return 
    */
 
+
   private void configureButtonBindings() {
+    new Trigger(() -> (m_driver.getLeftTriggerAxis() > 0.01))
+        .onTrue(new DriveManuallyArcade(() -> (m_driver.getLeftY() * straightDecelSpeedFactor),
+        () -> (-m_driver.getRightX() * turnDecelSpeedFactor), m_drivetrain));
+    //Holding the trigger slows the speed in which the drivetrain travels in. If true, the drive train will travel at a decellerated speed. Press Again to return to normal operation. 
+
+
+
+   /* Trigger aOpButton = new JoystickButton(m_operator, Button.kA.value).onTrue(
+      new ChangeArmAngle(arm, arm.getArmAngle() + 30);
 
     new JoystickButton(m_operator, Button.kB.value).whileTrue(
-        new ChangeArmAngle(arm, 0));
+      new ChangeArmAngle(arm, arm.getArmAngle() - 30));
 
-    new JoystickButton(m_operator, Button.kY.value).whileTrue(
-        new ChangeArmAngle(arm, 90));
+      */
 
-    new JoystickButton(m_operator, Button.kX.value).whileTrue(
-        new ChangeArmAngle(arm, 180));
+      //add vision button binding (Either A,B, or X button Driver)
 
-    new JoystickButton(m_operator, Button.kA.value).whileTrue(
-        new ChangeArmAngle(arm, 270));
+//add dock and engage button binding (Y-Button Driver)
 
-  }
+//****Operator Controller Button bindings****
+//bOpButton
+//new JoystickButton(m_operator, Button.kB.value).onTrue(new GridCubeMid(arm, drivetrain));
+
 
-  /**
+
+
+
+//xOpButton 
+new JoystickButton(m_operator, Button.kX.value).onTrue(new GridConeMid(arm, m_drivetrain));
+
+//aOpButton  
+new JoystickButton(m_operator, Button.kA.value).onTrue(new GridLow(arm,m_drivetrain));
+
+//rBumpOpButton
+ new JoystickButton(m_operator, Button.kRightBumper.value).onTrue(new ClawCubePickup(claw));
+
+
+//rOpTrigger
+new Trigger(() -> (m_operator.getRightTriggerAxis() > 0.01))
+        .whileTrue(new ClawConePickup(claw));
+
+//lOpTrigger
+ new Trigger(() -> (m_operator.getLeftTriggerAxis() > 0.01))
+        .whileTrue(new ClawEject(claw));
+
+//downopDpad -- down d-pad button
+new POVButton(m_operator, 180).onTrue(new ChargeArm(arm));
+
+//leftopDpad left d-pad button
+new POVButton(m_operator, 270).whileTrue(new ShelfPickup(arm));
+
+      }
+
+
+
+/**
    * Use this to pass the autonomous command to the main {@link Robot} class.
-   * 
+
    * @return the command to run in autonomous
    */
 
   public Command getAutonomousCommand() {
-
+    
     // An ExampleCommand will run in autonomous
-    return new ExampleCommand(new ExampleSubsystem());
+    return m_autoChooser.getSelected();
 
   }
 

Charge2023Test/src/main/java/frc/robot/commands/ChangeLedColor.java

@@ -0,0 +1,41 @@
+// 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;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Lights;
+
+public class ChangeLedColor extends CommandBase {
+
+  private Lights m_Lights;
+  private double color;
+
+  /** Creates a new ChangeLedColor. */
+  public ChangeLedColor(Lights subsystem, double color) {
+    m_Lights = subsystem;
+    this.color = color;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_Lights.changeColor(color);
+  }
+
+  // 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 false;
+  }
+}

Charge2023Test/src/main/java/frc/robot/commands/ClawConePickup.java

@@ -11,7 +11,7 @@ public class ClawConePickup extends CommandBase {
   private static final double kRollerMotorStopSpeed = 0;
   private final Claw m_claw;
 
-  /** Creates a new ClawPickup. */
+  /** Creates a new ClawConePickup. */
   public ClawConePickup(Claw subsystem) {
 
     m_claw = subsystem;
@@ -29,18 +29,23 @@ public void initialize() {}
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
+    //initiates the cone pickup sequence by extending the pistons and then running the motors. The motors will then stop after the claw has a "grip" on the cone (stopRollerMotors is called)
+
   public void execute() {
-m_claw.extendLeftPiston();
-m_claw.extendRightPiston();
-m_claw.runRollerMotors(0);
-m_claw.stopRollerMotors(kRollerMotorStopSpeed);
+//the pistons (which are connected to one roller) close in to squeeze the cone. *the retract values (located in claw constants) may need to be tweaked during testing to see which length is the correct length for the cone pickup.
+m_claw.retractLeftPiston();
+m_claw.retractRightPiston();
+
+m_claw.runRollerMotors(0); //the motors spin to intake the cone upwards
+m_claw.stopRollerMotorsCone(kRollerMotorStopSpeed); //once the current limit is reached, this should mean that the cone is being held and the motors are ready to stop. 
 
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    m_claw.haltMotors(0);
+    //motors are completely stopped
+    m_claw.haltMotors(0); 
   }
 
   // Returns true when the command should end.

Charge2023Test/src/main/java/frc/robot/commands/ClawPickup.java → Charge2023Test/src/main/java/frc/robot/commands/ClawCubePickup.java

@@ -7,12 +7,12 @@
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.Claw;
 
-public class ClawPickup extends CommandBase {
+public class ClawCubePickup extends CommandBase {
   private static final double kRollerMotorStopSpeed = 0;
   private final Claw m_claw;
 
-  /** Creates a new ClawPickup. */
-  public ClawPickup(Claw subsystem) {
+  /** Creates a new ClawCubePickup. */
+  public ClawCubePickup(Claw subsystem) {
 
     m_claw = subsystem;
 
@@ -30,8 +30,10 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    //motors run at a given speed and then stop once it recognizes that it is holding a cube.
 m_claw.runRollerMotors(0);
-m_claw.stopRollerMotors(kRollerMotorStopSpeed);
+
+m_claw.stopRollerMotorsCube(kRollerMotorStopSpeed);
   }
 
   // Called once the command ends or is interrupted.

Charge2023Test/src/main/java/frc/robot/commands/ClawEject.java

@@ -28,17 +28,22 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    //reverses the motors so the game piece is being ejected.
     m_claw.ejectRollerMotors(kshootSpeed);
 
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    m_claw.retractLeftPiston();
-    m_claw.retractRightPiston();
+    //after the game piece is ejected, the pistons return to the default position and motors are stopped.
+    if ((hasPieceCone() || hasPieceCube()) == false)) {
+    m_claw.extendLeftPiston();
+    m_claw.extendRightPiston();
     m_claw.haltMotors(0);
 
+    }
+
   }
 
   // Returns true when the command should end.

Charge2023Test/src/main/java/frc/robot/commands/DriveManuallyTank.java

@@ -5,14 +5,18 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Drivetrain;
 
 public class DriveManuallyTank extends CommandBase {
   /** Creates a new DriveManuallyTank. */
   public DriveManuallyTank() {
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
-  // Called when the command is initially scheduled.
+
+
+
+// Called when the command is initially scheduled.
   @Override
   public void initialize() {}