Merge branch 'main' of https://github.com/KHS-Robotics/Demonator13

src/main/deploy/pathplanner/autos/Amp Side Shoot 2.auto

@@ -32,6 +32,12 @@
                 "data": {
                   "name": "RampShooterForManualShot"
                 }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "SwerveStraighten"
+                }
               }
             ]
           }

src/main/deploy/pathplanner/autos/Center Shoot 2.auto

@@ -32,6 +32,12 @@
                 "data": {
                   "name": "RampShooterForManualShot"
                 }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "SwerveStraighten"
+                }
               }
             ]
           }

src/main/deploy/pathplanner/autos/Center Shoot 3.auto

@@ -32,6 +32,12 @@
                 "data": {
                   "name": "RampShooterForManualShot"
                 }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "SwerveStraighten"
+                }
               }
             ]
           }

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

@@ -15,6 +15,7 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.SerialPort.Port;
@@ -24,7 +25,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.RepeatCommand;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -292,6 +293,23 @@ private void configureOperatorStickBindings() {
       .andThen(new SetIntakeState(IntakeState.kUp))
     );
 
+    // UNTESTED NEW ARM CODE!!!
+
+    // Trigger stowArm = new Trigger(operatorStick::stowArm);
+    // stowArm.onTrue(StateCommandGenerator.goToStowCommand());
+
+    // Trigger ampArm = new Trigger(operatorStick::ampArm);
+    // ampArmp.onTrue(StateCommandGenerator.goToAmpCommand());
+
+    // Trigger flatArm = new Trigger(operatorStick::podiumArm);
+    // flatArm.onTrue(StateCommandGenerator.goToFlatCommand());
+
+    // Trigger handoffArm = new Trigger(operatorStick::handoffArm);
+    // handoffArm.onTrue(StateCommandGenerator.goToHandoffCommand());
+
+    // Trigger subwooferArm = new Trigger(operatorStick::subwooferArm);
+    // subwooferArm.onTrue(StateCommandGenerator.goToSubwooferCommand());
+
     var scoreAmp = new Trigger(operatorStick::scoreAmp);
     scoreAmp.onTrue(
       new InstantCommand(() -> RobotContainer.shooter.driveShooter(-14), RobotContainer.shooter)
@@ -384,6 +402,22 @@ private void registerNamedCommands() {
 
     // Swerves
     NamedCommands.registerCommand("StopSwerves", new InstantCommand(() -> swerveDrive.stop(), swerveDrive));
+    // SwerveStraighten - used when lowering intake and deploying demon horns to make initial odometry updates more accurate
+    NamedCommands.registerCommand("SwerveStraighten", 
+      new RepeatCommand(
+        new InstantCommand(() -> 
+          swerveDrive.setModuleStates(new SwerveModuleState[] {
+            new SwerveModuleState(0, Rotation2d.fromDegrees(0)),
+            new SwerveModuleState(0, Rotation2d.fromDegrees(0)),
+            new SwerveModuleState(0, Rotation2d.fromDegrees(0)),
+            new SwerveModuleState(0, Rotation2d.fromDegrees(0))
+          }),
+          swerveDrive
+        )
+      )
+      .withTimeout(1)
+      .andThen(new InstantCommand(() -> swerveDrive.stop(), swerveDrive))
+    );
   }
 
   private void configurePathPlannerLogging() {

src/main/java/frc/robot/StateCommandGenerator.java

@@ -0,0 +1,88 @@
+package frc.robot;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.commands.arm.SetArmState;
+import frc.robot.commands.arm.WaitForArmStow;
+import frc.robot.commands.intake.SetIntakeState;
+import frc.robot.commands.shooter.SetShooterState;
+import frc.robot.subsystems.Arm.ArmState;
+import frc.robot.subsystems.Intake.IntakeState;
+import frc.robot.subsystems.Shooter.ShooterState;
+
+public class StateCommandGenerator {
+    public static Command goToStowCommand() {
+        if (RobotContainer.arm.getPosition() > ArmState.kShoot.rotations + 0.02) {
+            // arm down
+            // move arm then move intake
+            return new SequentialCommandGroup(new SetArmState(ArmState.kStow), new SetIntakeState(IntakeState.kUp));
+        } else {
+            // arm up
+            // move arm and intake together
+            return new ParallelCommandGroup(new SetArmState(ArmState.kStow), new SetIntakeState(IntakeState.kUp));
+        }
+    }
+
+    public static Command goToAmpCommand() {
+        if (RobotContainer.arm.getPosition() < ArmState.kShoot.rotations + 0.02) {
+            // arm up
+            // move arm and intake together
+            return new ParallelCommandGroup(new SetIntakeState(IntakeState.kUp), new SetArmState(ArmState.kAmp));
+        } else {
+            // arm down
+            // move arm then move intake
+            return new ParallelCommandGroup(new SetArmState(ArmState.kAmp), new SequentialCommandGroup(new WaitForArmStow(), new SetIntakeState(IntakeState.kUp)));
+        }
+    }
+
+    public static Command goToFlatCommand() {
+        if (RobotContainer.intake.isIntakeUp()) {
+            // intake up
+            // move arm flat
+            return new ParallelCommandGroup(new SetArmState(ArmState.kShoot), new SetShooterState(ShooterState.kShootFromPodium));
+        } else if (RobotContainer.arm.getPosition() < ArmState.kStow.rotations + 0.02) {
+            // intake down and arm down
+            // move arm to stow move intake up move arm to flat
+            return new ParallelCommandGroup(new SetArmState(ArmState.kStow), new SequentialCommandGroup(new WaitForArmStow(), new SetIntakeState(IntakeState.kUp), new SetArmState(ArmState.kShoot)), new SetShooterState(ShooterState.kShootFromPodium));
+        } else {
+            // intake down and arm up
+            // 
+            return new ParallelCommandGroup(new SequentialCommandGroup(new ParallelCommandGroup(new SetArmState(ArmState.kStow), new SetIntakeState(IntakeState.kUp)), new SetArmState(ArmState.kShoot)), new SetShooterState(ShooterState.kShootFromPodium));
+        }
+
+    }
+
+    public static Command goToHandoffCommand() {
+        if (RobotContainer.arm.getPosition() < ArmState.kStow.rotations + 0.2) {
+            // intake up
+            // move intake then move 
+            return new SequentialCommandGroup(new SetIntakeState(IntakeState.kDown), new ParallelCommandGroup(new SetArmState(ArmState.kIntake), new SetShooterState(ShooterState.kIntake)));
+        } else if (RobotContainer.arm.isAtState(ArmState.kShootFromPodium)) {
+            // arm flat
+            // move arm up and intake down together, then arm down and shooter down together
+            return new SequentialCommandGroup(new ParallelCommandGroup(new SetArmState(ArmState.kStow), new SequentialCommandGroup(new WaitCommand(0.1), new SetIntakeState(IntakeState.kDown))), new ParallelCommandGroup(new SetArmState(ArmState.kIntake), new SetShooterState(ShooterState.kIntake)));
+        } else {
+            // arm down
+            // move arm and shooter together
+            return new ParallelCommandGroup(new SetArmState(ArmState.kIntake), new SetShooterState(ShooterState.kIntake));
+        }
+    }
+
+    public static Command goToSubwooferCommand() {
+        if (RobotContainer.arm.getPosition() < ArmState.kStow.rotations + 0.2) {
+            // intake up
+            // move intake then move 
+            return new SequentialCommandGroup(new SetIntakeState(IntakeState.kDown), new ParallelCommandGroup(new SetArmState(ArmState.kShootFromSubwoofer), new SetShooterState(ShooterState.kShootFromSubwoofer)));
+        } else if (RobotContainer.arm.isAtState(ArmState.kShootFromPodium)) {
+            // arm flat
+            // move arm up and intake down together, then arm down and shooter down together
+            return new SequentialCommandGroup(new ParallelCommandGroup(new SetArmState(ArmState.kStow), new SequentialCommandGroup(new WaitCommand(0.1), new SetIntakeState(IntakeState.kDown))), new ParallelCommandGroup(new SetArmState(ArmState.kShootFromSubwoofer), new SetShooterState(ShooterState.kShootFromSubwoofer)));
+        } else {
+            // arm down
+            // move arm and shooter together
+            return new ParallelCommandGroup(new SetArmState(ArmState.kShootFromSubwoofer), new SetShooterState(ShooterState.kShootFromSubwoofer));
+        }
+    }
+}

src/main/java/frc/robot/commands/arm/WaitForArmStow.java

@@ -0,0 +1,34 @@
+// 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.arm;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.Arm.ArmState;
+
+public class WaitForArmStow extends Command {
+  /** Creates a new WaitForArmStow. */
+  public WaitForArmStow() {
+    // 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() {}
+
+  // 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 Math.abs(RobotContainer.arm.getPosition() - ArmState.kStow.rotations) < 0.05;
+  }
+}

src/main/java/frc/robot/commands/intake/WaitForIntakeUp.java

@@ -0,0 +1,33 @@
+// 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.intake;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.RobotContainer;
+
+public class WaitForIntakeUp extends Command {
+  /** Creates a new WaitForIntakeUp. */
+  public WaitForIntakeUp() {
+    // 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() {}
+
+  // 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 RobotContainer.intake.isIntakeUp();
+  }
+}

src/main/java/frc/robot/subsystems/Arm.java

@@ -98,7 +98,7 @@ public enum ArmState {
   public void periodic() {
     // SmartDashboard.putNumber("armAngle", getPosition());
     // SmartDashboard.putNumber("armSetpoint", rotationSetpoint);
-    SmartDashboard.putNumber("armError", Math.abs(rotationSetpoint - getPosition()));
+    // SmartDashboard.putNumber("armError", Math.abs(rotationSetpoint - getPosition()));
 
     // kG = SmartDashboard.getNumber("kG", kG);
     // kP = SmartDashboard.getNumber("kP", kP);

src/main/java/frc/robot/subsystems/Intake.java

@@ -62,7 +62,7 @@ public void goToSetpoint(double rotations) {
     double ffOutput = kG * Math.cos(Math.PI * 2 * getPosition());
     double output = pidOutput + ffOutput;
 
-    if (getPosition() <= 0.04 && rotations <= 0.01) {
+    if (getPosition() <= 0.04 && rotations <= IntakeState.kDown.rotations) {
       output = 0;
     }
 
@@ -104,7 +104,11 @@ public double getPosition() {
   }
 
   public boolean isIntakeDown() {
-    return rotationSetpoint < 0.04;
+    return getPosition() < 0.07;
+  }
+
+  public boolean isIntakeUp() {
+    return getPosition() > 0.4;
   }
 
   public boolean hasNoteInside() {
@@ -127,7 +131,7 @@ public enum IntakeState {
   public void periodic() {
     // SmartDashboard.putNumber("IntakeAngle", getPosition());
     // SmartDashboard.putNumber("IntakeSetpoint", rotationSetpoint);
-    SmartDashboard.putNumber("IntakeError", Math.abs(getPosition() - rotationSetpoint));
+    // SmartDashboard.putNumber("IntakeError", Math.abs(getPosition() - rotationSetpoint));
     // SmartDashboard.putBoolean("IntakeHasNote", hasNoteInside());
 
     // kP = SmartDashboard.getNumber("kp", kP);

src/main/java/frc/robot/subsystems/Shooter.java

@@ -398,7 +398,7 @@ public enum ShooterState {
   public void periodic() {
     // SmartDashboard.putNumber("shooterAngle", getPosition());
     // SmartDashboard.putNumber("shooterAngleSetpoint", rotationSetpoint);
-    SmartDashboard.putNumber("shooterAngleError", Math.abs(rotationSetpoint - getPosition()));
+    // SmartDashboard.putNumber("shooterAngleError", Math.abs(rotationSetpoint - getPosition()));
     // SmartDashboard.putNumber("shooterAngleAbsolute", getAbsoluteAngle());
     // SmartDashboard.putNumber("Shooter-Velocity", getVelocity());
     // SmartDashboard.putBoolean("Shooter-HasNote", hasNote());