From 032e176efeac426ecd3443053e25911ec6a9b3e4 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 12 Mar 2022 14:05:33 -0700 Subject: [PATCH 1/4] velocity correction and shooter tables time --- simgui.json | 3 + src/main/deploy/ShooterData.csv | 10 ++-- src/main/java/frc4388/robot/Robot.java | 7 +++ .../frc4388/robot/subsystems/SwerveDrive.java | 1 + src/main/java/frc4388/utility/Vector2D.java | 56 +++++++++++++++++++ .../frc4388/utility/VelocityCorrection.java | 47 ++++++++++++++++ 6 files changed, 119 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc4388/utility/Vector2D.java create mode 100644 src/main/java/frc4388/utility/VelocityCorrection.java diff --git a/simgui.json b/simgui.json index 4e5f439..7037638 100644 --- a/simgui.json +++ b/simgui.json @@ -22,6 +22,7 @@ "types": { "/FMSInfo": "FMSInfo", "/LiveWindow/BoomBoom": "Subsystem", + "/LiveWindow/Climber": "Subsystem", "/LiveWindow/Hood": "Subsystem", "/LiveWindow/Intake": "Subsystem", "/LiveWindow/LED": "Subsystem", @@ -46,6 +47,8 @@ "/LiveWindow/Ungrouped/Talon FX [23]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [24]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [2]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [30]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [31]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [3]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [4]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [5]": "Motor Controller", diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index 5e4b222..b0f3627 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -1,5 +1,5 @@ -Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) -96 ,-25.00 ,0.425 -144 ,-47.57 ,0.475 -192 ,-55.55 ,0.500 -240 ,-96.00 ,0.525 \ No newline at end of file +Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Time (s) +96 ,-25.00 ,0.425 ,0 +144 ,-47.57 ,0. ,0 +192 ,-55.55 ,0.500 ,0 +240 ,-96.00 ,0.525 ,0 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 51ed393..1215bbe 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/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()); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index f0419d3..b5c3176 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/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(); diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java new file mode 100644 index 0000000..6a56a21 --- /dev/null +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -0,0 +1,56 @@ +// 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.wpilibj.drive.Vector2d; + +/** Aarav's good vector class (better than WPILib) */ +public class Vector2D extends Vector2d { + + public double x; + public double y; + + public Vector2D() { + super(); + } + + public Vector2D(double x, double y) { + super(x, y); + + this.x = x; + this.y = y; + } + + 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, 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() { + return ("(" + this.x + ", " + this.y + ")"); + } + +} diff --git a/src/main/java/frc4388/utility/VelocityCorrection.java b/src/main/java/frc4388/utility/VelocityCorrection.java new file mode 100644 index 0000000..cc11d0d --- /dev/null +++ b/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); + } + +} From 7e2505f0f377c5c372245a501d5e7b5e9445c840 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 12 Mar 2022 14:21:03 -0700 Subject: [PATCH 2/4] unit vector method --- src/main/java/frc4388/utility/Vector2D.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java index 6a56a21..0e194e4 100644 --- a/src/main/java/frc4388/utility/Vector2D.java +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -35,6 +35,10 @@ public class Vector2D extends Vector2d { 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); @@ -50,6 +54,7 @@ public class Vector2D extends Vector2d { @Override public String toString() { + Vector2d test = new Vector2d(); return ("(" + this.x + ", " + this.y + ")"); } From 06ccba971ef3f4398946d823d5f9387537f1362b Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 12 Mar 2022 14:35:47 -0700 Subject: [PATCH 3/4] vector2d angle --- src/main/java/frc4388/robot/RobotContainer.java | 1 + src/main/java/frc4388/utility/Vector2D.java | 4 ++++ 2 files changed, 5 insertions(+) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index fb53f70..09051b9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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; diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java index 0e194e4..7255d66 100644 --- a/src/main/java/frc4388/utility/Vector2D.java +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -4,6 +4,7 @@ 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) */ @@ -11,9 +12,11 @@ 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) { @@ -21,6 +24,7 @@ public class Vector2D extends Vector2d { this.x = x; this.y = y; + this.angle = Math.atan2(this.y, this.x); } public static Vector2D add(Vector2D v1, Vector2D v2) { From 35662f72f9ff05490d517feb1997e7a2a8e4e647 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 12 Mar 2022 14:54:42 -0700 Subject: [PATCH 4/4] shooter tables duration --- src/main/deploy/ShooterData.csv | 2 +- .../frc4388/robot/subsystems/BoomBoom.java | 39 +++++++++++++------ 2 files changed, 29 insertions(+), 12 deletions(-) diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index b0f3627..16e194d 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -1,4 +1,4 @@ -Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Time (s) +Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Duration (s) 96 ,-25.00 ,0.425 ,0 144 ,-47.57 ,0. ,0 192 ,-55.55 ,0.500 ,0 diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index b7eaced..a2e87df 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/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