Kingsley/shootfromfar

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

@@ -8,6 +8,7 @@
 import com.ctre.phoenix6.SignalLogger;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
+
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
@@ -57,11 +58,13 @@ private void configureBindings() {
                     .withVelocityY(
                         Utils.deadzone(-m_driver.getLeftX() * Constants.DrivebaseMaxSpeed))
                     .withRotationalRate(
-                        Utils.deadzone(
-                            -m_driver.getRightX() * Constants.DrivebaseMaxAngularRate))));
+                        -m_driver.getRightX() * Constants.DrivebaseMaxAngularRate)));
+
+    m_driver.a().whileTrue(drivetrain.applyRequest(() -> brake));
+    m_driver.x().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldRelative()));
 
-    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.shootFromFar()));
+    m_driver.leftTrigger().onTrue(m_shooter.runOnce(() -> m_shooter.shootFromFar()));
   }
 
   public Robot() {
@@ -95,7 +98,7 @@ public Robot() {
     m_chooser.addOption("WompWompKieran Red", new WompWompKieran(m_drivetrain, true));
     m_chooser.setDefaultOption("WompWompKieran Blue", new WompWompKieran(m_drivetrain, false));
     SmartDashboard.putData(m_chooser);
-
+    
     configureBindings();
   }
 

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

@@ -4,8 +4,11 @@
 
 package frc.robot.subsystems;
 
+import java.util.function.DoubleSupplier;
+
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
+
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.util.datalog.DataLog;
@@ -16,17 +19,18 @@
 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.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants;
-import java.util.function.DoubleSupplier;
 
 /** Our Arm Subsystem */
 public class Arm extends SubsystemBase {
   public enum Setpoint {
     kAmp,
     kIntake,
     kSpeaker,
-    kStowed
+    kStowed,
+    kFarShoot
   }
 
   private final PIDController m_pid;
@@ -117,7 +121,7 @@ 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}
@@ -144,8 +148,31 @@ public Command goToSetpoint(Setpoint setpoint) {
       case kStowed:
         position = 3.7;
         break;
+
+      case kFarShoot:
+        position = calculateArmAngle();
+        break;
     }
 
     return moveToPosition(position);
   }
-}
+
+  public double calculateArmAngle() {
+    double arm_length = 0.65;
+    double H = 2.46 - (Math.sin(m_encoder.getAbsolutePosition())*arm_length); // Height from top of the arm to the speaker shooting area
+    double arm_vertical_offset_from_floor = 0.3; // offset of floor to bottom of arm
+    double horizontal_robot_offset_from_speaker_wall = 0.5; // offset of the robot to the speaker wall when the robot is flush against the speaker
+    double K3 = arm_length*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 shootFromFar() {
+    return Commands.run(() -> moveToPosition(calculateArmAngle()));
+    /** you have to press a button on the controller. then you need to no move the arm angle using the thing and then shoot note */
+  }
+
+}

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

@@ -7,6 +7,7 @@
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
+
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.util.datalog.DataLog;
@@ -74,7 +75,25 @@ public Command stop() {
    *
    * @return A {@link Command} to hold the speed at the setpoint.
    */
+
   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(500));
+  }
 }