From fa7d48c864f0a636e382e096cd3b04304014d135 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 7 Mar 2022 19:56:25 -0700 Subject: [PATCH] boilerplate vel correction --- src/main/java/frc4388/robot/Robot.java | 12 +++ .../java/frc4388/robot/RobotContainer.java | 4 +- .../java/frc4388/robot/commands/Shoot.java | 39 +------- .../frc4388/robot/subsystems/SwerveDrive.java | 5 +- .../frc4388/utility/VelocityCorrection.java | 97 +++++++++++++++++++ 5 files changed, 115 insertions(+), 42 deletions(-) create mode 100644 src/main/java/frc4388/utility/VelocityCorrection.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index e9d690b..6628051 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/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)), diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9e2c886..bc5b1ef 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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); diff --git a/src/main/java/frc4388/robot/commands/Shoot.java b/src/main/java/frc4388/robot/commands/Shoot.java index b2a574f..9755bc5 100644 --- a/src/main/java/frc4388/robot/commands/Shoot.java +++ b/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 diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 36e0f52..65860f5 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/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; } /** diff --git a/src/main/java/frc4388/utility/VelocityCorrection.java b/src/main/java/frc4388/utility/VelocityCorrection.java new file mode 100644 index 0000000..d7c6c0c --- /dev/null +++ b/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; + } + +}