From 5d368d95eb069592d3c97fbefaaebb85248874e8 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Fri, 1 Mar 2024 21:32:51 -0500 Subject: [PATCH 1/2] [#360] Tuned 1,2,3 --- src/main/java/frc/robot/Constants.java | 237 +++++++++--------- src/main/java/frc/robot/RobotContainer.java | 5 +- .../robot/command/shoot/SourceCollect.java | 8 +- .../java/frc/robot/command/shoot/Tune.java | 52 ++++ 4 files changed, 180 insertions(+), 122 deletions(-) create mode 100644 src/main/java/frc/robot/command/shoot/Tune.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bd10fb38..8418e523 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -147,12 +147,14 @@ public static class AutonomousConstants { public static final double CONTROL_LOOP_PERIOD = 0.004; // IS this right? - public static final PathConstraints PATH_CONSTRAINTS = new PathConstraints(2.0, 1, 1.0, 0.5); // TODO get - // constants + public static final PathConstraints PATH_CONSTRAINTS = + new PathConstraints(2.0, 1, 1.0, 0.5); // TODO get + // constants // For Pathfinding // TODO get real poses to pathfind to - public static final Pose2d TARGET_POSE = new Pose2d(new Translation2d(0, 0), new Rotation2d(0d)); + public static final Pose2d TARGET_POSE = + new Pose2d(new Translation2d(0, 0), new Rotation2d(0d)); } @@ -166,15 +168,17 @@ public static class TunerConstants { .withKD(0.2).withKS(0).withKV(1.5).withKA(0); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = new Slot0Configs().withKP(3).withKI(0).withKD(0).withKS(0) - .withKV(0).withKA(0); + private static final Slot0Configs driveGains = + new Slot0Configs().withKP(3).withKI(0).withKD(0).withKS(0).withKV(0).withKA(0); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType steerClosedLoopOutput = ClosedLoopOutputType.Voltage; + private static final ClosedLoopOutputType steerClosedLoopOutput = + ClosedLoopOutputType.Voltage; // The closed-loop output type to use for the drive motors; // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.TorqueCurrentFOC; + private static final ClosedLoopOutputType driveClosedLoopOutput = + ClosedLoopOutputType.TorqueCurrentFOC; // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot @@ -206,24 +210,24 @@ public static class TunerConstants { private static final double kSteerFrictionVoltage = 0.25; private static final double kDriveFrictionVoltage = 0.25; - private static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withPigeon2Id(kPigeonId) - .withCANbusName(kCANbusName); - - private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withWheelRadius(kWheelRadiusInches).withSlipCurrent(kSlipCurrentA) - .withSteerMotorGains(steerGains).withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(steerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(driveClosedLoopOutput) - .withSpeedAt12VoltsMps(kSpeedAt12VoltsMps).withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage) - .withFeedbackSource(SteerFeedbackType.FusedCANcoder) - .withCouplingGearRatio(kCoupleRatio) - .withSteerMotorInverted(kSteerMotorReversed); + private static final SwerveDrivetrainConstants DrivetrainConstants = + new SwerveDrivetrainConstants().withPigeon2Id(kPigeonId) + .withCANbusName(kCANbusName); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory().withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withWheelRadius(kWheelRadiusInches).withSlipCurrent(kSlipCurrentA) + .withSteerMotorGains(steerGains).withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(steerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(driveClosedLoopOutput) + .withSpeedAt12VoltsMps(kSpeedAt12VoltsMps).withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage) + .withFeedbackSource(SteerFeedbackType.FusedCANcoder) + .withCouplingGearRatio(kCoupleRatio) + .withSteerMotorInverted(kSteerMotorReversed); // OFFSETS Rhapsody private static final double kFrontLeftEncoderOffsetRh = 0.0546875; @@ -259,46 +263,46 @@ public static class TunerConstants { private static final double kBackRightXPosInches = -10.825; private static final double kBackRightYPosInches = -10.825; - private static final SwerveModuleConstants FrontLeft = ConstantCreator.createModuleConstants( - CAN.kFrontLeftSteerMotorId, - CAN.kFrontLeftDriveMotorId, CAN.kFrontLeftEncoderId, - kFrontLeftEncoderOffset, Units.inchesToMeters(kFrontLeftXPosInches), - Units.inchesToMeters(kFrontLeftYPosInches), kInvertLeftSide); - private static final SwerveModuleConstants FrontRight = ConstantCreator.createModuleConstants( - CAN.kFrontRightSteerMotorId, - CAN.kFrontRightDriveMotorId, CAN.kFrontRightEncoderId, - kFrontRightEncoderOffset, Units.inchesToMeters(kFrontRightXPosInches), - Units.inchesToMeters(kFrontRightYPosInches), kInvertRightSide); + private static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants(CAN.kFrontLeftSteerMotorId, + CAN.kFrontLeftDriveMotorId, CAN.kFrontLeftEncoderId, + kFrontLeftEncoderOffset, Units.inchesToMeters(kFrontLeftXPosInches), + Units.inchesToMeters(kFrontLeftYPosInches), kInvertLeftSide); + private static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants(CAN.kFrontRightSteerMotorId, + CAN.kFrontRightDriveMotorId, CAN.kFrontRightEncoderId, + kFrontRightEncoderOffset, Units.inchesToMeters(kFrontRightXPosInches), + Units.inchesToMeters(kFrontRightYPosInches), kInvertRightSide); private static final SwerveModuleConstants BackLeft = ConstantCreator.createModuleConstants( CAN.kBackLeftSteerMotorId, CAN.kBackLeftDriveMotorId, CAN.kBackLeftEncoderId, kBackLeftEncoderOffset, Units.inchesToMeters(kBackLeftXPosInches), Units.inchesToMeters(kBackLeftYPosInches), kInvertLeftSide); - private static final SwerveModuleConstants BackRight = ConstantCreator.createModuleConstants( - CAN.kBackRightSteerMotorId, - CAN.kBackRightDriveMotorId, CAN.kBackRightEncoderId, - kBackRightEncoderOffset, Units.inchesToMeters(kBackRightXPosInches), - Units.inchesToMeters(kBackRightYPosInches), kInvertRightSide); - - private static final SwerveModuleConstants FrontLeftRh = ConstantCreator.createModuleConstants( - CAN.kFrontLeftSteerMotorId, - CAN.kFrontLeftDriveMotorId, CAN.kFrontLeftEncoderId, - kFrontLeftEncoderOffsetRh, Units.inchesToMeters(kFrontLeftXPosInchesRh), - Units.inchesToMeters(kFrontLeftYPosInchesRh), kInvertLeftSide); - private static final SwerveModuleConstants FrontRightRh = ConstantCreator.createModuleConstants( - CAN.kFrontRightSteerMotorId, - CAN.kFrontRightDriveMotorId, CAN.kFrontRightEncoderId, - kFrontRightEncoderOffsetRh, Units.inchesToMeters(kFrontRightXPosInchesRh), - Units.inchesToMeters(kFrontRightYPosInchesRh), kInvertRightSide); - private static final SwerveModuleConstants BackLeftRh = ConstantCreator.createModuleConstants( - CAN.kBackLeftSteerMotorId, - CAN.kBackLeftDriveMotorId, CAN.kBackLeftEncoderId, kBackLeftEncoderOffsetRh, - Units.inchesToMeters(kBackLeftXPosInchesRh), - Units.inchesToMeters(kBackLeftYPosInchesRh), kInvertLeftSide); - private static final SwerveModuleConstants BackRightRh = ConstantCreator.createModuleConstants( - CAN.kBackRightSteerMotorId, - CAN.kBackRightDriveMotorId, CAN.kBackRightEncoderId, - kBackRightEncoderOffsetRh, Units.inchesToMeters(kBackRightXPosInchesRh), - Units.inchesToMeters(kBackRightYPosInchesRh), kInvertRightSide); + private static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants(CAN.kBackRightSteerMotorId, + CAN.kBackRightDriveMotorId, CAN.kBackRightEncoderId, + kBackRightEncoderOffset, Units.inchesToMeters(kBackRightXPosInches), + Units.inchesToMeters(kBackRightYPosInches), kInvertRightSide); + + private static final SwerveModuleConstants FrontLeftRh = + ConstantCreator.createModuleConstants(CAN.kFrontLeftSteerMotorId, + CAN.kFrontLeftDriveMotorId, CAN.kFrontLeftEncoderId, + kFrontLeftEncoderOffsetRh, Units.inchesToMeters(kFrontLeftXPosInchesRh), + Units.inchesToMeters(kFrontLeftYPosInchesRh), kInvertLeftSide); + private static final SwerveModuleConstants FrontRightRh = + ConstantCreator.createModuleConstants(CAN.kFrontRightSteerMotorId, + CAN.kFrontRightDriveMotorId, CAN.kFrontRightEncoderId, + kFrontRightEncoderOffsetRh, Units.inchesToMeters(kFrontRightXPosInchesRh), + Units.inchesToMeters(kFrontRightYPosInchesRh), kInvertRightSide); + private static final SwerveModuleConstants BackLeftRh = + ConstantCreator.createModuleConstants(CAN.kBackLeftSteerMotorId, + CAN.kBackLeftDriveMotorId, CAN.kBackLeftEncoderId, kBackLeftEncoderOffsetRh, + Units.inchesToMeters(kBackLeftXPosInchesRh), + Units.inchesToMeters(kBackLeftYPosInchesRh), kInvertLeftSide); + private static final SwerveModuleConstants BackRightRh = + ConstantCreator.createModuleConstants(CAN.kBackRightSteerMotorId, + CAN.kBackRightDriveMotorId, CAN.kBackRightEncoderId, + kBackRightEncoderOffsetRh, Units.inchesToMeters(kBackRightXPosInchesRh), + Units.inchesToMeters(kBackRightYPosInchesRh), kInvertRightSide); public static final Swerve getDrivetrain(Limelights limelights) { if (Constants.isMercury()) { @@ -317,10 +321,10 @@ public class VisionConstants { // This is a magic number from gridlock, may need to be changed or removed // entirely public static final double PROCESS_LATENCY = 0.0472; // TODO test - public static final Translation2d FIELD_LIMIT = new Translation2d(Units.feetToMeters(54.0), - Units.feetToMeters(26.0)); - public static final Translation2d VISION_LIMIT = new Translation2d(Units.feetToMeters(9), - Units.feetToMeters(5)); + public static final Translation2d FIELD_LIMIT = + new Translation2d(Units.feetToMeters(54.0), Units.feetToMeters(26.0)); + public static final Translation2d VISION_LIMIT = + new Translation2d(Units.feetToMeters(9), Units.feetToMeters(5)); public static final double ALIGNMENT_TOLERANCE = 8d; // TODO: make this an actual value public static final PIDController TAG_AIM_CONTROLLER = new PIDController(0.1, 0, 0); public static final PIDController CHASE_CONTROLLER = new PIDController(0.05, 0, 0); @@ -341,11 +345,14 @@ public class CollisionConstants { public static final double TIP_DEADZONE = 2d; public static final double ACCELERATION_DUE_TO_GRAVITY = 9.80665; - public static final double ACCELERATION_TOLERANCE_TELEOP = 3; //percent of pigeonAcceleration + public static final double ACCELERATION_TOLERANCE_TELEOP = 3; // percent of + // pigeonAcceleration public static final double MIN_ACCELERATION_DIFF_TELEOP = 0.25; // TODO: get real - public static final double ACCELERATION_TOLERANCE_AUTON = 2.00; //percent of pigeonAcceleration + public static final double ACCELERATION_TOLERANCE_AUTON = 2.00; // percent of + // pigeonAcceleration public static final double MIN_ACCELERATION_DIFF_AUTON = 0.25; // TODO: get real - public static final double ACCELERATION_TOLERANCE_SHOOTER = 1.00; //percent of pigeonAcceleration + public static final double ACCELERATION_TOLERANCE_SHOOTER = 1.00; // percent of + // pigeonAcceleration public static final double MIN_ACCELERATION_DIFF_SHOOTER = 0.25; // TODO: get real public enum CollisionType { @@ -356,26 +363,30 @@ public enum CollisionType { public class MusicConstants { public static final String BOH_RHAP_FILEPATH = "bohemianrhapsody.chrp"; public static final String JEOPARDY_FILEPATH = "jeopardy.chrp"; - public static final String BEWARE_THE_FOREST_MUSHROOMS_FILEPATH = "bewaretheforestmushrooms.chrp"; + public static final String BEWARE_THE_FOREST_MUSHROOMS_FILEPATH = + "bewaretheforestmushrooms.chrp"; public static final String UNDER_PRESSURE_FILEPATH = "underpressure.chrp"; public static final String NATIONAL_PARK_FILEPATH = "nationalpark.chrp"; public static final String ENCOUNTER_FILEPATH = "encounter.chrp"; public static final String PIRATES_OF_THE_CARIBBEAN_FILEPATH = "piratesofthecaribbean.chrp"; - public static final String CRAZY_LITTLE_THING_CALLED_LOVE_FILEPATH = "crazylittlethingcalledlove.chrp"; - public static final String ANOTHER_ONE_BITES_THE_DUST_FILEPATH = "anotheronebitesthedust.chrp"; + public static final String CRAZY_LITTLE_THING_CALLED_LOVE_FILEPATH = + "crazylittlethingcalledlove.chrp"; + public static final String ANOTHER_ONE_BITES_THE_DUST_FILEPATH = + "anotheronebitesthedust.chrp"; public static final String SWEET_CAROLINE_FILEPATH = "sweetcaroline.chrp"; public static final String WE_ARE_THE_CHAMPIONS_FILEPATH = "wearethechampions.chrp"; - public static final String[] SONG_NAMES = { BOH_RHAP_FILEPATH, JEOPARDY_FILEPATH, + public static final String[] SONG_NAMES = {BOH_RHAP_FILEPATH, JEOPARDY_FILEPATH, BEWARE_THE_FOREST_MUSHROOMS_FILEPATH, UNDER_PRESSURE_FILEPATH, NATIONAL_PARK_FILEPATH, ENCOUNTER_FILEPATH, PIRATES_OF_THE_CARIBBEAN_FILEPATH, CRAZY_LITTLE_THING_CALLED_LOVE_FILEPATH, ANOTHER_ONE_BITES_THE_DUST_FILEPATH, - SWEET_CAROLINE_FILEPATH, WE_ARE_THE_CHAMPIONS_FILEPATH }; + SWEET_CAROLINE_FILEPATH, WE_ARE_THE_CHAMPIONS_FILEPATH}; public static final List SET_LIST = Arrays.asList(SONG_NAMES); } public class CollectorConstants { // TODO: get real public static final boolean COLLECTOR_MOTOR_INVERTED = true; - public static final int COLLECTOR_MOTOR_STATOR_CURRENT_LIMIT = 60; // TODO: make sure they are not set to 0 + public static final int COLLECTOR_MOTOR_STATOR_CURRENT_LIMIT = 60; // TODO: make sure they + // are not set to 0 public static final boolean COLLECTOR_MOTOR_BRAKE_MODE = false; public static final double MOTOR_KP = 0; @@ -394,10 +405,10 @@ public class FlywheelConstants { // TODO: get real public static final int MOTOR_STATOR_CURRENT_LIMIT = 40; public static final boolean MOTOR_BRAKE_MODE = false; public static final double MOTOR_KP = 0; - public static final double MOTOR_KI = 0; + public static final double MOTOR_KI = 0.0006; public static final double MOTOR_KD = 0; public static final double MOTOR_KS = 0; - public static final double MOTOR_KV = 0.1115; + public static final double MOTOR_KV = 0.1113; public static final double MOTOR_KA = 0; public static final double RPM_TOLERANCE = 50d; @@ -440,11 +451,13 @@ public class PivotConstants { // TODO: get real public static final double ANGLE_TOLERANCE = 0.5d; public static final double ENCODER_OFFSET = 0.6118; // In rotations - public static final SensorDirectionValue ENCODER_DIRECTION = SensorDirectionValue.Clockwise_Positive; + public static final SensorDirectionValue ENCODER_DIRECTION = + SensorDirectionValue.Clockwise_Positive; public static final double ENCODER_TO_MECHANISM_RATIO = 1d; public static final double ROTOR_TO_ENCODER_RATIO = 618.75; - public static final double BIAS_INCREMENT = 1d; // Degrees to bias by per button press TODO get amount to bias + public static final double BIAS_INCREMENT = 1d; // Degrees to bias by per button press TODO + // get amount to bias // by public static final double STOW_ANGLE = 30d; @@ -464,12 +477,8 @@ public class ShooterConstants { { // As distance gets smaller angle goes up put(1.08d, 60d); - put(1.97d, 47d); - put(2.89d, 39d); - put(3.796, 37d); - put(4.72, 34d); - put(5.74, 27d); - + put(2.01d, 50d); + put(2.94d, 39d); } }; @@ -478,11 +487,8 @@ public class ShooterConstants { { // As distance get smaller RPM gets smaller put(1.08d, 2000d); - put(1.97d, 2250d); - put(2.89d, 2500d); - put(3.796, 2500d); - put(4.72, 2750d); - put(5.74, 3800d); + put(2.01d, 2250d); + put(2.94d, 2500d); } }; @@ -526,7 +532,8 @@ public class CandConstants { // TODO get real public static final double SOURCE_ANGLE = 45d; // TODO test // TODO find time to shoot - public static final double TIME_TO_SHOOT = 2d; // Time in seconds it takes from indexer start to flywheel exit + public static final double TIME_TO_SHOOT = 2d; // Time in seconds it takes from indexer + // start to flywheel exit } public class ClimbConstants { // TODO: find real values @@ -546,17 +553,26 @@ public class ClimbConstants { // TODO: find real values public static final double WINCH_CIRCUFERENCE = WINCH_DIAMETER_INCHES * Math.PI; public static final double MAX_HEIGHT = 999d; - public static final double LOWER_LENGTH = 22d; // center of pivot-center of pivot length of lower arm in inches - public static final double UPPER_LENGTH = 25d; // center of pivot-center of pivot length of upper arm in inches + public static final double LOWER_LENGTH = 22d; // center of pivot-center of pivot length of + // lower arm in inches + public static final double UPPER_LENGTH = 25d; // center of pivot-center of pivot length of + // upper arm in inches - public static final Pose3d LOWER_OFFSET = new Pose3d(); // NOTE: Poses are in meters despite george washington's + public static final Pose3d LOWER_OFFSET = new Pose3d(); // NOTE: Poses are in meters despite + // george washington's // best efforts - public static final Pose3d UPPER_OFFSET = new Pose3d(); // NOTE 2: these poses should exclude side to side + public static final Pose3d UPPER_OFFSET = new Pose3d(); // NOTE 2: these poses should + // exclude side to side // offset, since it gets set below - public static final Transform3d LEFT_RIGHT_OFFSET = new Transform3d(); // NOTE 3: this is the side to side - // offset of the pivot point of the arms, - // should exclude anything but side to + public static final Transform3d LEFT_RIGHT_OFFSET = new Transform3d(); // NOTE 3: this is + // the side to side + // offset of the + // pivot point of the + // arms, + // should exclude + // anything but side + // to // side values public static final double CLIMB_PID_SETPOINT_EXTENDED = 10; // TODO: find real values public static final double CLIMB_PID_SETPOINT_RETRACTED = 0; @@ -576,15 +592,15 @@ public class LEDsConstants { public static final int SWIRL_SEGMENT_SIZE = 5; - public static final Map STRAND_START = new HashMap(){ + public static final Map STRAND_START = new HashMap() { { - put(-1, 0); - put(1, 0); - put(2, 14); + put(-1, 0); + put(1, 0); + put(2, 14); } }; - public static final Map STRAND_LENGTH = new HashMap(){ + public static final Map STRAND_LENGTH = new HashMap() { { put(-1, LEDsConstants.LED_LENGTH); put(1, 14); @@ -601,20 +617,9 @@ public class LEDsConstants { public static final int PINK_HUE = 355; public enum LED_STATES { - CUSTOMCONTROL(0), - DISABLED(1), - EMERGENCY(2), - START(3), - COLLECTED(4), - SHOT(5), - FINISHED_CLIMB(6), - SHOOTING(7), - COLLECTING(8), - CHASING(9), - CLIMBING(10), - HAS_PIECE(11), - HAS_VISION(12), - DEFAULT(13); + CUSTOMCONTROL(0), DISABLED(1), EMERGENCY(2), START(3), COLLECTED(4), SHOT( + 5), FINISHED_CLIMB(6), SHOOTING(7), COLLECTING(8), CHASING( + 9), CLIMBING(10), HAS_PIECE(11), HAS_VISION(12), DEFAULT(13); private final int priority; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 818ad0c5..54f3e2a8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -41,6 +41,7 @@ import frc.robot.command.shoot.SmartShoot; import frc.robot.command.shoot.SourceCollect; import frc.robot.command.shoot.Stow; +import frc.robot.command.shoot.Tune; import frc.robot.command.shoot.AutonCand.AmpShotAuton; import frc.robot.command.shoot.AutonCand.CandC1; import frc.robot.command.shoot.AutonCand.CandC2; @@ -198,13 +199,13 @@ protected void configureButtonBindings() { /* copilot */ new Trigger(coPilot::getBButton) - .whileTrue(new SmartCollect(() -> 0.50, () -> 0.60, collector, indexer, - pivot)); // TODO: find correct button/trigger + .whileTrue(new SmartCollect(() -> 0.50, () -> 0.60, collector, indexer, pivot)); // TODO: find correct button/trigger // cand shots for the robot new Trigger(coPilot::getAButton).whileTrue(new AmpShot(flywheel, pivot)); new Trigger(coPilot::getXButton).whileTrue(new PointBlankShot(flywheel, pivot)); // new Trigger(coPilot::getYButton).whileTrue(new PodiumShot(flywheel, pivot)); + // new Trigger(coPilot::getXButton).whileTrue(new Tune(flywheel, pivot)); new Trigger(coPilot::getYButton).whileTrue(new SourceCollect(flywheel, pivot)); // new Trigger(coPilot::getBButton).whileTrue(new Climb(climber, // TODO need new button start? Back? diff --git a/src/main/java/frc/robot/command/shoot/SourceCollect.java b/src/main/java/frc/robot/command/shoot/SourceCollect.java index a5543150..7cb065be 100644 --- a/src/main/java/frc/robot/command/shoot/SourceCollect.java +++ b/src/main/java/frc/robot/command/shoot/SourceCollect.java @@ -21,25 +21,25 @@ public SourceCollect(Flywheel flywheel, Pivot pivot) { this.flywheel = flywheel; this.pivot = pivot; - addRequirements(flywheel, pivot); + addRequirements(flywheel); } @Override public void initialize() { flywheel.setAllMotorsRPM(CandConstants.SOURCE_RPM); - pivot.setTargetAngle(CandConstants.SOURCE_ANGLE); + // pivot.setTargetAngle(CandConstants.SOURCE_ANGLE); } @Override public void execute() { flywheel.setAllMotorsRPM(CandConstants.SOURCE_RPM); - pivot.setTargetAngle(CandConstants.SOURCE_ANGLE); + // pivot.setTargetAngle(CandConstants.SOURCE_ANGLE); } @Override public void end(boolean interrupted) { flywheel.coast(true); - pivot.setTargetAngle(PivotConstants.STOW_ANGLE); + // pivot.setTargetAngle(PivotConstants.STOW_ANGLE); } @Override diff --git a/src/main/java/frc/robot/command/shoot/Tune.java b/src/main/java/frc/robot/command/shoot/Tune.java new file mode 100644 index 00000000..1bb3859a --- /dev/null +++ b/src/main/java/frc/robot/command/shoot/Tune.java @@ -0,0 +1,52 @@ +package frc.robot.command.shoot; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.PivotConstants; +import frc.robot.subsystems.Flywheel; +import frc.robot.subsystems.Pivot; +import frc.thunder.shuffleboard.LightningShuffleboard; + +public class Tune extends Command { + private final Flywheel flywheel; + private final Pivot pivot; + + private double flywheelTargetRPM = 0; + private double pivotTargetAngle = PivotConstants.STOW_ANGLE; + + /** + * Creates a new PointBlankShot. + * + * @param flywheel subsystem + * @param pivot subsystem + */ + public Tune(Flywheel flywheel, Pivot pivot) { + this.flywheel = flywheel; + this.pivot = pivot; + + addRequirements(pivot, flywheel); + } + + @Override + public void initialize() {} + + @Override + public void execute() { + + flywheelTargetRPM = LightningShuffleboard.getDouble("TUNE", "Target RPM", flywheelTargetRPM); + pivotTargetAngle = LightningShuffleboard.getDouble("TUNE", "Target Angle", pivotTargetAngle); + + flywheel.setAllMotorsRPM(flywheelTargetRPM); + pivot.setTargetAngle(pivotTargetAngle); + } + + @Override + public void end(boolean interrupted) { + flywheel.coast(true); + pivot.setTargetAngle(PivotConstants.STOW_ANGLE); + } + + @Override + public boolean isFinished() { + return false; + } +} From 0ef450346897f0db990d377f871cc681935e5c07 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Sat, 2 Mar 2024 10:35:50 -0500 Subject: [PATCH 2/2] [#360] new Tune --- src/main/java/frc/robot/Constants.java | 29 +++++++++++++------ src/main/java/frc/robot/RobotContainer.java | 4 +-- .../java/frc/robot/command/shoot/Tune.java | 3 +- .../java/frc/robot/subsystems/Flywheel.java | 19 +++++------- 4 files changed, 32 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8418e523..8c33afb8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -404,14 +404,25 @@ public class FlywheelConstants { // TODO: get real public static final boolean MOTOR_BOTTOM_INVERT = false; public static final int MOTOR_STATOR_CURRENT_LIMIT = 40; public static final boolean MOTOR_BRAKE_MODE = false; - public static final double MOTOR_KP = 0; - public static final double MOTOR_KI = 0.0006; - public static final double MOTOR_KD = 0; - public static final double MOTOR_KS = 0; - public static final double MOTOR_KV = 0.1113; - public static final double MOTOR_KA = 0; - public static final double RPM_TOLERANCE = 50d; + // TUNED TOP + public static final double TOP_MOTOR_KP = 0.15; + public static final double TOP_MOTOR_KI = 0; + public static final double TOP_MOTOR_KD = 0; + public static final double TOP_MOTOR_KS = 0.3; + public static final double TOP_MOTOR_KV = 0.11; + public static final double TOP_MOTOR_KA = 0; + + // TUNED BOTTOM + public static final double BOTTOM_MOTOR_KP = 0.15; + public static final double BOTTOM_MOTOR_KI = 0; + public static final double BOTTOM_MOTOR_KD = 0; + public static final double BOTTOM_MOTOR_KS = 0.3; + public static final double BOTTOM_MOTOR_KV = 0.115; + public static final double BOTTOM_MOTOR_KA = 0; + + + public static final double RPM_TOLERANCE = 100d; public static final double BIAS_INCREMENT = 1.25; // RPS to bias by per button press public static final double COAST_VOLTAGE = 0.1; @@ -457,8 +468,7 @@ public class PivotConstants { // TODO: get real public static final double ROTOR_TO_ENCODER_RATIO = 618.75; public static final double BIAS_INCREMENT = 1d; // Degrees to bias by per button press TODO - // get amount to bias - // by + // get amount to bias by public static final double STOW_ANGLE = 30d; @@ -489,6 +499,7 @@ public class ShooterConstants { put(1.08d, 2000d); put(2.01d, 2250d); put(2.94d, 2500d); + } }; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 54f3e2a8..6b030587 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -203,9 +203,9 @@ protected void configureButtonBindings() { // cand shots for the robot new Trigger(coPilot::getAButton).whileTrue(new AmpShot(flywheel, pivot)); - new Trigger(coPilot::getXButton).whileTrue(new PointBlankShot(flywheel, pivot)); + // new Trigger(coPilot::getXButton).whileTrue(new PointBlankShot(flywheel, pivot)); // new Trigger(coPilot::getYButton).whileTrue(new PodiumShot(flywheel, pivot)); - // new Trigger(coPilot::getXButton).whileTrue(new Tune(flywheel, pivot)); + new Trigger(coPilot::getXButton).whileTrue(new Tune(flywheel, pivot)); new Trigger(coPilot::getYButton).whileTrue(new SourceCollect(flywheel, pivot)); // new Trigger(coPilot::getBButton).whileTrue(new Climb(climber, // TODO need new button start? Back? diff --git a/src/main/java/frc/robot/command/shoot/Tune.java b/src/main/java/frc/robot/command/shoot/Tune.java index 1bb3859a..b90a5fe1 100644 --- a/src/main/java/frc/robot/command/shoot/Tune.java +++ b/src/main/java/frc/robot/command/shoot/Tune.java @@ -31,9 +31,10 @@ public void initialize() {} @Override public void execute() { - flywheelTargetRPM = LightningShuffleboard.getDouble("TUNE", "Target RPM", flywheelTargetRPM); pivotTargetAngle = LightningShuffleboard.getDouble("TUNE", "Target Angle", pivotTargetAngle); + LightningShuffleboard.setBool("TUNE", "Flywheel On target", flywheel.allMotorsOnTarget()); + LightningShuffleboard.setBool("TUNE", "Pivot On target", pivot.onTarget()); flywheel.setAllMotorsRPM(flywheelTargetRPM); pivot.setTargetAngle(pivotTargetAngle); diff --git a/src/main/java/frc/robot/subsystems/Flywheel.java b/src/main/java/frc/robot/subsystems/Flywheel.java index e7911d39..d39ba0ae 100644 --- a/src/main/java/frc/robot/subsystems/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/Flywheel.java @@ -11,10 +11,8 @@ public class Flywheel extends SubsystemBase { private ThunderBird topMotor; private ThunderBird bottomMotor; - private final VelocityVoltage topRPMPID = new VelocityVoltage(0, 0, false, FlywheelConstants.MOTOR_KV, - 0, false,false, false); - private final VelocityVoltage bottomRPMPID = new VelocityVoltage(0, 0, false, FlywheelConstants.MOTOR_KV, - 0, false,false, false); + private final VelocityVoltage topRPMPID = new VelocityVoltage(0); + private final VelocityVoltage bottomRPMPID = new VelocityVoltage(0); private double topTargetRPS = 0; private double bottomTargetRPS = 0; @@ -29,10 +27,10 @@ public Flywheel() { bottomMotor = new ThunderBird(CAN.FLYWHEEL_MOTOR_BOTTOM, CAN.CANBUS_FD, FlywheelConstants.MOTOR_BOTTOM_INVERT, FlywheelConstants.MOTOR_STATOR_CURRENT_LIMIT, FlywheelConstants.MOTOR_BRAKE_MODE); - topMotor.configPIDF(0, FlywheelConstants.MOTOR_KP, FlywheelConstants.MOTOR_KI, - FlywheelConstants.MOTOR_KD, FlywheelConstants.MOTOR_KS, FlywheelConstants.MOTOR_KV); - bottomMotor.configPIDF(0, FlywheelConstants.MOTOR_KP, FlywheelConstants.MOTOR_KI, - FlywheelConstants.MOTOR_KD, FlywheelConstants.MOTOR_KS, FlywheelConstants.MOTOR_KV); + topMotor.configPIDF(0, FlywheelConstants.TOP_MOTOR_KP, FlywheelConstants.TOP_MOTOR_KI, + FlywheelConstants.TOP_MOTOR_KD, FlywheelConstants.TOP_MOTOR_KS, FlywheelConstants.TOP_MOTOR_KV); + bottomMotor.configPIDF(0, FlywheelConstants.BOTTOM_MOTOR_KP, FlywheelConstants.BOTTOM_MOTOR_KI, + FlywheelConstants.BOTTOM_MOTOR_KD, FlywheelConstants.BOTTOM_MOTOR_KS, FlywheelConstants.BOTTOM_MOTOR_KV); topMotor.applyConfig(); bottomMotor.applyConfig(); @@ -51,8 +49,7 @@ private void initLogging() { () -> bottomMotorRPMOnTarget()); LightningShuffleboard.setDoubleSupplier("Flywheel", "Top Power", () -> topMotor.get()); - LightningShuffleboard.setDoubleSupplier("Flywheel", "Bottom Power", - () -> bottomMotor.get()); + LightningShuffleboard.setDoubleSupplier("Flywheel", "Bottom Power", () -> bottomMotor.get()); LightningShuffleboard.setDoubleSupplier("Flywheel", "Bias", this::getBias); } @@ -64,7 +61,7 @@ public void periodic() { topMotor.set(0d); } else { topMotor.setControl(topRPMPID.withVelocity((topTargetRPS + bias))); - bottomMotor.setControl(bottomRPMPID.withVelocity((topTargetRPS + bias))); + bottomMotor.setControl(bottomRPMPID.withVelocity((bottomTargetRPS + bias))); } }