Merge pull request #16 from primo4586/feeder-intake

command group feeder intake

Author: ori-coval

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

@@ -54,6 +54,8 @@ public static final class IntakeArm{ //TODO: This must be tuned to specific robo
         public static final double startingValue = 0;
         public static final double zeroEncoderValue = 0;
         public static final double intakeArmSpeed = 0;
+        public static final double intakeSetPoint = 0;
+        public static final double trapSetPoint = 0;
         // switch
         public static final int switchID = 0;
 
@@ -322,6 +324,7 @@ public static class FeederConstants{
     public static final double FeederMotorSpeed = 0.8;
     public static final double getNoteSpeed = 0;
 
+
   }
 
     public static class TrapArmConstants{

src/main/java/frc/robot/commands/IntakeArmCommands/SetSpeed.java renamed to src/main/java/frc/robot/commands/IntakeArmCommands/IntakeArmSetSpeed.java

@@ -10,10 +10,10 @@
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
-public class SetSpeed extends InstantCommand {
+public class IntakeArmSetSpeed extends InstantCommand {
   private final IntakeArmSubsystem intakeArm = IntakeArmSubsystem.getInstance();
   private double speed;
-  public SetSpeed(double speed) {
+  public IntakeArmSetSpeed(double speed) {
     // Use addRequirements() here to declare subsystem dependencies.
     this.speed = speed;
   }

src/main/java/frc/robot/commands/feederCommands/FeederIntakeCommandGroup.java

@@ -0,0 +1,29 @@
+// 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.feederCommands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.IntakeArmCommands.MoveIntakeArmToDegree;
+import frc.robot.commands.IntakeCommands.CollectToFeeder;
+import frc.robot.commands.IntakeCommands.IntakeSetSpeed;
+import static frc.robot.Constants.IntakeArm.*;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class FeederIntakeCommandGroup extends SequentialCommandGroup {
+
+  /** Creates a new FeederIntakeCommandGroup. */
+  
+  public FeederIntakeCommandGroup() {
+    // Add your commands in the addCommands() call, e.g.
+    // addCommands(new FooCommand(), new BarCommand());
+    addCommands(
+        new MoveIntakeArmToDegree(intakeSetPoint),
+        new CollectToFeeder(),
+        new IntakeSetSpeed(0),
+        new MoveIntakeArmToDegree(trapSetPoint));
+  }
+}