final-ish changes before sfr

Aragon-Robotics-Team · Mar 7, 2024 · b83870d · b83870d
1 parent c720eca
commit b83870d
Show file tree

Hide file tree

Showing 30 changed files with 175 additions and 105 deletions.
diff --git a/frc-2024/src/main/java/frc/robot/Constants.java b/frc-2024/src/main/java/frc/robot/Constants.java
@@ -35,7 +35,7 @@ public static class DriveConstants {
     public static final double kPTurning = 0.5;
     public static final double kITurning = 0.0;
     public static final double kDTurning = 0.0;
-    public static final double kPDriving = 0.3;
+    public static final double kPDriving = 0.4;
     public static final double kIDriving = 0.0;
     public static final double kDDriving = 0.0;
     public static final double kWheelBase = Units.inchesToMeters(28);

diff --git a/frc-2024/src/main/java/frc/robot/RobotContainer.java b/frc-2024/src/main/java/frc/robot/RobotContainer.java
@@ -31,6 +31,7 @@
 import frc.robot.commands.intakeshooter.ArcadeIntake;
 import frc.robot.commands.intakeshooter.ArcadeShoot;
 import frc.robot.commands.intakeshooter.AutoShoot;
+import frc.robot.commands.intakeshooter.OutakeToSwitch;
 import frc.robot.commands.intakeshooter.Outtake;
 import frc.robot.commands.intakeshooter.ReverseShooter;
 import frc.robot.commands.pivot.ArcadePivot;
@@ -101,7 +102,8 @@ private static final class Config{
 
   private Intake m_intake = new Intake();
   private ArcadeIntake m_runIntake = new ArcadeIntake(m_intake, m_joystick2);
-  private Outtake m_outtake = new Outtake(m_intake, -0.6);
+  // private Outtake m_outtake = new Outtake(m_intake, -0.6);
+  private OutakeToSwitch m_outtake = new OutakeToSwitch(m_intake);
 
   private Shooter m_shooter = new Shooter();
   private ArcadeShoot m_shoot = new ArcadeShoot(m_shooter, m_joystick2);
@@ -174,7 +176,7 @@ private void configureBindings() {
     m_ampButton.whileTrue(m_ampScore);
     m_intakeButton.whileTrue(m_groundIntake);
     m_subwooferButton.whileTrue(m_subwoofer);
-    m_outtakeButton.whileTrue(m_outtake);
+    m_outtakeButton.onTrue(m_outtake);
     // m_podiumButton.whileTrue(m_podium);
     // m_elevatorUpButton.whileTrue(m_elevatorUp);
     // m_elevatorDownButton.whileTrue(m_elevatorDown);

diff --git a/frc-2024/src/main/java/frc/robot/Subsystems/Intake.java b/frc-2024/src/main/java/frc/robot/Subsystems/Intake.java
@@ -15,7 +15,7 @@
 public class Intake extends SubsystemBase {
 
   private CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.kIntakeMotorID, MotorType.kBrushless);
-  // private DigitalInput m_sensor = new DigitalInput(0);
+  private DigitalInput m_sensor = new DigitalInput(5);
 
 
   /** Creates a new Intake. */
@@ -24,6 +24,10 @@ public Intake() {
     // m_intakeMotor.burnFlash(); //does something to preven burnouts?
   }
 
+  public boolean getSwitchTriggered() {
+    return !m_sensor.get();
+  }
+
   public void setSpeed(double speed){
     m_intakeMotor.set(speed);
   }
@@ -36,6 +40,7 @@ public void setSpeed(double speed){
   public void periodic() {
     SmartDashboard.putNumber("Mech/Intake/Motor Temperature", m_intakeMotor.getMotorTemperature());
     SmartDashboard.putNumber("Mech/Intake/Motor Current", m_intakeMotor.getOutputCurrent());
+    SmartDashboard.putBoolean("Mech/Intake/switch", getSwitchTriggered());
     // This method will be called once per scheduler run
   }
 }
diff --git a/frc-2024/src/main/java/frc/robot/commands/elevator/PIDElevatorAmp.java b/frc-2024/src/main/java/frc/robot/commands/elevator/PIDElevatorAmp.java
@@ -13,7 +13,7 @@ public class PIDElevatorAmp extends Command {
 
   private static final class Config{
     public static final double kSetpoint = -2.0;
-    public static final double kDeadband = 0.005;
+    public static final double kDeadband = 0.000;
     public static final double kP = 0.6;
     public static final double kI = 0;
     public static final double kD = 0;

diff --git a/frc-2024/src/main/java/frc/robot/commands/groups/AmpScore.java b/frc-2024/src/main/java/frc/robot/commands/groups/AmpScore.java
@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.commands.elevator.PIDElevatorAmp;
 import frc.robot.commands.pivot.PIDPivotAmp;
+import frc.robot.commands.pivot.PIDPivotSubwoofer;
 import frc.robot.commands.wrist.PIDWristAmp;
 import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Pivot;
@@ -21,11 +22,16 @@ public class AmpScore extends ParallelCommandGroup {
   /** Creates a new AmpScore. */
   public AmpScore(Pivot pivot, Wrist wrist, Elevator elevator) {
     // Add your commands in the addCommands() call, e.g.
-    // addCommands(new FooCommand(), new BarCommand());
+    // // addCommands(new FooCommand(), new BarCommand());
+    // addCommands(
+    //   new PIDWristAmp(wrist),
+    //   new SequentialCommandGroup(new WaitCommand(3), new PIDPivotAmp(pivot)), //0.638
+    //   new SequentialCommandGroup(new WaitCommand(6), new PIDElevatorAmp(elevator))
+    // );
     addCommands(
-      new PIDWristAmp(wrist),
-      new SequentialCommandGroup(new WaitCommand(3), new PIDPivotAmp(pivot)), //0.638
-      new SequentialCommandGroup(new WaitCommand(6), new PIDElevatorAmp(elevator))
+      new PIDPivotAmp(pivot),
+      new SequentialCommandGroup(new WaitCommand(0.25), new PIDWristAmp(wrist)),
+      new SequentialCommandGroup(new WaitCommand(0.5), new PIDElevatorAmp(elevator))
     );
   }
 }
diff --git a/frc-2024/src/main/java/frc/robot/commands/groups/GroundIntake.java b/frc-2024/src/main/java/frc/robot/commands/groups/GroundIntake.java
@@ -24,7 +24,7 @@ public GroundIntake(Pivot pivot, Wrist wrist, Elevator elevator) {
     // addCommands(new FooCommand(), new BarCommand());
     addCommands(
       new PIDWristIntake(wrist),
-      new SequentialCommandGroup(new WaitCommand(0.5), new PIDPivotIntake(pivot)),
+      new SequentialCommandGroup(new WaitCommand(0.25), new PIDPivotIntake(pivot)),
       new SequentialCommandGroup(new WaitCommand(6), new PIDElevatorZero(elevator))
     );
   }

diff --git a/frc-2024/src/main/java/frc/robot/commands/groups/Stowed.java b/frc-2024/src/main/java/frc/robot/commands/groups/Stowed.java
@@ -29,9 +29,9 @@ public Stowed(Pivot pivot, Wrist wrist, Elevator elevator) {
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
     addCommands(
+      new PIDElevatorZero(elevator),
       new PIDPivotStow(pivot),
-      new SequentialCommandGroup(new WaitCommand(3), new PIDElevatorZero(elevator)),
-      new SequentialCommandGroup(new WaitCommand(1), new PIDWristStow(wrist))
+      new SequentialCommandGroup(new WaitCommand(0.3), new PIDWristStow(wrist))
     );
   }
 }
diff --git a/frc-2024/src/main/java/frc/robot/commands/groups/Subwoofer.java b/frc-2024/src/main/java/frc/robot/commands/groups/Subwoofer.java
@@ -7,11 +7,10 @@
 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.elevator.PIDElevatorSubwoofer;
 import frc.robot.commands.elevator.PIDElevatorZero;
-import frc.robot.commands.pivot.PIDPivotIntake;
+import frc.robot.commands.pivot.PIDPivotStow;
 import frc.robot.commands.pivot.PIDPivotSubwoofer;
-import frc.robot.commands.wrist.PIDWristIntake;
+import frc.robot.commands.wrist.PIDWristStow;
 import frc.robot.commands.wrist.PIDWristSubwoofer;
 import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Pivot;
@@ -26,9 +25,9 @@ public Subwoofer(Pivot pivot, Wrist wrist, Elevator elevator) {
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
     addCommands(
-      new PIDWristSubwoofer(wrist),
-      new SequentialCommandGroup(new WaitCommand(3), new PIDPivotSubwoofer(pivot)),
-      new SequentialCommandGroup(new WaitCommand(6), new PIDElevatorSubwoofer(elevator)) 
+      new PIDElevatorZero(elevator),
+      new PIDPivotStow(pivot),
+      new PIDWristSubwoofer(wrist)
     );
   }
 }
diff --git a/frc-2024/src/main/java/frc/robot/commands/intakeshooter/OutakeToSwitch.java b/frc-2024/src/main/java/frc/robot/commands/intakeshooter/OutakeToSwitch.java
@@ -0,0 +1,58 @@
+// 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.intakeshooter;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Intake;
+
+public class OutakeToSwitch extends Command {
+  private Intake m_intake;
+  private int m_state;
+  private double m_initTime;
+  /** Creates a new OutakeToSwitch. */
+  public OutakeToSwitch(Intake intake) {
+    m_intake = intake;
+
+    addRequirements(intake);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    if (m_intake.getSwitchTriggered()) {
+      m_state = 2;
+    } else {
+      m_state = 1;
+    }
+    m_initTime = Timer.getFPGATimestamp();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_intake.setSpeed(-0.6);
+    if (m_state == 1) {
+      if (m_intake.getSwitchTriggered())
+        m_state++;
+    } else {
+      if (!m_intake.getSwitchTriggered())
+        m_state++;
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_intake.setSpeed(0.0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return m_state == 3 || Timer.getFPGATimestamp() - m_initTime >= 0.3;
+  }
+}
diff --git a/frc-2024/src/main/java/frc/robot/commands/pivot/ArcadePivot.java b/frc-2024/src/main/java/frc/robot/commands/pivot/ArcadePivot.java
@@ -31,7 +31,7 @@ public ArcadePivot(Pivot pivot, Joystick joystick) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    System.out.println("I'm running");
+    // System.out.println("I'm running");
     m_pivot.setSpeed(0);
   }
 
@@ -41,10 +41,10 @@ public void execute() {
     m_joystickInput = m_joystick.getRawAxis(Config.kAxis)*Config.kMultiplier;
     // if (!m_pivot.ifForwardTriggered() && m_joystickInput > 0){
     //   m_pivot.setSpeed(0);
-    //   System.out.println("Forward pressed, moving forward, setting speed to 0");
+    //   // System.out.println("Forward pressed, moving forward, setting speed to 0");
     // } else if (!m_pivot.ifBackwardTriggered() && m_joystickInput < 0){
     //   m_pivot.setSpeed(0);
-    //   System.out.println("Backwards pressed, moving backward, setting speed to 0");
+    //   // System.out.println("Backwards pressed, moving backward, setting speed to 0");
     // } else {
       SmartDashboard.putNumber("Pivot speed", m_joystickInput);
       m_pivot.setSpeed(m_joystickInput);

diff --git a/frc-2024/src/main/java/frc/robot/commands/pivot/PIDBack.java b/frc-2024/src/main/java/frc/robot/commands/pivot/PIDBack.java
@@ -40,11 +40,11 @@ public void initialize() {
   @Override
   public void execute() {
     m_speed = m_pid.calculate(m_pivot.getEncoderPosition(), Config.kSetpoint);
-    System.out.println("I'm running");
+    // System.out.println("I'm running");
 
     if (!(Math.abs(m_pivot.getEncoderPosition() - Config.kSetpoint)<= Config.kDeadband)){
-      System.out.println("I'm running in if-else loop");
-      System.out.println(m_speed);
+      // // System.out.println("I'm running in if-else loop");
+      // System.out.println(m_speed);
       SmartDashboard.putNumber("PID value", m_speed);
       m_pivot.setSpeed(m_speed);
     }

diff --git a/frc-2024/src/main/java/frc/robot/commands/pivot/PIDFront.java b/frc-2024/src/main/java/frc/robot/commands/pivot/PIDFront.java
@@ -40,11 +40,11 @@ public void initialize() {
   @Override
   public void execute() {
     m_speed = m_pid.calculate(m_pivot.getEncoderPosition(), Config.kSetpoint);
-    System.out.println("I'm running");
+    // System.out.println("I'm running");
 
     if (!(Math.abs(m_pivot.getEncoderPosition() - Config.kSetpoint)<= Config.kDeadband)){
-      System.out.println("I'm running in if-else loop");
-      System.out.println(m_speed);
+      // // System.out.println("I'm running in if-else loop");
+      // System.out.println(m_speed);
       SmartDashboard.putNumber("PID value", m_speed);
       SmartDashboard.putNumber("PID Error", m_pivot.getEncoderPosition() - Config.kSetpoint);
       m_pivot.setSpeed(m_speed);
@@ -55,7 +55,7 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     m_pivot.setSpeed(0);
-    System.out.println("Ended");
+    // System.out.println("Ended");
   }
 
   // Returns true when the command should end.

diff --git a/frc-2024/src/main/java/frc/robot/commands/pivot/PIDPivotAmp.java b/frc-2024/src/main/java/frc/robot/commands/pivot/PIDPivotAmp.java
@@ -40,11 +40,11 @@ public void initialize() {
   @Override
   public void execute() {
     m_speed = m_pid.calculate(m_pivot.getEncoderPosition(), Config.kSetpoint);
-    System.out.println("I'm running");
+    // System.out.println("I'm running");
 
     if (!(Math.abs(m_pivot.getEncoderPosition() - Config.kSetpoint)<= Config.kDeadband)){
-      System.out.println("I'm running in if-else loop");
-      System.out.println(m_speed);
+      // // System.out.println("I'm running in if-else loop");
+      // System.out.println(m_speed);
       SmartDashboard.putNumber("PID value", m_speed);
       SmartDashboard.putNumber("PID Error", m_pivot.getEncoderPosition() - Config.kSetpoint);
       m_pivot.setSpeed(m_speed);
@@ -55,7 +55,7 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     m_pivot.setSpeed(0);
-    System.out.println("Ended");
+    // System.out.println("Ended");
   }
 
   // Returns true when the command should end.

diff --git a/frc-2024/src/main/java/frc/robot/commands/pivot/PIDPivotIntake.java b/frc-2024/src/main/java/frc/robot/commands/pivot/PIDPivotIntake.java
@@ -40,11 +40,11 @@ public void initialize() {
   @Override
   public void execute() {
     m_speed = m_pid.calculate(m_pivot.getEncoderPosition(), Config.kSetpoint);
-    System.out.println("I'm running");
+    // System.out.println("I'm running");
 
     if (!(Math.abs(m_pivot.getEncoderPosition() - Config.kSetpoint)<= Config.kDeadband)){
-      System.out.println("I'm running in if-else loop");
-      System.out.println(m_speed);
+      // // System.out.println("I'm running in if-else loop");
+      // System.out.println(m_speed);
       SmartDashboard.putNumber("PID value", m_speed);
       SmartDashboard.putNumber("PID Error", m_pivot.getEncoderPosition() - Config.kSetpoint);
       m_pivot.setSpeed(m_speed);
@@ -55,7 +55,7 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     m_pivot.setSpeed(0);
-    System.out.println("Ended");
+    // System.out.println("Ended");
   }
 
   // Returns true when the command should end.

diff --git a/frc-2024/src/main/java/frc/robot/commands/pivot/PIDPivotPodium.java b/frc-2024/src/main/java/frc/robot/commands/pivot/PIDPivotPodium.java
@@ -40,11 +40,11 @@ public void initialize() {
   @Override
   public void execute() {
     m_speed = m_pid.calculate(m_pivot.getEncoderPosition(), Config.kSetpoint);
-    System.out.println("I'm running");
+    // System.out.println("I'm running");
 
     if (!(Math.abs(m_pivot.getEncoderPosition() - Config.kSetpoint)<= Config.kDeadband)){
-      System.out.println("I'm running in if-else loop");
-      System.out.println(m_speed);
+      // // System.out.println("I'm running in if-else loop");
+      // System.out.println(m_speed);
       SmartDashboard.putNumber("PID value", m_speed);
       SmartDashboard.putNumber("PID Error", m_pivot.getEncoderPosition() - Config.kSetpoint);
       m_pivot.setSpeed(m_speed);
@@ -55,7 +55,7 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     m_pivot.setSpeed(0);
-    System.out.println("Ended");
+    // System.out.println("Ended");
   }
 
   // Returns true when the command should end.

diff --git a/frc-2024/src/main/java/frc/robot/commands/pivot/PIDPivotStow.java b/frc-2024/src/main/java/frc/robot/commands/pivot/PIDPivotStow.java
@@ -40,11 +40,11 @@ public void initialize() {
   @Override
   public void execute() {
     m_speed = m_pid.calculate(m_pivot.getEncoderPosition(), Config.kSetpoint);
-    System.out.println("I'm running");
+    // System.out.println("I'm running");
 
     if (!(Math.abs(m_pivot.getEncoderPosition() - Config.kSetpoint)<= Config.kDeadband)){
-      System.out.println("I'm running in if-else loop");
-      System.out.println(m_speed);
+      // // System.out.println("I'm running in if-else loop");
+      // System.out.println(m_speed);
       SmartDashboard.putNumber("PID value", m_speed);
       SmartDashboard.putNumber("PID Error", m_pivot.getEncoderPosition() - Config.kSetpoint);
       m_pivot.setSpeed(m_speed);
@@ -55,7 +55,7 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     m_pivot.setSpeed(0);
-    System.out.println("Ended");
+    // System.out.println("Ended");
   }
 
   // Returns true when the command should end.

diff --git a/frc-2024/src/main/java/frc/robot/commands/pivot/PIDPivotSubwoofer.java b/frc-2024/src/main/java/frc/robot/commands/pivot/PIDPivotSubwoofer.java
@@ -12,7 +12,7 @@
 public class PIDPivotSubwoofer extends Command {
 
   private static final class Config{
-    public static final double kSetpoint = 0;
+    public static final double kSetpoint = -0.102186730994668;
     public static final double kDeadband = 0.000;
     public static final double kP = 2.5;
     public static final double kI = 0.20;
@@ -40,11 +40,11 @@ public void initialize() {
   @Override
   public void execute() {
     m_speed = m_pid.calculate(m_pivot.getEncoderPosition(), Config.kSetpoint);
-    System.out.println("I'm running");
+    // System.out.println("I'm running");
 
     if (!(Math.abs(m_pivot.getEncoderPosition() - Config.kSetpoint)<= Config.kDeadband)){
-      System.out.println("I'm running in if-else loop");
-      System.out.println(m_speed);
+      // // System.out.println("I'm running in if-else loop");
+      // System.out.println(m_speed);
       SmartDashboard.putNumber("PID value", m_speed);
       SmartDashboard.putNumber("PID Error", m_pivot.getEncoderPosition() - Config.kSetpoint);
       m_pivot.setSpeed(m_speed);
@@ -55,7 +55,7 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     m_pivot.setSpeed(0);
-    System.out.println("Ended");
+    // System.out.println("Ended");
   }
 
   // Returns true when the command should end.