From 68c69d726222a4fc6795551c402e3ab36ee7b1e6 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Sun, 13 Mar 2022 17:02:55 -0600 Subject: [PATCH] drum pid + tentative shooting values + get voltage --- src/main/deploy/ShooterData.csv | 7 +++--- src/main/java/frc4388/robot/Constants.java | 16 +++++++------ src/main/java/frc4388/robot/Robot.java | 2 ++ .../java/frc4388/robot/RobotContainer.java | 24 ++++++++++++++----- .../frc4388/robot/subsystems/BoomBoom.java | 12 +++++++++- .../frc4388/robot/subsystems/SwerveDrive.java | 8 +++++-- .../robot/subsystems/SwerveModule.java | 4 ++++ .../java/frc4388/robot/subsystems/Turret.java | 6 +---- .../robot/subsystems/VisionOdometry.java | 2 +- 9 files changed, 55 insertions(+), 26 deletions(-) diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index 16e194d..1284d91 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -1,5 +1,4 @@ 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 -240 ,-96.00 ,0.525 ,0 \ No newline at end of file +81, ,0 ,8000 ,0 +150, ,-62.1 ,10000 ,0 +227, ,-103.9 ,10500 ,0 diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 6393625..62e5422 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -32,7 +32,7 @@ import frc4388.utility.LEDPatterns; */ public final class Constants { public static final class SwerveDriveConstants { - public static final double ROTATION_SPEED = 4.0; + public static final double ROTATION_SPEED = 2.0; public static final double WIDTH = 23.5; public static final double HEIGHT = 23.5; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11; @@ -183,7 +183,7 @@ public final class Constants { true, 27, 0, 0); public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22; - public static final double TURRET_SPEED_MULTIPLIER = 0.75d; + public static final double TURRET_SPEED_MULTIPLIER = 0.4d; public static final int DEGREES_PER_ROT = 0; public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; @@ -195,19 +195,21 @@ public final class Constants { /* Turret Constants */ // ID public static final int TURRET_MOTOR_CAN_ID = 19; + //Gains for turret public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); + //Gains for hood public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7); public static final double SHOOTER_TURRET_MIN = -1.0; - public static final double TURRET_FORWARD_LIMIT = 17.0; - public static final double TURRET_REVERSE_LIMIT = -105.0; - - public static final Gains DRUM_SHOOTER_GAINS = new Gains(0, 0, 0, 0, 0, 0); // TODO: tune values + public static final double TURRET_FORWARD_LIMIT = 17.0; // TODO: find + public static final double TURRET_REVERSE_LIMIT = -105.0; // TODO: find + //Shooter gains for actual Drum + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0, 15.0, 0.05, 0, 0); // TODO: tune values /* Hood Constants */ public static final int SHOOTER_ANGLE_ADJUST_ID = 20; public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; // TODO: Find public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; // TODO: Find - public static final double HOOD_FORWARD_LIMIT = -0.0; // TODO: find + public static final double HOOD_FORWARD_LIMIT = 0.0; // TODO: find public static final double HOOD_REVERSE_LIMIT = -150; // TODO: find } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 04a55bc..d04fae9 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -143,6 +143,8 @@ public class Robot extends TimedRobot { m_robotContainer.m_robotSwerveDrive.getCurrent(); // m_robotContainer.m_robotTurret.getCurrent(); SmartDashboard.putNumber("Total Robot Current Draw", current); + SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage()); + SmartDashboard.putNumber("Drive Train Current", m_robotContainer.m_robotSwerveDrive.getCurrent()); // Runs the Scheduler. This is responsible for polling buttons, adding // newly-scheduled // commands, running already-scheduled commands, removing finished or diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 676f275..8667896 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -259,9 +259,11 @@ public class RobotContainer { // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft - .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))) + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(10000))) + // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.6))) + // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); // .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-96))) - .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); + .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0))); new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage)) @@ -271,13 +273,23 @@ public class RobotContainer { .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) + // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + + // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) + // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom)); new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(-500), m_robotBoomBoom)); + + // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + // .whileHeld(new InstantCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) + // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); // Right Bumper > Storage In // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index a2e87df..bc8d8cf 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -37,6 +37,7 @@ public class BoomBoom extends SubsystemBase { double velP; double input; + public double pidOffset = 0; public boolean m_isDrumReady = false; public double m_fireVel; @@ -56,6 +57,7 @@ public class BoomBoom extends SubsystemBase { public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { m_shooterFalconLeft = shooterFalconLeft; m_shooterFalconRight = shooterFalconRight; + setShooterGains(); try { // This is a helper class that allows us to read a CSV file into a Java array. @@ -183,6 +185,9 @@ public class BoomBoom extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run // speed2 = SmartDashboard.getNumber("Shooter Offset", 0.0); + SmartDashboard.putNumber("Shooter Current", getCurrent()); + SmartDashboard.putNumber("Shooter Voltage", m_shooterFalconLeft.getMotorOutputVoltage()); + SmartDashboard.putNumber("Shooter Actual Velocity", m_shooterFalconLeft.getSelectedSensorVelocity()); } public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) { @@ -211,7 +216,8 @@ public class BoomBoom extends SubsystemBase { } public void runDrumShooterVelocityPID(double targetVel) { - m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init + SmartDashboard.putNumber("Target Drum Velocity", 10000 + pidOffset); + m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel + pidOffset); // Init // New BoomBoom controller stuff // Controls a motor with the output of the BangBang controller @@ -223,6 +229,10 @@ public class BoomBoom extends SubsystemBase { // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); } + public void updateOffset(double change) { + pidOffset = pidOffset + change; + } + public void increaseSpeed(double amount) { speed2 = speed2 + amount; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index b5c3176..2e3d754 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -112,9 +112,9 @@ public class SwerveDrive extends SubsystemBase { double ySpeedMetersPerSecond = speed.getY(); chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - -rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180))) + -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180))) : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond, - -rot * SwerveDriveConstants.ROTATION_SPEED); + -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds); setModuleStates(states); } @@ -263,4 +263,8 @@ public class SwerveDrive extends SubsystemBase { public double getCurrent(){ return m_leftFront.getCurrent() + m_rightFront.getCurrent() + m_rightBack.getCurrent() + m_leftBack.getCurrent(); } + + public double getVoltage(){ + return m_leftFront.getVoltage() + m_rightFront.getVoltage() + m_rightBack.getVoltage() + m_leftBack.getVoltage(); + } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 299ddd4..11e861e 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -195,4 +195,8 @@ public class SwerveModule extends SubsystemBase { public double getCurrent(){ return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent(); } + + public double getVoltage(){ + return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage())); + } } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 9ca1c32..fe99959 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -7,7 +7,7 @@ package frc4388.robot.subsystems; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMax.SoftLimitDirection; -import com.revrobotics.SparkMaxRelativeEncoder.Type; + import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxLimitSwitch; @@ -60,8 +60,6 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.setInverted(true); - // m_boomBoomRotateMotor.getAlternateEncoder(4096).setPosition(0); - m_boomBoomRotatePIDController.setP(m_shooterTGains.kP); m_boomBoomRotatePIDController.setI(m_shooterTGains.kI); m_boomBoomRotatePIDController.setD(m_shooterTGains.kD); @@ -104,12 +102,10 @@ public class Turret extends SubsystemBase { } public double getboomBoomRotatePosition() { - // return 0.0; return m_boomBoomRotateEncoder.getPosition(); } public double getBoomBoomAngleDegrees() { - // return 0.0; return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360 / ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT; } diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java index d4b60db..91c435c 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -112,7 +112,7 @@ public class VisionOdometry extends SubsystemBase { } guess = correctGuessForCenter(guess, m_shooter.getBoomBoomAngleDegrees()); - guess = correctGuessForGyro(guess, m_drive.getRegGyro().getDegrees()); + // guess = correctGuessForGyro(guess, m_drive.getRegGyro().getDegrees()); SmartDashboard.putNumber("Vision ODO x: ", guess.x); SmartDashboard.putNumber("Vision ODO y: ", guess.y);