pre load in push pt 2

Vortx3735 · Apr 3, 2024 · b59338f · b59338f
1 parent 2269a64
commit b59338f
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 55 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -68,7 +68,7 @@ public static class ArmConstants {
     public static final double armTravelPos = groundArmPos + 0.05; 
 
     public static final double armLength = Units.inchesToMeters(24.727914);
-    public static final double encoderPitchDiameter = 62.23*1000;
+    public static final double encoderPitchDiameter = 62.23/1000;
     public static final double pivotHeight = Units.inchesToMeters(10.907);
     public static final double[][] armAngleArray = {
       // test for different angles from different x distances

diff --git a/src/main/java/frc/robot/commands/AutoAim.java b/src/main/java/frc/robot/commands/AutoAim.java
@@ -45,15 +45,16 @@ public AutoAim(Arm a, Shooter s) {
 
   public Command aimSpeaker() {
     // if(LimelightHelpers.getFiducialID("limelight") == VisionConstants.speakerMidTag) {
-      double kP = 0.0001;
-      double xDistLLToShooter = ArmConstants.armLength*Math.cos(RobotContainer.arm.getRadiansTravelled()) 
+      double kP = 0.01;
+      double xDistLLToShooter = VisionConstants.limelightXDistToArmPivot
+                                + ArmConstants.armLength*Math.cos(RobotContainer.arm.getRadiansTravelled()) 
                                 - ShooterConstants.shooterLength*Math.cos(RobotContainer.arm.getRadiansTravelled() 
-                                                                            + ShooterConstants.differenceFromArm)
-                                + VisionConstants.limelightXDistToArmPivot;
-      double yDistLLToShooter = ArmConstants.armLength*Math.sin(RobotContainer.arm.getRadiansTravelled()) 
+                                                                            - ShooterConstants.differenceFromArm);
+      double yDistLLToShooter = -VisionConstants.limelightYDistToArmPivot
+                                + ArmConstants.armLength*Math.sin(RobotContainer.arm.getRadiansTravelled()) 
                                 - ShooterConstants.shooterLength*Math.sin(RobotContainer.arm.getRadiansTravelled()
-                                                                            + ShooterConstants.differenceFromArm)
-                                - VisionConstants.limelightYDistToArmPivot;
+                                                                            - ShooterConstants.differenceFromArm);
+
       double targetOffsetAngle_Vertical = LimelightHelpers.getTY("limelight");  
 
       double angleToGoalDegrees = VisionConstants.limelightDegrees + targetOffsetAngle_Vertical;
@@ -87,51 +88,7 @@ public Command aimSpeaker() {
     // }
   }
 
-  public Command aimSpeakerAuto() {
-    if(LimelightHelpers.getFiducialID("limelight") == VisionConstants.speakerMidTag) {
-      double kP = 0.1;
-      double xDistLLToShooter = ArmConstants.armLength*Math.cos(RobotContainer.arm.getRadiansTravelled()) 
-                                - ShooterConstants.shooterLength*Math.cos(RobotContainer.arm.getRadiansTravelled() 
-                                                                            + ShooterConstants.differenceFromArm)
-                                + VisionConstants.limelightXDistToArmPivot;
-      double yDistLLToShooter = ArmConstants.armLength*Math.sin(RobotContainer.arm.getRadiansTravelled()) 
-                                - ShooterConstants.shooterLength*Math.sin(RobotContainer.arm.getRadiansTravelled()
-                                                                            + ShooterConstants.differenceFromArm)
-                                - VisionConstants.limelightYDistToArmPivot;
-      double targetOffsetAngle_Vertical = LimelightHelpers.getTY("limelight");  
-
-      double angleToGoalDegrees = VisionConstants.limelightDegrees + targetOffsetAngle_Vertical;
-      double angleToGoalRadians = Units.degreesToRadians(angleToGoalDegrees);
-
-      //calculate distance
-      double distanceFromLimelightToGoalMeters = (FieldConstants.speakerHeight - VisionConstants.limelightHeight) / Math.tan(angleToGoalRadians);
-      double xShooterToGoal = distanceFromLimelightToGoalMeters + xDistLLToShooter;
-
-      double yShooterToGoal = FieldConstants.speakerHeight - ArmConstants.pivotHeight - yDistLLToShooter;
-      double error = stupid(xShooterToGoal, yShooterToGoal)[0] - RobotContainer.arm.getRadiansTravelled();
-      double rpmNeeded = stupid(xShooterToGoal, yShooterToGoal)[1];
-
-      return new RunCommand(
-        () -> RobotContainer.arm.move(error*kP),
-        RobotContainer.arm
-      ).alongWith(
-        new RunCommand(
-          () -> RobotContainer.shooter.setShooterRPM(rpmNeeded),
-          RobotContainer.shooter)
-      );
-    } else {
-      System.out.println(((DriverStation.getAlliance().get() == DriverStation.Alliance.Red) ? "Red" : "Blue") + " Alliance Mid Speaker AprilTag Not Found!");
-      return new RunCommand(
-        () -> RobotContainer.arm.getDefaultCommand(),
-        RobotContainer.arm
-      ).alongWith(
-        new RunCommand(
-          () -> RobotContainer.shooter.getDefaultCommand(),
-          RobotContainer.shooter)
-      );
-    }
-  }
-
+  // 
   public static double[] stupid(double xDist, double yDist) {
         // this code is very effective until around 30 feet which is further than podium so it doesnt matter lol
         //      the reason its uneffective at until 30 is bc the note needs to follow the trajectory and enter the speaker
@@ -148,10 +105,10 @@ public static double[] stupid(double xDist, double yDist) {
         //y = -5.401353307, x = -5.60307862645667 is up against subwoofer
         double a_y = -9.8; // gravity
         double v_y = 0;
-        double p_y = -xDist; // vertical distance from target, this will change bc the arm will raise the further it gets away
+        double p_y = -yDist; // vertical distance from target, this will change bc the arm will raise the further it gets away
         double a_x = -0; // pretend this doesn't exist because I don't feel like doing air resistance
         double v_x = 0;
-        double p_x = -yDist; // horizontal distance from target, this will change bc the robot moves
+        double p_x = -xDist; // horizontal distance from target, this will change bc the robot moves
 
         double shootingvelo_y = Math.sqrt(p_y * a_y * 2);
         double shootingvelo_x = p_x/(shootingvelo_y/a_y);