From 8a25b7c1d5d955cf8505262f559deaab4036e1b2 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 14 Mar 2022 18:29:11 -0600 Subject: [PATCH] switch to pigeon2 + vector2d docs --- src/main/java/frc4388/robot/Robot.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 6 ++-- src/main/java/frc4388/robot/RobotMap.java | 4 +-- .../frc4388/robot/commands/TrackTarget.java | 2 +- .../frc4388/robot/subsystems/Extender.java | 2 -- .../frc4388/robot/subsystems/SwerveDrive.java | 22 +++++--------- src/main/java/frc4388/utility/Vector2D.java | 29 ++++++++++++++++++- .../utility/{ => desmos}/DesmosClient.html | 0 .../utility/{ => desmos}/DesmosServer.java | 2 +- 9 files changed, 44 insertions(+), 25 deletions(-) rename src/main/java/frc4388/utility/{ => desmos}/DesmosClient.html (100%) rename src/main/java/frc4388/utility/{ => desmos}/DesmosServer.java (99%) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index b5c7845..af3774a 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -24,9 +24,9 @@ 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.DesmosServer; import frc4388.utility.RobotTime; import frc4388.utility.VelocityCorrection; +import frc4388.utility.desmos.DesmosServer; /** * The VM is configured to automatically run this class, and to call the diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c6146b4..0b55c20 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -297,8 +297,10 @@ public class RobotContainer { // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))) + // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) + // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)) + .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0))); // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) // .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom)); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 86701be..91b1854 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -10,7 +10,7 @@ import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import com.ctre.phoenix.sensors.WPI_Pigeon2; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; @@ -63,7 +63,7 @@ public class RobotMap { public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); - public final WPI_PigeonIMU gyro = new WPI_PigeonIMU(SwerveDriveConstants.GYRO_ID); + public final WPI_Pigeon2 gyro = new WPI_Pigeon2(SwerveDriveConstants.GYRO_ID); public SwerveModule leftFront; public SwerveModule leftBack; diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index d2aa6ec..8a5f59f 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -18,7 +18,7 @@ import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.VisionOdometry; -import frc4388.utility.DesmosServer; +import frc4388.utility.desmos.DesmosServer; public class TrackTarget extends CommandBase { /** Creates a new TrackTarget. */ diff --git a/src/main/java/frc4388/robot/subsystems/Extender.java b/src/main/java/frc4388/robot/subsystems/Extender.java index 501bb56..1680b88 100644 --- a/src/main/java/frc4388/robot/subsystems/Extender.java +++ b/src/main/java/frc4388/robot/subsystems/Extender.java @@ -12,7 +12,6 @@ import com.revrobotics.CANSparkMax.SoftLimitDirection; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ExtenderConstants; -import frc4388.utility.DesmosServer; public class Extender extends SubsystemBase { @@ -51,7 +50,6 @@ public class Extender extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run SmartDashboard.putNumber("Extender Position", m_extenderMotor.getEncoder().getPosition()); - DesmosServer.putDouble("ExtenderPosition", m_extenderMotor.getEncoder().getPosition()); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 2e3d754..5f60cdb 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,11 +4,9 @@ package frc4388.robot.subsystems; -import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus; -import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import com.ctre.phoenix.sensors.WPI_Pigeon2; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.Vector; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -46,14 +44,8 @@ public class SwerveDrive extends SubsystemBase { m_backLeftLocation, m_backRightLocation); public SwerveModule[] modules; - public WPI_PigeonIMU m_gyro; - protected FusionStatus fstatus = new FusionStatus(); + public WPI_Pigeon2 m_gyro; - /* - * Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. - * The numbers used - * below are robot specific, and should be tuned. - */ public SwerveDrivePoseEstimator m_poseEstimator; public SwerveDriveOdometry m_odometry; @@ -65,7 +57,7 @@ public class SwerveDrive extends SubsystemBase { private final Field2d m_field = new Field2d(); public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack, - WPI_PigeonIMU gyro) { + WPI_Pigeon2 gyro) { m_leftFront = leftFront; m_leftBack = leftBack; @@ -74,14 +66,14 @@ public class SwerveDrive extends SubsystemBase { m_gyro = gyro; modules = new SwerveModule[] { m_leftFront, m_rightFront, m_leftBack, m_rightBack}; - + m_poseEstimator = new SwerveDrivePoseEstimator( m_gyro.getRotation2d(), new Pose2d(), m_kinematics, - VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)), - VecBuilder.fill(Units.degreesToRadians(1)), - VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); + VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)), // TODO: tune + VecBuilder.fill(Units.degreesToRadians(1)), // TODO: tune + VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d()); diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java index 7255d66..fe20cfc 100644 --- a/src/main/java/frc4388/utility/Vector2D.java +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -27,22 +27,50 @@ public class Vector2D extends Vector2d { this.angle = Math.atan2(this.y, this.x); } + /** + * Add two vectors, component-wise. + * @param v1 First vector in the addition. + * @param v2 Second vector in the addition. + * @return New vector which is the sum. + */ public static Vector2D add(Vector2D v1, Vector2D v2) { return new Vector2D(v1.x + v2.x, v1.y + v2.y); } + /** + * Subtract two vectors, component-wise. + * @param v1 First vector in the subtraction. + * @param v2 Second vector in the subtraction. + * @return New vector which is the difference. + */ public static Vector2D subtract(Vector2D v1, Vector2D v2) { return new Vector2D(v1.x - v2.x, v1.y - v2.y); } + /** + * Multiply a vector with a scalar, component-wise. + * @param v1 Vector to multiply. + * @param v2 Scalar to multiply. + * @return New vector which is the product. + */ public static Vector2D multiply(Vector2D v1, double scalar) { return new Vector2D(scalar * v1.x, scalar * v1.y); } + /** + * Find unit vector. + * @return The unit vector. + */ public Vector2D unit() { return new Vector2D(this.x / this.magnitude(), this.y / this.magnitude()); } + /** + * Round a vector to a certain number of places, component-wise. + * @param v Vector to round. + * @param places Number of places to round to. + * @return New rounded vector. + */ public static Vector2D round(Vector2D v, int places) { int scale = (int) Math.pow(10, places); @@ -58,7 +86,6 @@ public class Vector2D extends Vector2d { @Override public String toString() { - Vector2d test = new Vector2d(); return ("(" + this.x + ", " + this.y + ")"); } diff --git a/src/main/java/frc4388/utility/DesmosClient.html b/src/main/java/frc4388/utility/desmos/DesmosClient.html similarity index 100% rename from src/main/java/frc4388/utility/DesmosClient.html rename to src/main/java/frc4388/utility/desmos/DesmosClient.html diff --git a/src/main/java/frc4388/utility/DesmosServer.java b/src/main/java/frc4388/utility/desmos/DesmosServer.java similarity index 99% rename from src/main/java/frc4388/utility/DesmosServer.java rename to src/main/java/frc4388/utility/desmos/DesmosServer.java index 2c743cd..e3ee55b 100644 --- a/src/main/java/frc4388/utility/DesmosServer.java +++ b/src/main/java/frc4388/utility/desmos/DesmosServer.java @@ -1,4 +1,4 @@ -package frc4388.utility; +package frc4388.utility.desmos; import java.io.BufferedReader; import java.io.IOException;