Final oi

2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java

@@ -36,7 +36,7 @@ public class RobotContainer extends RobotContainerBase {
     private final Vision vision = new Vision(pd);
     private final DriveBase driveBase = new DriveBase(getDriverController());
     private final Arm arm = new Arm();
-    private final Claw claw = new Claw(arm);
+    private final Claw claw = new Claw();
     private static OI oi = new OI();
 
     private Notifier updateArbitraryFeedForwards;

2023-Robot/src/main/java/frc/team670/robot/commands/arm/ManualMoveElbow.java

@@ -30,7 +30,7 @@ public ManualMoveElbow(Arm arm, MustangController controller) {
     @Override
     public void execute() {
 
-        arm.setElbowOffset(arm.getElbow().getOffset() + controller.getLeftY());
+        arm.adjustElbowOffset(-controller.getLeftY());
 
     }
 

2023-Robot/src/main/java/frc/team670/robot/commands/arm/ManualMoveShoulder.java

@@ -30,7 +30,7 @@ public ManualMoveShoulder(Arm arm, MustangController controller) {
     @Override
     public void execute() {
 
-        arm.setShoulderOffset(arm.getShoulder().getOffset() + controller.getRightY());
+        arm.adjustShoulderOffset(controller.getRightY());
 
     }
 

2023-Robot/src/main/java/frc/team670/robot/commands/arm/MoveToTarget.java

@@ -14,7 +14,9 @@
 import frc.team670.mustanglib.subsystems.MustangSubsystemBase.HealthState;
 import frc.team670.mustanglib.utils.Logger;
 import frc.team670.robot.commands.claw.ClawIntake;
+import frc.team670.robot.commands.routines.ClawToggle;
 import frc.team670.robot.subsystems.Claw;
+import frc.team670.robot.subsystems.Claw.Status;
 import frc.team670.robot.subsystems.arm.Arm;
 import frc.team670.robot.subsystems.arm.ArmState;
 
@@ -39,18 +41,12 @@ public class MoveToTarget extends CommandGroupBase implements MustangCommand {
   private Map<MustangSubsystemBase, HealthState> healthReqs;
   private ArmState target;
   private Arm arm;
-  private Claw claw;
 
   private final List<Command> m_commands = new ArrayList<>();
   private int m_currentCommandIndex = -1;
   private boolean m_runWhenDisabled = true;
   private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelSelf;
 
-  public MoveToTarget(Arm arm, Claw claw, ArmState target) {
-    this(arm, target);
-    this.claw = claw;
-  }
-
   public MoveToTarget(Arm arm, ArmState target) {
     healthReqs = new HashMap<MustangSubsystemBase, HealthState>();
     healthReqs.put(arm, HealthState.GREEN);
@@ -69,12 +65,10 @@ public void initialize() {
     // 3) then call move directly to target for each of those returned paths
     m_commands.clear();
     ArmState[] path = Arm.getValidPath(arm.getTargetState(), target);
+
     for (int i = 1; i < path.length; i++) {
       addCommands(new MoveDirectlyToTarget(arm, path[i]));
     }
-    if (target != ArmState.STOWED && claw != null) {
-      addCommands(new ClawIntake(claw));
-    }
 
     m_currentCommandIndex = 0;
 

2023-Robot/src/main/java/frc/team670/robot/commands/claw/ClawIntake.java

@@ -4,6 +4,8 @@
 import frc.team670.mustanglib.subsystems.MustangSubsystemBase.HealthState;
 import frc.team670.mustanglib.commands.MustangCommand;
 import java.util.Map;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.team670.robot.subsystems.Claw;
 import frc.team670.robot.subsystems.Claw.Status;
@@ -12,19 +14,23 @@
  * @author Tarini, Samanyu and Ishaan
  */
 
-public class ClawIntake extends InstantCommand implements MustangCommand {
+public class ClawIntake extends CommandBase implements MustangCommand {
     private Claw claw;
-
-    public ClawIntake(Claw claw) {
+    private boolean waitTillFull;
+    public ClawIntake(Claw claw,boolean waitTillFull) {
         this.claw = claw;
+        this.waitTillFull=waitTillFull;
         addRequirements(claw);
     }
 
     @Override
-    public void execute() {
+    public void initialize(){
         claw.setStatus(Status.INTAKING);
     }
-
+    @Override
+    public boolean isFinished(){
+        return !waitTillFull||claw.isFull();
+    }
     @Override
     public Map<MustangSubsystemBase, HealthState> getHealthRequirements() {
         return null;

2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/MoveToPose.java

@@ -44,12 +44,12 @@ public MoveToPose(DriveBase driveBase, Pose2d goalPose) {
     }
 
     // public MoveToPose(DriveBase driveBase, Pose2d goalPose) {
-    //     this.driveBase = driveBase;
-    //     path = null;
-    //     this.goalPose = goalPose;
-    //     this.healthReqs = new HashMap<MustangSubsystemBase, HealthState>();
-    //     this.healthReqs.put(driveBase, HealthState.GREEN);
-    //     addRequirements(driveBase);
+    // this.driveBase = driveBase;
+    // path = null;
+    // this.goalPose = goalPose;
+    // this.healthReqs = new HashMap<MustangSubsystemBase, HealthState>();
+    // this.healthReqs.put(driveBase, HealthState.GREEN);
+    // addRequirements(driveBase);
     // }
 
     @Override
@@ -59,8 +59,12 @@ public Map<MustangSubsystemBase, HealthState> getHealthRequirements() {
 
     @Override
     public void initialize() {
-        goalPose = FieldConstants.allianceFlip(goalPose).transformBy(new Transform2d(FieldConstants.allianceFlip(new Translation2d(RobotConstants.DRIVEBASE_WIDTH + 0.1, 0)), FieldConstants.getRobotFacingRotation()));
-        SmartDashboard.putString("GOAL POSE", String.format("%f, %f, %f degrees", goalPose.getX(), goalPose.getY(), goalPose.getRotation().getDegrees()));
+        goalPose = FieldConstants.allianceFlip(goalPose)
+                .transformBy(new Transform2d(
+                        FieldConstants.allianceFlip(new Translation2d(RobotConstants.DRIVEBASE_WIDTH + 0.1, 0)),
+                        FieldConstants.getRobotFacingRotation()));
+        SmartDashboard.putString("GOAL POSE", String.format("%f, %f, %f degrees", goalPose.getX(), goalPose.getY(),
+                goalPose.getRotation().getDegrees()));
         PathPlannerTrajectory traj = PathPlanner.generatePath(new PathConstraints(1, 0.5), calcStartPoint(),
                 calcEndPoint(goalPose));
         driveBase.getPoseEstimator().addTrajectory(traj);
@@ -77,15 +81,14 @@ public boolean isFinished() {
     @Override
     public void end(boolean interrupted) {
         if (interrupted) {
-			pathDrivingCommand.cancel();
+            pathDrivingCommand.cancel();
             driveBase.getPoseEstimator().removeTrajectory();
-		}
+        }
 
-		driveBase.stop();
+        driveBase.stop();
         // driveBase.getPoseEstimator().removeTrajectory();
     }
 
-
     private PathPoint calcStartPoint() {
         return new PathPoint(driveBase.getPoseEstimator().getCurrentPose().getTranslation(), new Rotation2d(),
                 driveBase.getGyroscopeRotation());

2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ConeCube.java

@@ -20,8 +20,10 @@
 import frc.team670.robot.commands.arm.MoveToTarget;
 import frc.team670.robot.commands.claw.ClawEject;
 import frc.team670.robot.commands.claw.ClawIntake;
+import frc.team670.robot.commands.routines.ClawToggle;
 import frc.team670.robot.subsystems.Claw;
 import frc.team670.robot.subsystems.DriveBase;
+import frc.team670.robot.subsystems.Claw.Status;
 import frc.team670.robot.subsystems.arm.Arm;
 import frc.team670.robot.subsystems.arm.ArmState;
 
@@ -33,40 +35,39 @@ public Map<MustangSubsystemBase, HealthState> getHealthRequirements() {
         return new HashMap<MustangSubsystemBase, HealthState>();
     }
 
-
     public ConeCube(DriveBase driveBase, Claw claw, Arm arm, String pathName) {
         this.pathName = pathName;
         List<PathPlannerTrajectory> trajectoryGroup = PathPlanner.loadPathGroup(pathName, 2.0, 1.0);
 
         PIDConstants PID_translation = new PIDConstants(1.0, 0, 0);
-        PIDConstants PID_theta = new PIDConstants(1.0, 0, 0); 
+        PIDConstants PID_theta = new PIDConstants(1.0, 0, 0);
 
         driveBase.resetOdometry(trajectoryGroup.get(0).getInitialHolonomicPose());
 
         HashMap<String, Command> eventMap = new HashMap<>();
 
         // eventMap stuff
-        eventMap.put("clawIntake1", new ClawIntake(claw));
+        eventMap.put("clawIntake1", new ClawIntake(claw, false));
         eventMap.put("moveToHigh1", new MoveToTarget(arm, ArmState.SCORE_HIGH));
         eventMap.put("clawEject1", new ClawEject(claw));
         eventMap.put("moveToBackward", new MoveToTarget(arm, ArmState.BACKWARD_GROUND));
-        eventMap.put("clawIntake2", new ClawIntake(claw));
+        eventMap.put("clawIntake2", new ClawIntake(claw, false));
         eventMap.put("moveToHigh2", new MoveToTarget(arm, ArmState.SCORE_HIGH));
-        eventMap.put("clawEject2", new ClawEject(claw));
-        eventMap.put("moveToStowed", new MoveToTarget(arm, ArmState.STOWED));
-
+        eventMap.put("clawEject2", new ClawToggle(claw, arm, Status.EJECTING));
+
         SwerveDriveKinematics driveBaseKinematics = driveBase.getSwerveKinematics();
 
         SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder(driveBase.getPoseEstimator()::getCurrentPose,
                 driveBase::resetOdometry, driveBaseKinematics, PID_translation, PID_theta,
-                driveBase::setModuleStates, eventMap, false, new Subsystem[] {driveBase});
+                driveBase::setModuleStates, eventMap, false, new Subsystem[] { driveBase });
 
         CommandBase fullAuto = autoBuilder.fullAuto(trajectoryGroup);
 
         addCommands(fullAuto);
     }
 
-    // not exactly needed right now bc markers and events are the same for both sides (maybe
+    // not exactly needed right now bc markers and events are the same for both
+    // sides (maybe
     // utilized later)
 
     // public HashMap<String, Command> initialzeEventMap() {

2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CubeEngage.java

@@ -42,11 +42,10 @@ public CubeEngage(DriveBase driveBase, Claw claw, Arm arm, String pathName) {
 
         Map<String, Command> eventMap = new HashMap<>();
 
-        eventMap.put("clawIntake1", new ClawIntake(claw));
         eventMap.put("moveToHigh", new MoveToTarget(arm, ArmState.SCORE_HIGH));
         eventMap.put("clawEject", new ClawEject(claw));
         eventMap.put("moveToGround", new MoveToTarget(arm, ArmState.BACKWARD_GROUND));
-        eventMap.put("clawIntake2", new ClawIntake(claw));
+        eventMap.put("clawIntake2", new ClawIntake(claw, false));
         eventMap.put("autoLevel", new NonPidAutoLevel(driveBase, false)); // regardless of what side
                                                                           // (right/left) you are
                                                                           // on, markers are the
@@ -56,7 +55,7 @@ public CubeEngage(DriveBase driveBase, Claw claw, Arm arm, String pathName) {
 
         SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder(driveBase::getOdometerPose,
                 driveBase::resetOdometry, driveBaseKinematics, PID_translation, PID_theta,
-                driveBase::setModuleStates, eventMap, false, new Subsystem[] {driveBase});
+                driveBase::setModuleStates, eventMap, false, new Subsystem[] { driveBase });
 
         CommandBase fullAuto = autoBuilder.fullAuto(trajectoryGroup);
 

2023-Robot/src/main/java/frc/team670/robot/commands/routines/ClawToggle.java

@@ -0,0 +1,144 @@
+package frc.team670.robot.commands.routines;
+
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandGroupBase;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.team670.mustanglib.commands.MustangCommand;
+import frc.team670.mustanglib.subsystems.MustangSubsystemBase;
+import frc.team670.mustanglib.subsystems.MustangSubsystemBase.HealthState;
+import frc.team670.mustanglib.utils.Logger;
+import frc.team670.robot.commands.arm.MoveToTarget;
+import frc.team670.robot.commands.claw.ClawEject;
+import frc.team670.robot.commands.claw.ClawIdle;
+import frc.team670.robot.commands.claw.ClawIntake;
+import frc.team670.robot.subsystems.Claw;
+import frc.team670.robot.subsystems.Claw.Status;
+
+import frc.team670.robot.subsystems.arm.Arm;
+import frc.team670.robot.subsystems.arm.ArmState;
+
+//An implementation of an automatic claw decision program. To ensure that dynamic decisions are made in toggle, we have to make decisions on init. As such, we copied and pasted a clas from wpilib
+public class ClawToggle extends CommandGroupBase implements MustangCommand {
+    private Map<MustangSubsystemBase, HealthState> healthReqs;
+    private Status status;
+    private Claw claw;
+    private Arm arm;
+
+    public ClawToggle(Claw claw, Arm arm, Status status) {
+        healthReqs = new HashMap<MustangSubsystemBase, HealthState>();
+        healthReqs.put(claw, HealthState.GREEN);
+        this.claw = claw;
+        this.arm = arm;
+    }
+
+    private final List<Command> m_commands = new ArrayList<>();
+    private int m_currentCommandIndex = -1;
+    private boolean m_runWhenDisabled = true;
+    private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelSelf;
+
+    @Override
+    public void initialize() {
+        Logger.consoleLog("Ran ToggleClaw with the status " + status.toString());
+        m_commands.clear();
+        if (status == Status.EJECTING) {
+            addCommands(new ClawEject(claw), new MoveToTarget(arm, ArmState.STOWED));
+            healthReqs.put(arm, HealthState.GREEN);
+        } else if (status == Status.INTAKING) {
+            if (!claw.isFull()) {
+                addCommands(new ClawIntake(claw, true), new MoveToTarget(arm, ArmState.STOWED));
+                healthReqs.put(arm, HealthState.GREEN);
+            } else {
+                addCommands(new ClawIntake(claw, false));
+            }
+
+        } else if (status == Status.IDLE) {
+            addCommands(new ClawIdle(claw), new MoveToTarget(arm, ArmState.STOWED));
+            healthReqs.put(arm, HealthState.GREEN);
+        }
+        m_currentCommandIndex = 0;
+        if (!m_commands.isEmpty()) {
+            m_commands.get(0).initialize();
+        }
+    }
+
+    @Override
+    public Map<MustangSubsystemBase, HealthState> getHealthRequirements() {
+        return healthReqs;
+    }
+
+    @Override
+    public final void addCommands(Command... commands) {
+        if (m_currentCommandIndex != -1) {
+            throw new IllegalStateException(
+                    "Commands cannot be added to a composition while it's running");
+        }
+
+        CommandScheduler.getInstance().registerComposedCommands(commands);
+
+        for (Command command : commands) {
+            m_commands.add(command);
+            m_requirements.addAll(command.getRequirements());
+            m_runWhenDisabled &= command.runsWhenDisabled();
+            if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
+                m_interruptBehavior = InterruptionBehavior.kCancelSelf;
+            }
+        }
+    }
+
+    @Override
+    public final void execute() {
+        if (m_commands.isEmpty()) {
+            return;
+        }
+
+        Command currentCommand = m_commands.get(m_currentCommandIndex);
+
+        currentCommand.execute();
+        if (currentCommand.isFinished()) {
+            currentCommand.end(false);
+            m_currentCommandIndex++;
+            if (m_currentCommandIndex < m_commands.size()) {
+                m_commands.get(m_currentCommandIndex).initialize();
+            }
+        }
+    }
+
+    @Override
+    public final void end(boolean interrupted) {
+        if (interrupted
+                && !m_commands.isEmpty()
+                && m_currentCommandIndex > -1
+                && m_currentCommandIndex < m_commands.size()) {
+            m_commands.get(m_currentCommandIndex).end(true);
+        }
+        m_currentCommandIndex = -1;
+    }
+
+    @Override
+    public final boolean isFinished() {
+        return m_currentCommandIndex == m_commands.size();
+    }
+
+    @Override
+    public boolean runsWhenDisabled() {
+        return m_runWhenDisabled;
+    }
+
+    @Override
+    public InterruptionBehavior getInterruptionBehavior() {
+        return m_interruptBehavior;
+    }
+
+    @Override
+    public void initSendable(SendableBuilder builder) {
+        super.initSendable(builder);
+
+        builder.addIntegerProperty("index", () -> m_currentCommandIndex, null);
+    }
+}