Merge branch 'testRoboReveal' of https://github.com/Team4388/2022NoWayHome into testRoboReveal

src/main/java/frc4388/robot/Robot.java

@@ -25,6 +25,7 @@ import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import frc4388.utility.RobotTime;
+import frc4388.utility.VelocityCorrection;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -150,6 +151,12 @@ public class Robot extends TimedRobot {
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
     
+    VelocityCorrection vc = new VelocityCorrection(m_robotContainer.m_robotSwerveDrive, m_robotContainer.m_robotBoomBoom);
+    System.out.println("Position: " + vc.position);
+    System.out.println("Velocity: " + vc.cartesianVelocity);
+    System.out.println("Target: " + vc.target.toString());
+
+
     //SmartDashboard.putNumber("Turret Encoder Position", m_robotContainer.m_robotTurret.m_boomBoomRotateEncoder.getPosition());
     //SmartDashboard.putNumber("Hood Encoder Position", m_robotContainer.m_robotHood.m_angleEncoder.getPosition());
 

src/main/java/frc4388/robot/RobotContainer.java

@@ -79,6 +79,7 @@ import frc4388.robot.subsystems.VisionOdometry;
 import frc4388.utility.LEDPatterns;
 import frc4388.utility.ListeningSendableChooser;
 import frc4388.utility.PathPlannerUtil;
+import frc4388.utility.Vector2D;
 import frc4388.utility.PathPlannerUtil.Path.Waypoint;
 import frc4388.utility.controller.DeadbandedXboxController;
 

src/main/java/frc4388/robot/subsystems/BoomBoom.java

@@ -47,14 +47,12 @@ public class BoomBoom extends SubsystemBase {
   // SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later
 
   public static class ShooterTableEntry {
-    public Double distance, hoodExt, drumVelocity;
+    public Double distance, hoodExt, drumVelocity, duration;
   }
 
   private ShooterTableEntry[] m_shooterTable;
-  /*
-  * Creates new BoomBoom subsystem, has drum shooter and angle adjuster
-  */
-  /** Creates a new BoomBoom. */
+
+  /** Creates a new BoomBoom, which has a drum shooter and angle adjuster. */
   public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) {
     m_shooterFalconLeft = shooterFalconLeft;
     m_shooterFalconRight = shooterFalconRight;
@@ -94,20 +92,39 @@ public class BoomBoom extends SubsystemBase {
     }
   }
 
+  /**
+   * This is a function that takes a value (distance) and returns a value (drumVelocity) that is a
+   * linear interpolation of the two values (drumVelocity) at the two closest points in the table
+   * (m_shooterTable) to the given value (distance). 
+   * @param distance Distance in shooter table
+   * @return Drum Velocity in units per 100 ms
+   */
   public Double getVelocity(final Double distance) {
-    // This is a function that takes a value (distance) and returns a value (drumVelocity) that is a
-    // linear interpolation of the two values (drumVelocity) at the two closest points in the table
-    // (m_shooterTable) to the given value (distance).
     return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
   }
 
+  /**
+   * This is a function that takes a value (distance) and returns a value (hoodExt) that is a linear
+   * interpolation of the two values (hoodExt) at the two closest points in the table (m_shooterTable)
+   * to the given value (distance).
+   * @param distance Distance in shooter table
+   * @return Hood extension in units
+   */
   public Double getHood(final Double distance) {
-    // This is a function that takes a value (distance) and returns a value (hoodExt) that is a linear
-    // interpolation of the two values (hoodExt) at the two closest points in the table (m_shooterTable)
-    // to the given value (distance).
     return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue();
   }
 
+  /**
+   * This is a function that takes a value (distance) and returns a value (duration) that is a linear
+   * interpolation of the two values (duration) at the two closest points in the table (m_shooterTable)
+   * to the given value (distance).
+   * @param distance Distance in shooter table
+   * @return Shot duration in seconds
+   */
+  public Double getDuration(final Double distance) {
+    return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.duration).doubleValue();
+  }
+
   /**
    * Using the given lookup value (x) and lookup getter function, locates the nearest entries in the
    * given table to be used as the lower (x0) and upper (x1) bounds for interpolation. Returns the

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -163,6 +163,7 @@ public class SwerveDrive extends SubsystemBase {
     updateSmartDash();
 
     SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
+    SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360);
 
     m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
     super.periodic();

src/main/java/frc4388/utility/Vector2D.java

@@ -0,0 +1,65 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.utility;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.drive.Vector2d;
+
+/** Aarav's good vector class (better than WPILib) */
+public class Vector2D extends Vector2d {
+
+  public double x;
+  public double y;
+  public double angle;
+
+  public Vector2D() {
+    super();
+    this.angle = Math.atan2(this.y, this.x);
+  }
+
+  public Vector2D(double x, double y) {
+    super(x, y);
+
+    this.x = x;
+    this.y = y;
+    this.angle = Math.atan2(this.y, this.x);
+  }
+
+  public static Vector2D add(Vector2D v1, Vector2D v2) {
+    return new Vector2D(v1.x + v2.x, v1.y + v2.y);
+  }
+
+  public static Vector2D subtract(Vector2D v1, Vector2D v2) {
+    return new Vector2D(v1.x - v2.x, v1.y - v2.y);
+  }
+
+  public static Vector2D multiply(Vector2D v1, double scalar) {
+    return new Vector2D(scalar * v1.x, scalar * v1.y);
+  }
+
+  public Vector2D unit() {
+    return new Vector2D(this.x / this.magnitude(), this.y / this.magnitude());
+  }
+
+  public static Vector2D round(Vector2D v, int places) {
+    int scale = (int) Math.pow(10, places);
+
+    v = Vector2D.multiply(v, scale);
+
+    v.x = Math.round(v.x);
+    v.y = Math.round(v.y);
+    v.x = v.x / scale;
+    v.y = v.y / scale;
+
+    return v;
+  }
+
+  @Override
+  public String toString() {
+    Vector2d test = new Vector2d();
+    return ("(" + this.x + ", " + this.y + ")");
+  }
+
+}

src/main/java/frc4388/utility/VelocityCorrection.java

@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.utility;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.BoomBoom;
+import frc4388.robot.subsystems.SwerveDrive;
+
+/** Add your docs here. */
+public class VelocityCorrection {
+
+  SwerveDrive swerve;
+  BoomBoom boomBoom;
+
+  // vectors (in ft and ft/sec)
+  public Vector2D position;
+  public Vector2D cartesianVelocity;
+
+  // find
+  public Vector2D target;
+
+  public VelocityCorrection(SwerveDrive swerve, BoomBoom boomBoom) {
+
+    this.swerve = swerve;
+    this.boomBoom = boomBoom;
+
+    position = new Vector2D(5, 0);//new Vector2D(this.swerve.getOdometry().getX(), this.swerve.getOdometry().getY());
+    cartesianVelocity = new Vector2D(-2, 3);//new Vector2D(this.swerve.getChassisSpeeds().vxMetersPerSecond, this.swerve.getChassisSpeeds().vyMetersPerSecond);
+
+    target = getTargetPoint();
+  }
+
+  private Vector2D getTargetPoint() {
+    double approxShotTime = 1; // TODO: get shot time from shooter tables
+    
+    Vector2D targetPoint = Vector2D.multiply(this.cartesianVelocity, -1 * approxShotTime);
+    
+    return Vector2D.round(targetPoint, 5);
+  }
+
+}