Kingsley/shootfromfar

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

@@ -15,8 +15,8 @@ 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 int climberPort = 10;
+  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;
   public static final double climberD = 0;
@@ -35,7 +35,10 @@ 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 int armLeadPort = 9;
+  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

@@ -111,5 +111,8 @@ public Robot() {
             });
     m_codriver.y().onTrue(m_superstructure.stop());
     m_codriver.b().whileTrue(m_superstructure.outake());
+    m_driver
+        .leftTrigger()
+        .onTrue(m_arm.goToSetpoint(m_drivetrain.getState().Pose).andThen(m_shooter.shootFromFar()));
   }
 }

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.networktables.DoublePublisher;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
@@ -28,7 +29,8 @@ public enum Setpoint {
     kAmp,
     kIntake,
     kSpeaker,
-    kStowed
+    kStowed,
+    kFarShoot
   }
 
   private final CANSparkMax m_primaryMotor =
@@ -125,11 +127,10 @@ public Command manualControl(DoubleSupplier speed) {
         });
   }
 
-  /*
+  /**
    * 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) {
@@ -141,6 +142,7 @@ public Command goToSetpoint(Setpoint setpoint) {
       case kIntake -> position = 4.85;
       case kSpeaker -> position = 4.85;
       case kStowed -> position = 4.85;
+      case kFarShoot -> DataLogManager.log("WARNING: Invalid state kFarShoot with no pose.");
     }
 
     return moveToPosition(position);
@@ -150,4 +152,30 @@ public Command goToSetpoint(Setpoint setpoint) {
   public void periodic() {
     angle.set(m_encoder.getAbsolutePosition() * 2 * Math.PI);
   }
+
+  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 Command goToSetpoint(Pose2d currentPose) {
+    double position = calculateArmAngle(currentPose);
+    return moveToPosition(position);
+  }
 }

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

@@ -203,4 +203,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

@@ -16,6 +16,7 @@
 import edu.wpi.first.util.datalog.DoubleLogEntry;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants;
@@ -89,4 +90,8 @@ public Command shoot() {
   public void periodic() {
     m_ntRotationalVelocity.set(m_encoder.getVelocity());
   }
+
+  public Command shootFromFar() {
+    return Commands.run(() -> spinup(Constants.shootFromFarSpeed));
+  }
 }