Kingsley/shootfromfar

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

@@ -13,6 +13,7 @@ public final class Constants {
   public static final double shooterP = 0.5;
   public static final double shooterI = 0;
   public static final double shooterD = 0;
+  public static final double shootFromFarSpeed = 500;
   public static final int climberPort = 32;
   public static final double climberP = 0.35;
   public static final double climberI = 0;
@@ -25,6 +26,9 @@ public final class Constants {
   public static final double armG = 0;
   public static final double armV = 0;
   public static final double armA = 0;
+  public static final double armLength = 0.65;
+  public static final double armPositionOffset = 0;
+  public static final double robotHeight = 0; // height of robot from ground to bottom of arm
   public static final int armLeadPort = 21;
   public static final int armFollowerPort = 26;
   public static final int armEncoderPort = 3;

src/main/java/frc/robot/Robot.java

@@ -62,6 +62,13 @@ private void configureBindings() {
 
     m_driver.a().whileTrue(m_drivetrain.applyRequest(() -> brake));
     m_driver.x().onTrue(m_drivetrain.runOnce(() -> m_drivetrain.seedFieldRelative()));
+    m_driver
+        .leftTrigger()
+        .onTrue(
+            m_arm
+                .runOnce(() -> m_arm.goToSetpoint(m_drivetrain.getEstimatedPosition()))
+                .andThen(m_shooter.runOnce(() -> m_shooter.shootFromFar())));
+    // m_driver.leftTrigger().onTrue(m_shooter.runOnce(() -> m_shooter.shootFromFar()));
   }
 
   public Robot() {

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

@@ -8,6 +8,7 @@
 import com.revrobotics.CANSparkMax;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.util.datalog.DataLog;
 import edu.wpi.first.util.datalog.DoubleLogEntry;
 import edu.wpi.first.util.datalog.StringLogEntry;
@@ -26,7 +27,8 @@ public enum Setpoint {
     kAmp,
     kIntake,
     kSpeaker,
-    kStowed
+    kStowed,
+    kFarShoot
   }
 
   private final PIDController m_pid;
@@ -117,35 +119,49 @@ public Command manualControl(DoubleSupplier speed) {
     return Commands.run(() -> m_primaryMotor.set(speed.getAsDouble()), this);
   }
 
-  /*
+  /**
    * Makes the arm go to a setpoint from the {@link Setpoint} enum
    *
    * @param setpoint The setpoint to go to, a {@link Setpoint}
-   *
    * @return A {@link Command} to go to the setpoint
    */
   public Command goToSetpoint(Setpoint setpoint) {
     double position = 0;
     log_setpoint.append(setpoint.name());
 
     switch (setpoint) {
-      case kAmp:
-        position = 5.34;
-        break;
-
-      case kIntake:
-        position = 3.7;
-        break;
-
-      case kSpeaker:
-        position = 3.7;
-        break;
-
-      case kStowed:
-        position = 3.7;
-        break;
+      case kAmp -> position = 5.34;
+      case kIntake -> position = 3.7;
+      case kSpeaker -> position = 3.7;
+      case kStowed -> position = 3.7;
+      case kFarShoot -> DataLogManager.log("WARNING: Invalid state kFarShoot with no pose.");
     }
 
     return moveToPosition(position);
   }
+
+  public double calculateArmAngle(Pose2d currentPose) {
+    double H =
+        2.46
+            - (Math.sin(m_encoder.getAbsolutePosition() * 360 + Constants.armPositionOffset)
+                * Constants
+                    .armLength); // converts it from 0-1 to 0-365 and adds an offset depending where
+    // 0 degrees is.
+    double arm_vertical_offset_from_floor =
+        (Constants.armLength * Math.sin(m_encoder.getAbsolutePosition())) + Constants.robotHeight;
+    double horizontal_robot_offset_from_speaker_wall =
+        currentPose.getX(); // to be changed to wherever the robot is on field (get its value from
+    // drivetrain/drivebase)// offset of the robot to the speaker wall when the
+    // robot is flush against the speaker
+    double K3 = Constants.armLength * Math.sin(129);
+    double K2 = horizontal_robot_offset_from_speaker_wall;
+    double K1 = H - arm_vertical_offset_from_floor;
+    double R = Math.sqrt(K2 * K2 - K1 * K1);
+    double a = Math.atan(K3 / K1);
+    return 51 - a + Math.acos(K3 / R);
+  }
+
+  public void goToSetpoint(Pose2d currentPose) {
+    double position = calculateArmAngle(currentPose);
+  }
 }

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

@@ -14,6 +14,7 @@
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -203,4 +204,8 @@ public Command followTrajectory(String name, boolean isRed) {
         () -> isRed,
         this);
   }
+
+  public Pose2d getEstimatedPosition() {
+    return m_odometry.getEstimatedPosition();
+  }
 }

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

@@ -77,4 +77,21 @@ public Command stop() {
   public Command maintain() {
     return achieveSpeeds(m_pid.getSetpoint());
   }
+
+  /**
+   * Checks if the Shooter is at its setpoint and the loop is stable.
+   *
+   * @return A {@link Trigger} from the result.
+   */
+  public Trigger atSetpoint() {
+    return new Trigger(
+        () ->
+            m_pid.getSetpoint()
+                    == Units.rotationsPerMinuteToRadiansPerSecond(m_encoder.getVelocity())
+                && m_pid.atSetpoint());
+  }
+
+  public Command shootFromFar() {
+    return Commands.run(() -> spinup(Constants.shootFromFarSpeed));
+  }
 }