Merge branch 'pole-calib' of https://github.com/Bearbots6964/FRC2023 …

…into pole-calib
Bearbots6964 · Mar 28, 2023 · 0865843 · 0865843
2 parents a65ae8c + c2e5d34
commit 0865843
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 6 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -44,7 +44,7 @@ public class RobotContainer {
   private final AutoCommand m_AutoCommand = new AutoCommand(m_PID, m_Tank, m_Claw, m_Arm);
   private final InvertDriveCommand m_InvertDriveCommand = new InvertDriveCommand(m_Tank, this);
   private final PlaceConeSecondLevelCommand m_PlaceConeSecondLevelCommand =
-      new PlaceConeSecondLevelCommand(m_Tank, m_Arm);
+      new PlaceConeSecondLevelCommand(m_Tank, m_Arm, m_claw);
   private final IncreaseMaxSpeedCommand m_IncreaseMaxSpeedCommand =
       new IncreaseMaxSpeedCommand(m_Tank);
   private final DecreaseMaxSpeedCommand m_DecreaseMaxSpeedCommand =

diff --git a/src/main/java/frc/robot/commands/AutoCommand.java b/src/main/java/frc/robot/commands/AutoCommand.java
@@ -8,10 +8,10 @@
 import frc.robot.subsystems.*;
 
 public class AutoCommand extends SequentialCommandGroup {
-  public AutoCommand(PID m_pid, Tank m_driveBase, Claw m_claw, Arm m_arm) {
-    addCommands(new PlaceConeSecondLevelCommand(m_driveBase, m_arm), 
+  public AutoCommand(PID m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) {
+    addCommands(new PlaceConeSecondLevelCommand(m_driveBase, m_arm, m_claw));
 
-    new BalanceCommand(m_pid, m_driveBase));
+    //new BalanceCommand(m_pid, m_driveBase))
     // new PlaceCubeFirstLevelCommand(m_driveBase, m_claw, m_arm)
     // new PlaceConeSecondLevelCommand(m_driveBase, m_arm)
   }

diff --git a/src/main/java/frc/robot/commands/PlaceConeSecondLevelCommand.java b/src/main/java/frc/robot/commands/PlaceConeSecondLevelCommand.java
@@ -10,14 +10,15 @@
 public class PlaceConeSecondLevelCommand extends CommandBase {
   @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
   private final Tank drive;
-
+  private final Claw claw;
   private final Arm arm;
 
   private boolean firstStep = true;
 
-  public PlaceConeSecondLevelCommand(Tank drive, Arm arm) {
+  public PlaceConeSecondLevelCommand(Tank drive, Arm arm, Claw claw) {
     this.drive = drive;
     this.arm = arm;
+    this.claw = claw;
     addRequirements(drive);
     addRequirements(arm);
   }
@@ -37,6 +38,7 @@ public void initialize() {
   public void execute() {
     if (firstStep) {
       if (Math.abs(arm.armMotor.getEncoder().getPosition()) < 105) {
+        claw.closeClaw();
         arm.armMotor.set(0.4);
       }
 
@@ -48,6 +50,7 @@ public void execute() {
 
       // go over charge station
       if (Math.abs(drive.getAverageDistance()) < 4.6 && firstStep == false) {
+        claw.stopClaw(); 
         drive.setAllMotors(-0.3); // move back so that cone falls in
         arm.armMotor.set(-0.45);
       }