Merge branch 'master' into peter/LEDintegration

CurtinFRC · Aug 12, 2024 · 7314b82 · 7314b82
2 parents ae107b1 + 11af318
commit 7314b82
Show file tree

Hide file tree

Showing 6 changed files with 67 additions and 31 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -30,7 +30,7 @@ public final class Constants {
   public static final double intakeI = 0;
   public static final double intakeD = 0;
 
-  public static final double armP = 10.5;
+  public static final double armP = 10;
   public static final double armI = 0;
   public static final double armD = 0;
   public static final double armS = 0;

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -11,11 +11,13 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandRobot;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.autos.WompWompKieran;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.Arm;
+import frc.robot.subsystems.Arm.Setpoint;
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 import frc.robot.subsystems.Index;
@@ -81,24 +83,38 @@ public Robot() {
             () ->
                 m_drive
                     .withVelocityX(
-                        Utils.deadzone(-m_driver.getLeftY() * Constants.DrivebaseMaxSpeed))
+                        Utils.deadzone(-m_driver.getLeftY() * Constants.DrivebaseMaxSpeed * 0.5))
                     .withVelocityY(
-                        Utils.deadzone(-m_driver.getLeftX() * Constants.DrivebaseMaxSpeed))
+                        Utils.deadzone(-m_driver.getLeftX() * Constants.DrivebaseMaxSpeed * 0.5))
                     .withRotationalRate(
                         Utils.deadzone(
-                            -m_driver.getRightX() * Constants.DrivebaseMaxAngularRate))));
+                            -m_driver.getRightX() * Constants.DrivebaseMaxAngularRate * 0.5))));
     m_intake.setDefaultCommand(m_superstructure.intake());
     m_shooter.setDefaultCommand(m_shooter.stop());
     m_index.setDefaultCommand(m_index.stop());
-    // m_arm.setDefaultCommand(m_arm.goToSetpoint(Arm.Setpoint.kStowed));
-    m_arm.setDefaultCommand(m_arm.manualControl(m_codriver::getLeftY));
+    m_arm.setDefaultCommand(m_arm.goToSetpoint(Setpoint.kIntake));
+
+    new Trigger(() -> m_codriver.getLeftY() > 0.05)
+        .whileTrue(m_arm.manualControl(m_codriver::getLeftY));
 
     m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> m_brake));
     m_driver.y().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative()));
 
     m_codriver.a().onTrue(m_climber.climb());
-    m_codriver.rightBumper().whileTrue(m_intake.outake(8));
-    m_codriver.leftBumper().whileTrue(m_superstructure.shoot());
+    m_codriver.leftBumper().whileTrue(m_index.shoot());
+    m_codriver.rightBumper().whileTrue(m_shooter.spinup(500).andThen(m_shooter.maintain()));
+    m_codriver
+        .leftTrigger()
+        .whileTrue(
+            Commands.parallel(
+                m_arm.goToSetpoint(Setpoint.kSpeaker).andThen(m_arm.maintain()),
+                m_shooter.spinup(500).andThen(m_shooter.maintain())));
+    m_codriver
+        .rightTrigger()
+        .whileTrue(
+            Commands.parallel(
+                m_arm.goToSetpoint(Setpoint.kAmp).andThen(m_arm.maintain()),
+                m_shooter.applyVolts(8)));
 
     m_codriverX.whileTrue(m_superstructure.intake()).onFalse(m_intake.stop());
     m_scheduler

diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
@@ -30,10 +30,12 @@ public Command intake() {
   }
 
   public Command shoot() {
-    return m_shooter
-        .spinup(500)
-        .withTimeout(3)
-        .andThen(Commands.parallel(m_shooter.maintain(), m_index.shoot()));
+    return m_shooter.spinup(500).andThen(Commands.parallel(m_shooter.maintain(), m_index.shoot()));
+  }
+
+  public Command outake_shooter() {
+    // return Commands.parallel(m_shooter.applyVolts(6), m_index.shoot());
+    return m_shooter.applyVolts(6);
   }
 
   public Command stop() {

diff --git a/src/main/java/frc/robot/URCL.java b/src/main/java/frc/robot/URCL.java
@@ -50,7 +50,7 @@ public class URCL {
   private static Notifier notifier;
   private static final DataLog datalog = DataLogManager.getLog();
   private static RawLogEntry persistentLogEntry =
-      new RawLogEntry(datalog, "URCL/Raw/Persistent", "", "URCLr2_persistent");
+      new RawLogEntry(datalog, "/URCL/Raw/Persistent", "", "URCLr2_persistent");
   private static RawLogEntry periodicLogEntry =
       new RawLogEntry(datalog, "/URCL/Raw/Periodic", "", "URCLr2_periodic");
   private static RawLogEntry aliasLogEntry =

diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
@@ -60,31 +60,33 @@ public enum Setpoint {
   private final DoublePublisher pid_setpoint = driveStats.getDoubleTopic("PID/Setpoint").publish();
   private final DoublePublisher angle = driveStats.getDoubleTopic("Angle").publish();
   public final Trigger m_atSetpoint = new Trigger(m_pid::atSetpoint);
+  private double m_lastPosition = 0.2;
 
   /** Creates a new {@link Arm} {@link edu.wpi.first.wpilibj2.command.Subsystem}. */
   public Arm() {
     m_followerMotor.follow(m_primaryMotor);
-    m_encoder.reset();
-    m_encoder.setPositionOffset(0.1);
+    m_pid.setTolerance(0.1);
   }
 
   /** Achieves and maintains speed for the primary motor. */
   private Command achievePosition(double position) {
     return run(
         () -> {
-          var pid_output = m_pid.calculate(m_encoder.getAbsolutePosition() * 2 * Math.PI, position);
+          m_lastPosition = position;
+          m_pid.setSetpoint(position);
+          var pid_output = m_pid.calculate(getAngle());
           log_pid_output.append(pid_output);
           log_pid_setpoint.append(m_pid.getSetpoint());
           var ff_output = m_feedforward.calculate(position, (5676 / 250));
           log_ff_output.append(ff_output);
           log_ff_position_setpoint.append(position);
           log_ff_velocity_setpoint.append((5676 / 250));
           this.ff_output.set(ff_output);
-          this.pid_output.set(0.5 + pid_output);
+          this.pid_output.set(pid_output);
           this.pid_error.set(m_pid.getPositionError());
           this.pid_setpoint.set(m_pid.getSetpoint());
-          m_primaryMotor.setVoltage(0.5 + pid_output * -1);
-          // m_primaryMotor.setVoltage(0.5 + pid_output);
+          // m_primaryMotor.setVoltage(pid_output * -1);
+          m_primaryMotor.setVoltage(pid_output);
         });
   }
 
@@ -98,17 +100,27 @@ public Command stop() {
    * @param position The desired position.
    * @return a {@link Command} to get to the desired position.
    */
-  private Command moveToPosition(double position) {
+  public Command moveToPosition(double position) {
     return achievePosition(position).until(m_pid::atSetpoint);
   }
 
+  /**
+   * Moves the arm to the specified position then stops.
+   *
+   * @param position The desired position.
+   * @return a {@link Command} to get to the desired position.
+   */
+  public Command hold(double angle) {
+    return achievePosition(angle);
+  }
+
   /**
    * Holds the arm at the current position setpoint.
    *
    * @return A {@link Command} to hold the position at the setpoint.
    */
   public Command maintain() {
-    return achievePosition(m_pid.getSetpoint());
+    return defer(() -> achievePosition(m_lastPosition));
   }
 
   /**
@@ -137,17 +149,21 @@ public Command goToSetpoint(Setpoint setpoint) {
     log_setpoint.append(setpoint.name());
 
     switch (setpoint) {
-      case kAmp -> position = Math.PI / 2 + 4.8;
-      case kIntake -> position = 4.85;
-      case kSpeaker -> position = 4.85;
-      case kStowed -> position = 4.85;
+      case kAmp -> position = 1.68;
+      case kIntake -> position = 0.2;
+      case kSpeaker -> position = 0.2;
+      case kStowed -> position = 0.2;
     }
 
     return moveToPosition(position);
   }
 
   @Override
   public void periodic() {
-    angle.set(m_encoder.getAbsolutePosition() * 2 * Math.PI);
+    angle.set(getAngle());
+  }
+
+  public double getAngle() {
+    return m_encoder.getAbsolutePosition() * 2 * Math.PI;
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java
@@ -39,9 +39,7 @@ public class Shooter extends SubsystemBase {
   public final Trigger m_atSetpoint = new Trigger(m_pid::atSetpoint);
 
   /** Creates a new {@link Shooter} {@link edu.wpi.first.wpilibj2.command.Subsystem}. */
-  public Shooter() {
-    m_pid.setTolerance(0.05, 0.05);
-  }
+  public Shooter() {}
 
   /** Acheives and maintains speed. */
   private Command achieveSpeeds(double speed) {
@@ -79,15 +77,19 @@ public Command stop() {
    * @return A {@link Command} to hold the speed at the setpoint.
    */
   public Command maintain() {
-    return runOnce(() -> LED.Maintain()).andThen(achieveSpeeds(m_pid.getSetpoint()));
+    return defer(() -> runOnce(() -> LED.Maintain()).andThen(achieveSpeeds(m_pid.getSetpoint())));
   }
 
   public Command shoot() {
     return spinup(500).andThen(maintain());
   }
 
+  public Command applyVolts(double volts) {
+    return run(() -> m_motor.setVoltage(volts));
+  }
+
   @Override
   public void periodic() {
-    m_ntRotationalVelocity.set(m_encoder.getVelocity());
+    m_ntRotationalVelocity.set(Units.rotationsPerMinuteToRadiansPerSecond(m_encoder.getVelocity()));
   }
 }