boilerplate vel correction

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

@@ -19,12 +19,14 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.drive.Vector2d;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 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
@@ -138,9 +140,19 @@ public class Robot extends TimedRobot {
     // robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
+
+    // velocity correction tests
+    VelocityCorrection vc = new VelocityCorrection(m_robotContainer.m_robotSwerveDrive, m_robotContainer.m_robotBoomBoom);
     
     //SmartDashboard.putNumber("Turret Encoder Position", m_robotContainer.m_robotTurret.m_boomBoomRotateEncoder.getPosition());
     //SmartDashboard.putNumber("Hood Encoder Position", m_robotContainer.m_robotHood.m_angleEncoder.getPosition());
+    // System.out.println("Position: (" + vc.position.x + ", " + vc.position.y + ")");
+    // System.out.println("Velocity: (" + vc.cartesianVelocity.x + ", " + vc.cartesianVelocity.y + ")");
+    // System.out.println("Tangential Velocity: (" + vc.tangentialVelocity.x + ", " + vc.tangentialVelocity.y + ")");
+    // System.out.println("Radial Velocity: (" + vc.radialVelocity.x + ", " + vc.radialVelocity.y + ")");
+
+    SmartDashboard.putNumber("Turret Encoder Position", m_robotContainer.m_robotTurret.m_boomBoomRotateEncoder.getPosition());
+    SmartDashboard.putNumber("Hood Encoder Position", m_robotContainer.m_robotHood.m_angleEncoder.getPosition());
 
     // odo chooser stuff
     addOdoChoices(new Pose2d(1, 1, new Rotation2d(Math.PI/6)), 

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

@@ -414,8 +414,8 @@ public class RobotContainer {
     Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d();
     // FIXME: Chassis speeds are created from joystick inputs and do not reflect
     // actual robot velocity.
-    Translation2d velocity = new Translation2d(m_robotSwerveDrive.getChassisSpeeds()[0],
-        m_robotSwerveDrive.getChassisSpeeds()[1]);
+    Translation2d velocity = new Translation2d(m_robotSwerveDrive.getChassisSpeeds().vxMetersPerSecond,
+        m_robotSwerveDrive.getChassisSpeeds().vyMetersPerSecond);
     Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false,
         SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false);
     pathPoints.add(waypoint);

src/main/java/frc4388/robot/commands/Shoot.java

@@ -96,7 +96,7 @@ public class Shoot extends CommandBase {
    * Updates error for custom PID.
    */
   public void updateError() {
-    m_targetPoint = new Pose2d(hTargetDistanceFromHub(), vTargetDistanceFromHub(), SwerveDriveConstants.HUB_POSE.getRotation());
+    m_targetPoint = SwerveDriveConstants.HUB_POSE;
     m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get());
     // m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees());
     error = (m_targetAngle - turretDummy.get() + 360) % 360;
@@ -152,43 +152,6 @@ public class Shoot extends CommandBase {
     output = kP * proportional + kI * integral + kD * derivative;
     normOutput = output/360 * inverted;
   }
-  // TODO: horizontal velocity correction
-  public double hTargetDistanceFromHub() {
-    
-    double hVel = m_swerve.getChassisSpeeds()[0];
-    double velBeforeCorrection = m_boomBoom.getVelocity(m_distance);
-    double vDistanceFromHub = m_odoY;
-    double approxTravelTime = vDistanceFromHub / velBeforeCorrection;
-    double hTargetDistanceFromHub = hVel * approxTravelTime;
-    
-    // return hTargetDistanceFromHub;
-    return 0.0; // this is for no velocity correction
-  }
-
-  public Pose2d findTargetPoint() {
-
-    // position vector and radius
-    Translation2d position = new Translation2d(m_odoX, m_odoY);
-    double radius = position.getNorm();
-
-    // equation of circle = x^2 + y^2 = m_distance^2
-    // derivative of circle = 2x + 2y * y' = 0 --> y' = -x/y
-
-    // velocity vector (x, y)
-    Translation2d cartesianVelocity = new Translation2d(m_swerve.getChassisSpeeds()[0], m_swerve.getChassisSpeeds()[1]);
-
-    // unit tangential vector
-    Translation2d tangential = new Translation2d(0, 0);
-    // velocity vector (tangential, radial)
-    Translation2d polarVelocity = new Translation2d(0, 0);
-
-    return SwerveDriveConstants.HUB_POSE;
-  }
-
-  // TODO: vertical velocity correction
-  public double vTargetDistanceFromHub() {
-    return 0.0; // this is for no velocity correction
-  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override

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

@@ -5,6 +5,7 @@
 package frc4388.robot.subsystems;
 
 import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
+import com.ctre.phoenix.sensors.PigeonIMUConfiguration;
 import com.ctre.phoenix.sensors.WPI_PigeonIMU;
 
 import edu.wpi.first.math.VecBuilder;
@@ -186,8 +187,8 @@ public class SwerveDrive extends SubsystemBase {
    * Gets the current chassis speeds in m/s and rad/s.
    * @return Current chassis speeds (vx, vy, ω)
    */
-  public double[] getChassisSpeeds() {
-    return new double[] {chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond};
+  public ChassisSpeeds getChassisSpeeds() {
+    return chassisSpeeds;
   }
 
   /**

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

@@ -0,0 +1,97 @@
+// 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.wpilibj.drive.Vector2d;
+import frc4388.robot.subsystems.BoomBoom;
+import frc4388.robot.subsystems.SwerveDrive;
+
+/** Add your docs here. */
+public class VelocityCorrection {
+
+  SwerveDrive swerve;
+  BoomBoom boomBoom;
+
+  // vectors
+  public Vector2d position;
+  public Vector2d cartesianVelocity;
+  public Vector2d tangentialVelocity, unitTangentialVelocity;
+  public Vector2d radialVelocity, unitRadialVelocity;
+
+  double radius;
+
+  // 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(1, 0);//new Vector2d(this.swerve.getChassisSpeeds().vxMetersPerSecond, this.swerve.getChassisSpeeds().vyMetersPerSecond);
+
+    tangentialVelocity = getTangentialVelocity();
+    radialVelocity = getRadialVelocity();
+
+    radius = position.magnitude();
+
+    target = getTargetPoint();
+  }
+
+  private Vector2d getRadialVelocity() {
+    Vector2d ret = VelocityCorrection.subtract(cartesianVelocity, tangentialVelocity); 
+    unitRadialVelocity = VelocityCorrection.multiply(ret, (1/ret.magnitude()));
+    return ret;
+  }
+
+  private Vector2d getTangentialVelocity() {
+
+    double hubElevation = Math.atan2(position.y, position.x);
+    unitTangentialVelocity = new Vector2d(Math.cos(hubElevation - (Math.PI/2)), Math.sin(hubElevation - (Math.PI/2)));
+    double tangentialVelocityMag = cartesianVelocity.scalarProject(unitTangentialVelocity);
+
+    return VelocityCorrection.multiply(unitTangentialVelocity, tangentialVelocityMag);
+  }
+
+  private Vector2d getTargetPoint() {
+
+    double drumVelocity = boomBoom.getVelocity(radius);
+    double ballVelocity = drumVelocity; // TODO: convert from drum velocity to actual ball velocity 
+    double approxShotTime = radius / ballVelocity; // TODO: better approximation to get shot time (physics/calculus?)
+
+    Vector2d tangentialTargetPoint = VelocityCorrection.multiply(unitTangentialVelocity, -1 * approxShotTime);
+    Vector2d radialTargetPoint = VelocityCorrection.multiply(unitRadialVelocity, -1 * approxShotTime);
+    Vector2d targetPoint = VelocityCorrection.add(tangentialTargetPoint, radialTargetPoint);
+
+    return targetPoint;
+  }
+
+  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 static Vector2d round(Vector2d v) {
+    v = VelocityCorrection.multiply(v, 1000);
+    v.x = Math.round(v.x);
+    v.y = Math.round(v.y);
+    v.x = v.x / 1000;
+    v.y = v.y / 1000;
+    return v;
+  }
+
+}