From 38459ba40c3d27f69082352187ef719e485c1cb2 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 16 Mar 2022 14:32:41 -0600 Subject: [PATCH] small changes shtuf --- src/main/java/frc4388/robot/Robot.java | 15 --------------- .../commands/StorageCommands/ManageStorage.java | 2 +- .../frc4388/robot/subsystems/SwerveDrive.java | 6 +++--- .../java/frc4388/robot/subsystems/Turret.java | 9 ++++++--- src/main/java/frc4388/utility/Vector2D.java | 10 ++++++++++ 5 files changed, 20 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index cb70709..fbb7cfa 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -162,15 +162,6 @@ public class Robot extends TimedRobot { // robot's periodic // 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()); // odo chooser stuff addOdoChoices(new Pose2d(1, 1, new Rotation2d(Math.PI/6)), @@ -178,12 +169,6 @@ public class Robot extends TimedRobot { new Pose2d(1, 3, new Rotation2d(Math.PI/4))); updateOdoChooser(); SmartDashboard.putData("Odometry Chooser", odoChooser); - - // print odometry data to smart dashboard for debugging (if causing timeout - // errors, you can comment it) - SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX()); - SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY()); - SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees()); } public void updateOdoChooser() { diff --git a/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java b/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java index 0639307..44cc062 100644 --- a/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java +++ b/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java @@ -53,7 +53,7 @@ public class ManageStorage extends CommandBase { // * CommandScheduler.getInstance().schedule(new ExampleCommand()); // * new ExampleCommand().schedule(); - // * new ExampleCommand().execute(); + // * new ExampleCommand().execute(); (accompanied by initialize and onFinished) new SpitOutWrongColor(this.storage, this.drum, this.turret); // ? is this how you run a command inside a command } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index ee63473..262c950 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -169,9 +169,9 @@ public class SwerveDrive extends SubsystemBase { // chassis speeds // TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds - SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond); - SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond); + // SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond); + // SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond); + // SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index c0bf99b..d2dc34f 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -45,10 +45,8 @@ public class Turret extends SubsystemBase { m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_boomBoomRightLimit.enableLimitSwitch(true); - m_boomBoomLeftLimit.enableLimitSwitch(true); + setTurretLimitSwitches(true); - // setTurretLimitSwitches(false); // SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); // SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); @@ -74,8 +72,13 @@ public class Turret extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run + + SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition()); SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); + SmartDashboard.putBoolean("Left Limit Switch Pressed", m_boomBoomLeftLimit.isPressed()); + SmartDashboard.putBoolean("Right Limit Switch Pressed", m_boomBoomRightLimit.isPressed()); + if (m_boomBoomLeftLimit.isPressed()) m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_LIMIT - 2); if (m_boomBoomRightLimit.isPressed()) m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_LIMIT + 2); } diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java index fe20cfc..8ce1968 100644 --- a/src/main/java/frc4388/utility/Vector2D.java +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -57,6 +57,16 @@ public class Vector2D extends Vector2d { return new Vector2D(scalar * v1.x, scalar * v1.y); } + /** + * Divide a vector with a scalar, component-wise. + * @param v1 Vector to divide. + * @param v2 Scalar to divide. + * @return New vector which is the division. + */ + public static Vector2D divide(Vector2D v1, double scalar) { + return new Vector2D(v1.x / scalar, v1.y / scalar); + } + /** * Find unit vector. * @return The unit vector.