From 234a0b54d7688bab1888bf70a51c16833f1cc774 Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Mon, 4 Mar 2024 18:07:30 -0800 Subject: [PATCH 01/31] telemetry additions --- simgui-ds.json | 6 ++ simgui.json | 70 ++++++++++++++++++- .../java/frc/robot/subsystems/Intake.java | 1 + .../robot/subsystems/swerve/Drivetrain.java | 11 ++- src/main/java/frc/robot/util/Telemetry.java | 2 +- 5 files changed, 85 insertions(+), 5 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 23ded49..3fef02d 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -93,5 +93,11 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } ] } diff --git a/simgui.json b/simgui.json index 8214de6..4ff00d6 100644 --- a/simgui.json +++ b/simgui.json @@ -1,15 +1,57 @@ { + "HALProvider": { + "Other Devices": { + "CANCoder (v6)[9]": { + "header": { + "open": true + } + }, + "Talon FX (v6)[18]": { + "header": { + "open": true + } + }, + "Talon FX (v6)[27]/Fwd Limit": { + "header": { + "open": true + } + }, + "Talon FX (v6)[27]/Rev Limit": { + "header": { + "open": true + } + }, + "Talon FX (v6)[28]": { + "header": { + "open": true + } + } + } + }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", "/SmartDashboard/Auton Chooser": "String Chooser", - "/SmartDashboard/Field": "Field2d" + "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/PIDController[3]": "PIDController" }, "windows": { "/FMSInfo": { "window": { "visible": true } + }, + "/SmartDashboard/PIDController[3]": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "TEAM 1072": { + "open": true } } }, @@ -48,5 +90,31 @@ "open": true }, "visible": true + }, + "Plot": { + "Plot <0>": { + "plots": [ + { + "backgroundColor": [ + 0.05999999865889549, + 0.05999999865889549, + 0.05999999865889549, + 0.9399999976158142 + ], + "height": 338, + "series": [ + { + "color": [ + 0.2980392277240753, + 0.44705885648727417, + 0.6901960968971252, + 1.0 + ], + "id": "NT:/TEAM 1072/Shooter/Shooter Index Occupied" + } + ] + } + ] + } } } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index fb97c91..d95441d 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -8,6 +8,7 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotMap; diff --git a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java index 2cfb9fc..4456461 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java @@ -22,11 +22,14 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import edu.wpi.first.units.MutableMeasure; import edu.wpi.first.units.Velocity; import edu.wpi.first.units.Voltage; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -62,9 +65,9 @@ public class Drivetrain extends SubsystemBase { private static PIDController omegaAmpController = new PIDController(RobotMap.Drivetrain.OMEGA_AMP_KP, 0, 0); // Standard deviations of pose estimate (x, y, heading) private static Matrix stateStdDevs = VecBuilder.fill(0.3, 0.3, 0.1); // increase to trust encoder (state) - // measurements less + // measurements less private static Matrix visionStdDevs = VecBuilder.fill(0.2, 0.2, 0.1); // increase to trust vsion - // measurements less + // measurements less private boolean robotCentric; @@ -410,11 +413,13 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { public void periodic() { updatePose(); + SmartDashboard.putData(omegaAmpController); + if (Limelight.hasTargets()) { Pose2d visionBot = Limelight.getBotPose2d(); if (Limelight.isPoseValid(visionBot, getPoseEstimatorPose2d())) { poseEstimator.addVisionMeasurement(visionBot, Limelight.getTimestamp()); } } - } + } } diff --git a/src/main/java/frc/robot/util/Telemetry.java b/src/main/java/frc/robot/util/Telemetry.java index 77c29b2..1e32ba7 100644 --- a/src/main/java/frc/robot/util/Telemetry.java +++ b/src/main/java/frc/robot/util/Telemetry.java @@ -16,7 +16,7 @@ public class Telemetry { // private NetworkTable table; private NetworkTableInstance inst; - private NetworkTable main; + private static NetworkTable main; // private static NetworkTable _realOutputs; private static NetworkTable _swerve; From 07b02fed1ea88be846222ce21fa1f3c0f1902120 Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Tue, 5 Mar 2024 18:05:53 -0800 Subject: [PATCH 02/31] Tune pivot --- src/main/java/frc/robot/OI.java | 10 +++++---- src/main/java/frc/robot/Robot.java | 2 -- src/main/java/frc/robot/RobotMap.java | 22 ++++++++++--------- .../frc/robot/commands/CommandGroups.java | 6 ++--- .../commands/elevator/ElevatorManual.java | 2 +- .../robot/commands/elevator/ZeroElevator.java | 8 +++---- .../frc/robot/commands/intake/ZeroIntake.java | 2 +- .../frc/robot/commands/pivot/ZeroPivot.java | 2 +- .../java/frc/robot/subsystems/Elevator.java | 10 +++------ .../java/frc/robot/subsystems/Intake.java | 4 ++++ src/main/java/frc/robot/subsystems/Pivot.java | 22 ++++++++++++------- src/main/java/frc/robot/util/Telemetry.java | 13 ++++++----- 12 files changed, 57 insertions(+), 46 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index c17b3f2..64f90e0 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -49,12 +49,12 @@ public XboxGamepad getOperator() { } private void initBindings() { - // driver.getLeftBumper().onTrue(CommandGroups.FULL_SHOOT_AMP); + driver.getLeftBumper().onTrue(CommandGroups.FULL_SHOOT_AMP); driver.getRightBumper().onTrue(CommandGroups.FULL_SHOOT_SPEAKER); - driver.getButtonB().whileTrue(new ZeroPivot()); + driver.getButtonA().whileTrue(new PivotToAngle(RobotMap.Pivot.Goal.AMP)); - // driver.getButtonA().whileTrue(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(1))); - driver.getButtonA().whileFalse(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0))); + driver.getButtonB().whileTrue(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0.3))); + driver.getButtonB().whileFalse(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0))); // driver.getButtonA().onTrue(new AlignToStage("left")); driver.getButtonSelect().onTrue(new InstantCommand(() -> { @@ -75,6 +75,8 @@ private void initBindings() { operator.getButtonY().whileTrue(new ElevatorManual()); operator.getButtonX().whileTrue(new ZeroElevator()); + + //TESTING // operator.getUpDPadButton().onTrue(CommandGroups.PRE_ALIGN_CLIMB); // operator.getDownDPadButton().onTrue(CommandGroups.POST_ALIGN_CLIMB); // operator.getRightDPadButton().onTrue(CommandGroups.FULL_SHOOT_TRAP); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6730455..c8a04ee 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -132,8 +132,6 @@ public void teleopInit() { public void teleopPeriodic() { // Intake.getInstance().setRollerPower(RobotMap.Intake.ROLLER_SPEED); // Shooter.getInstance().setShooter(RobotMap.Shooter.SHOOTING_SPEED); - Telemetry.putBoolean("pivot", "Is Stalling", Pivot.getInstance().isStalling()); - if (Pivot.getInstance().isStalling()) System.out.println("pivot stallinggggg"); } @Override diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 4df3c72..d9df8f8 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -191,9 +191,9 @@ public static final class Pivot { public static final int MASTER_ID = 24; public static final int LIMIT_SWITCH_ID = 2; - public static final InvertedValue MASTER_INVERT = InvertedValue.Clockwise_Positive; + public static final InvertedValue MASTER_INVERT = InvertedValue.CounterClockwise_Positive; - public static final double ZERO_SPEED = -0.08; + public static final double ZERO_SPEED = -0.4; public static final double PIVOT_kP = 41.939; public static final double PIVOT_kG = 0; @@ -204,20 +204,20 @@ public static final class Pivot { public static final double TRAP1_ANGLE = 50; public static final double TRAP2_ANGLE = 40; public static final double TRAP_SCORE_ANGLE = 30; - public static final double AMP_ANGLE = 58; + public static final double AMP_ANGLE = 90; - public static final double PIVOT_GEAR_RATIO = 106.0 + 2.0/3.0; + public static final double PIVOT_GEAR_RATIO = 67.76; public static final double PIVOT_ROT_TO_ANGLE = 360; // motor rotations to degrees - public static final double STALLING_CURRENT = 80; + public static final double STALLING_CURRENT = 20; // stalls when current is 50 - public static final double MAX_ERROR = 1; // degrees + public static final double MAX_ERROR = 0.5; // degrees public static final double PIVOT_FORWARD_SOFT_LIMIT = 65; public static final double PIVOT_REVERSE_SOFT_LIMIT = 0; - public static final double MAX_CRUISE_ACCLERATION = 344.53125 / 2; - public static final double MAX_CRUISE_VElOCITY = 344.53125; + public static final double MAX_CRUISE_ACCLERATION = 388.0 / PIVOT_ROT_TO_ANGLE; + public static final double MAX_CRUISE_VElOCITY = 388.0 / PIVOT_ROT_TO_ANGLE; public static enum Goal { AMP, @@ -255,7 +255,7 @@ public static final class Intake { public static final int ROLLER_ID = 7; public static final int LIMIT_SWITCH_ID = 0; - public static final boolean DEPLOY_INVERT = false; + public static final boolean DEPLOY_INVERT = true; public static final boolean ROLLER_INVERT = false; public static final double ZERO_SPEED = -0.5; @@ -265,6 +265,8 @@ public static final class Intake { public static final double DEPLOY_kP = 0.07; public static final double INTAKE_DEPLOY = 23; // rotations + public static final double INTAKE_STALLING_CURRENT = 40; + public static final int ROLLER_CURRENT_LIMIT = 90; } @@ -281,7 +283,7 @@ public static final class Indexer { public static final class Camera { public static final double FORWARD = Units.inchesToMeters(11.823); // TODO - public static final double UP = Units.inchesToMeters(11.823); // meters + public static final double UP = Units.inchesToMeters(21.47); // meters public static final double PITCH = -30.494; // degrees public static final int[] ID_SPEAKER_BLUE = {7, 8}; diff --git a/src/main/java/frc/robot/commands/CommandGroups.java b/src/main/java/frc/robot/commands/CommandGroups.java index d23325d..9053ed1 100644 --- a/src/main/java/frc/robot/commands/CommandGroups.java +++ b/src/main/java/frc/robot/commands/CommandGroups.java @@ -15,11 +15,11 @@ public class CommandGroups { public static final Command FULL_ZERO = new ZeroPivot().alongWith(new ZeroElevator(), new ZeroIntake()); - public static final Command FULL_INTAKE = new MoveNoteToShooter().raceWith(new IndexToShooter().alongWith(new IntakeNote(), new ZeroPivot())); + public static final Command FULL_INTAKE = new MoveNoteToShooter().raceWith(new IndexToShooter().alongWith(new IntakeNote())); - public static final Command FULL_SHOOT_SPEAKER = new PivotToAngle(RobotMap.Pivot.Goal.SPEAKER).alongWith(new RevShooter(RobotMap.Shooter.Goal.SPEAKER)).andThen(new ShootNote()); + public static final Command FULL_SHOOT_SPEAKER = new PivotToAngle(RobotMap.Pivot.Goal.SPEAKER).alongWith(new RevShooter(RobotMap.Shooter.Goal.SPEAKER)).andThen(new ShootNote()).andThen(new ZeroPivot()); - public static final Command FULL_SHOOT_AMP = new PivotToAngle(RobotMap.Pivot.Goal.AMP).alongWith(new RevShooter(RobotMap.Shooter.Goal.AMP)).andThen(new ShootNote()); + public static final Command FULL_SHOOT_AMP = new PivotToAngle(RobotMap.Pivot.Goal.AMP).alongWith(new RevShooter(RobotMap.Shooter.Goal.AMP)).andThen(new ShootNote()).andThen(new ZeroPivot()); public static final Command FULL_SHOOT_NO_ALIGN = new RevShooter(RobotMap.Shooter.Goal.AMP).andThen(new ShootNote()); diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorManual.java b/src/main/java/frc/robot/commands/elevator/ElevatorManual.java index 7e72ada..37ee7b1 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorManual.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorManual.java @@ -11,7 +11,7 @@ public ElevatorManual() { @Override public void execute() { - Elevator.getInstance().setElevatorPower(0.6); + Elevator.getInstance().setElevatorPower(0.4); } public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/elevator/ZeroElevator.java b/src/main/java/frc/robot/commands/elevator/ZeroElevator.java index b2fc573..31dd887 100644 --- a/src/main/java/frc/robot/commands/elevator/ZeroElevator.java +++ b/src/main/java/frc/robot/commands/elevator/ZeroElevator.java @@ -17,9 +17,9 @@ public boolean isFinished() { return Elevator.getInstance().isLimitHit(); } - // public void end(boolean interrupted) { - // Elevator.getInstance().setElevatorPower(0); - // Elevator.getInstance().setSensorPosition(0); - // } + public void end(boolean interrupted) { + Elevator.getInstance().setElevatorPower(0); + Elevator.getInstance().setSensorPosition(0); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/intake/ZeroIntake.java b/src/main/java/frc/robot/commands/intake/ZeroIntake.java index 7e01911..7e29a50 100644 --- a/src/main/java/frc/robot/commands/intake/ZeroIntake.java +++ b/src/main/java/frc/robot/commands/intake/ZeroIntake.java @@ -14,7 +14,7 @@ public void execute() { } public boolean isFinished() { - return Intake.getInstance().limitSwitchHit(); + return Intake.getInstance().isStalling(); } public void end(boolean interrupted) { diff --git a/src/main/java/frc/robot/commands/pivot/ZeroPivot.java b/src/main/java/frc/robot/commands/pivot/ZeroPivot.java index 8d40857..bab5c87 100644 --- a/src/main/java/frc/robot/commands/pivot/ZeroPivot.java +++ b/src/main/java/frc/robot/commands/pivot/ZeroPivot.java @@ -14,7 +14,7 @@ public void execute() { } public boolean isFinished() { - return Pivot.getInstance().isLimitHit() || Pivot.getInstance().isStalling(); + return Pivot.getInstance().isStalling(); } public void end(boolean interrupted) { diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index ceab68c..8f37fa9 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -42,11 +42,6 @@ private void configMotors() { masterConfig.Voltage.PeakForwardVoltage = RobotMap.MAX_VOLTAGE; masterConfig.Voltage.PeakReverseVoltage = -RobotMap.MAX_VOLTAGE; - masterConfig.CurrentLimits.StatorCurrentLimitEnable = true; - masterConfig.CurrentLimits.StatorCurrentLimit = 90; - masterConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - masterConfig.CurrentLimits.SupplyCurrentLimit = 90; - masterConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = RobotMap.Elevator.ELEVATOR_FORWARD_SOFT_LIMIT; masterConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = RobotMap.Elevator.ELEVATOR_REVERSE_SOFT_LIMIT; masterConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; @@ -55,10 +50,11 @@ private void configMotors() { masterConfig.Slot0.kP = RobotMap.Elevator.ELEVATOR_kP; masterConfig.Slot0.kG = RobotMap.Elevator.ELEVATOR_kG; - follower.setControl(new Follower(RobotMap.Elevator.MASTER_ID, false)); - master.getConfigurator().apply(masterConfig); follower.getConfigurator().apply(followerConfig); + + follower.setControl(new Follower(RobotMap.Elevator.MASTER_ID, false)); + } public double getPosition() { diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index d95441d..0579ada 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -71,6 +71,10 @@ public void setDeployPower(double power) { public void setRollerPower(double power) { roller.setVoltage(power * RobotMap.MAX_VOLTAGE); } + + public boolean isStalling() { + return deploy.getOutputCurrent() >= RobotMap.Intake.INTAKE_STALLING_CURRENT; + } public static Intake getInstance() { if (instance == null) instance = new Intake(); diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index d9d69b7..b2c42e5 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -27,6 +27,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.RobotMap; +import frc.robot.util.MathUtil; import frc.robot.util.Telemetry; public class Pivot extends SubsystemBase { @@ -68,15 +69,12 @@ private void configMotors() { masterConfig.Feedback.SensorToMechanismRatio = RobotMap.Pivot.PIVOT_GEAR_RATIO; - masterConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = RobotMap.Pivot.PIVOT_FORWARD_SOFT_LIMIT; - masterConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = RobotMap.Pivot.PIVOT_REVERSE_SOFT_LIMIT; - masterConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - masterConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + // masterConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = RobotMap.Pivot.PIVOT_FORWARD_SOFT_LIMIT; + // masterConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = RobotMap.Pivot.PIVOT_REVERSE_SOFT_LIMIT; + // masterConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; + // masterConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; masterConfig.Slot0.kP = RobotMap.Pivot.PIVOT_kP; - // masterConfig.Slot0.kS = RobotMap.Pivot.PIVOT_kS; - // masterConfig.Slot0.kV = RobotMap.Pivot.PIVOT_kV; - // masterConfig.Slot0.kA = RobotMap.Pivot.PIVOT_kA; masterConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; masterConfig.Slot0.kG = RobotMap.Pivot.PIVOT_kG; @@ -97,6 +95,10 @@ public boolean isStalling() { return master.getStatorCurrent().getValue() > RobotMap.Pivot.STALLING_CURRENT; } + public double getMasterCurrent() { + return master.getStatorCurrent().getValue(); + } + /* * Get pivot angle in degrees per second */ @@ -109,11 +111,15 @@ public double getVoltage() { } public void moveToPosition(double desiredAngle) { - MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(desiredAngle); + MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(desiredAngle / RobotMap.Pivot.PIVOT_ROT_TO_ANGLE); master.setControl(motionMagicVoltage); } public void setPercentOutput(double power) { + if (MathUtil.compareDouble(power, 0)) + { + master.stopMotor(); + } DutyCycleOut percentOutput = new DutyCycleOut(power); master.setControl(percentOutput); } diff --git a/src/main/java/frc/robot/util/Telemetry.java b/src/main/java/frc/robot/util/Telemetry.java index 1e32ba7..f90c753 100644 --- a/src/main/java/frc/robot/util/Telemetry.java +++ b/src/main/java/frc/robot/util/Telemetry.java @@ -210,14 +210,11 @@ public void elevator() { } public void intake() { - NetworkTableEntry intakeLimitSwitchHit = _intake.getEntry("Intake Limit Switch Hit"); - intakeLimitSwitchHit.setBoolean(intake.limitSwitchHit()); + NetworkTableEntry intakeStalling = _intake.getEntry("Intake Stalling"); + intakeStalling.setBoolean(intake.isStalling()); } public void pivot() { - NetworkTableEntry pivotLimitSwitchHit = _pivot.getEntry("Pivot Limit Switch Hit"); - pivotLimitSwitchHit.setBoolean(pivot.isLimitHit()); - NetworkTableEntry pivotSensorPosition = _pivot.getEntry("Pivot Sensor Position"); pivotSensorPosition.setDouble(pivot.getPosition()); @@ -227,6 +224,12 @@ public void pivot() { NetworkTableEntry pivotVelocity = _pivot.getEntry("Pivot Velocity"); pivotVelocity.setDouble(pivot.getVelocity()); + NetworkTableEntry pivotStalling = _pivot.getEntry("Pivot Stalling"); + pivotStalling.setBoolean(pivot.isStalling()); + + NetworkTableEntry pivotMasterCurrent = _pivot.getEntry("Pivot Master Motor Current"); + pivotMasterCurrent.setDouble(pivot.getMasterCurrent()); + // NetworkTableEntry setPivotAngle = _pivot.getEntry("Pivot Angle"); // setPivotAngle.setDouble(pivot.getPivotSetpoint(0)) } From 82ce6031d7aa7e4e24a5f7fd3670f8155276d8d3 Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Wed, 6 Mar 2024 18:13:22 -0800 Subject: [PATCH 03/31] Working amp and climb --- src/main/java/frc/robot/OI.java | 6 +- src/main/java/frc/robot/RobotMap.java | 20 ++++--- src/main/java/frc/robot/auton/Autons.java | 57 +++++++++++-------- .../commands/drivetrain/SwerveManual.java | 16 +++++- .../robot/commands/elevator/ElevatorDown.java | 25 ++++++++ .../commands/elevator/ElevatorManual.java | 2 +- src/main/java/frc/robot/subsystems/Pivot.java | 5 ++ .../robot/subsystems/swerve/Drivetrain.java | 4 +- 8 files changed, 93 insertions(+), 42 deletions(-) create mode 100644 src/main/java/frc/robot/commands/elevator/ElevatorDown.java diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 64f90e0..9596c08 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.CommandGroups; import frc.robot.commands.drivetrain.AlignToStage; +import frc.robot.commands.elevator.ElevatorDown; //import frc.robot.commands.CommandGroups; // import frc.robot.commands.drivetrain.AlignToStage; import frc.robot.commands.elevator.ElevatorManual; @@ -49,10 +50,9 @@ public XboxGamepad getOperator() { } private void initBindings() { - driver.getLeftBumper().onTrue(CommandGroups.FULL_SHOOT_AMP); + driver.getButtonY().onTrue(CommandGroups.FULL_SHOOT_AMP); driver.getRightBumper().onTrue(CommandGroups.FULL_SHOOT_SPEAKER); - driver.getButtonA().whileTrue(new PivotToAngle(RobotMap.Pivot.Goal.AMP)); driver.getButtonB().whileTrue(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0.3))); driver.getButtonB().whileFalse(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0))); // driver.getButtonA().onTrue(new AlignToStage("left")); @@ -74,7 +74,7 @@ private void initBindings() { operator.getLeftBumper().whileTrue(new OuttakeNote()); operator.getButtonY().whileTrue(new ElevatorManual()); - operator.getButtonX().whileTrue(new ZeroElevator()); + operator.getButtonX().whileTrue(new ElevatorDown()); //TESTING // operator.getUpDPadButton().onTrue(CommandGroups.PRE_ALIGN_CLIMB); diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index d9df8f8..00e92b5 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -133,8 +133,10 @@ public static final class Drivetrain { public static final double ROBOT_WIDTH = Units.inchesToMeters(28); public static final double MAX_DRIVING_SPEED = 5.0; // m/s //TODO + public static final double EXTENDED_MAX_DRIVING_SPEED = 0.3; public static final double MAX_ACCELERATION = 7; public static final double MAX_ANGLE_VELOCITY = Math.PI; + public static final double EXTENDED_MAX_ANGLE_VELOCITY = Math.PI/10; public static final double MAX_ANGLE_ACCELERATION = MAX_ANGLE_VELOCITY / 2.0; /** @@ -179,7 +181,7 @@ public static final class Shooter { public static final double SHOOTING_SPEED = 0.8; public static final double REVVED_RPS = 1200.0 / 60.0; - public static final double REVVED_AMP_RPS = 500.0 / 60.0; + public static final double REVVED_AMP_RPS = 250.0 / 60.0; public static enum Goal { AMP, @@ -191,11 +193,11 @@ public static final class Pivot { public static final int MASTER_ID = 24; public static final int LIMIT_SWITCH_ID = 2; - public static final InvertedValue MASTER_INVERT = InvertedValue.CounterClockwise_Positive; + public static final InvertedValue MASTER_INVERT = InvertedValue.Clockwise_Positive; - public static final double ZERO_SPEED = -0.4; + public static final double ZERO_SPEED = -0.2; - public static final double PIVOT_kP = 41.939; + public static final double PIVOT_kP = 47; public static final double PIVOT_kG = 0; public static final double PIVOT_kS = 0; //0.10574; public static final double PIVOT_kV = 0; //69.706; @@ -204,20 +206,20 @@ public static final class Pivot { public static final double TRAP1_ANGLE = 50; public static final double TRAP2_ANGLE = 40; public static final double TRAP_SCORE_ANGLE = 30; - public static final double AMP_ANGLE = 90; + public static final double AMP_ANGLE = 100; - public static final double PIVOT_GEAR_RATIO = 67.76; + public static final double PIVOT_GEAR_RATIO = 36; public static final double PIVOT_ROT_TO_ANGLE = 360; // motor rotations to degrees public static final double STALLING_CURRENT = 20; // stalls when current is 50 public static final double MAX_ERROR = 0.5; // degrees - public static final double PIVOT_FORWARD_SOFT_LIMIT = 65; + public static final double PIVOT_FORWARD_SOFT_LIMIT = 65 / 67.76 * 36; public static final double PIVOT_REVERSE_SOFT_LIMIT = 0; - public static final double MAX_CRUISE_ACCLERATION = 388.0 / PIVOT_ROT_TO_ANGLE; - public static final double MAX_CRUISE_VElOCITY = 388.0 / PIVOT_ROT_TO_ANGLE; + public static final double MAX_CRUISE_ACCLERATION = 388.0 / PIVOT_ROT_TO_ANGLE / 67.76 * 36; + public static final double MAX_CRUISE_VElOCITY = 388.0 / PIVOT_ROT_TO_ANGLE / 67.76 * 36; public static enum Goal { AMP, diff --git a/src/main/java/frc/robot/auton/Autons.java b/src/main/java/frc/robot/auton/Autons.java index 822b3e2..5674eda 100644 --- a/src/main/java/frc/robot/auton/Autons.java +++ b/src/main/java/frc/robot/auton/Autons.java @@ -1,6 +1,8 @@ package frc.robot.auton; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.CommandGroups; // import frc.robot.commands.CommandGroups; // import frc.robot.commands.indexer.IndexToShooter; // import frc.robot.commands.shooter.MoveNoteToShooter; @@ -11,36 +13,43 @@ public class Autons { public static final SequentialCommandGroup fourNotePath = new SequentialCommandGroup( - new SwervePositionController(Trajectories.startToNote1_four, () -> Rotation2d.fromDegrees(180), false), - new SwervePositionController(Trajectories.note1ToShoot1_four, () -> Rotation2d.fromDegrees(180), true), - new SwervePositionController(Trajectories.shoot1ToNote2_four, () -> Rotation2d.fromDegrees(180), false), - new SwervePositionController(Trajectories.note2ToShoot2_four, () -> Rotation2d.fromDegrees(180), true), - new SwervePositionController(Trajectories.shoot2ToNote3_four, () -> Rotation2d.fromDegrees(-157.83), false), - new SwervePositionController(Trajectories.note3ToShoot3_four, () -> Rotation2d.fromDegrees(-136.16), true) + // new SwervePositionController(Trajectories.startToNote1_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.FULL_SHOOT_SPEAKER) + // .andThen(CommandGroups.FULL_INTAKE) + // .andThen(new SwervePositionController(Trajectories.note1ToShoot1_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.FULL_SHOOT_SPEAKER) + // .andThen(new SwervePositionController(Trajectories.shoot1ToNote2_four, () -> Rotation2d.fromDegrees(180), false)) + // .andThen(CommandGroups.FULL_INTAKE) + // .andThen(new SwervePositionController(Trajectories.note2ToShoot2_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.FULL_SHOOT_SPEAKER)) + // .andThen(new SwervePositionController(Trajectories.shoot2ToNote3_four, () -> Rotation2d.fromDegrees(-157.83), false)) + // .andThen(CommandGroups.FULL_INTAKE) + // .andThen(new SwervePositionController(Trajectories.note3ToShoot3_four, () -> Rotation2d.fromDegrees(-136.16), true).alongWith(CommandGroups.FULL_SHOOT_SPEAKER)) ); public static final SequentialCommandGroup threeNotePath = new SequentialCommandGroup( - new SwervePositionController(Trajectories.startToShoot1_three, () -> Rotation2d.fromDegrees(180), true), - new SwervePositionController(Trajectories.shoot1ToMiddle1_three, () -> Rotation2d.fromDegrees(140.1), false), - new SwervePositionController(Trajectories.middleToNote1_three, () -> Rotation2d.fromDegrees(180), false), - new SwervePositionController(Trajectories.noteToMiddle2_three, () -> Rotation2d.fromDegrees(180), false), - new SwervePositionController(Trajectories.middle2ToShoot2_three, () -> Rotation2d.fromDegrees(180), true), - new SwervePositionController(Trajectories.shoot2ToNote2_three, () -> Rotation2d.fromDegrees(138.59), false), - new SwervePositionController(Trajectories.note2ToShoot3_three, () -> Rotation2d.fromDegrees(180), true) + + // new SwervePositionController(Trajectories.startToShoot1_three, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.FULL_SHOOT_SPEAKER), + // CommandGroups.FULL_INTAKE, + // new SwervePositionController(Trajectories.shoot1ToMiddle1_three, () -> Rotation2d.fromDegrees(140.1), false), + // new SwervePositionController(Trajectories.middleToNote1_three, () -> Rotation2d.fromDegrees(180), false), + // CommandGroups.FULL_INTAKE, + // new SwervePositionController(Trajectories.noteToMiddle2_three, () -> Rotation2d.fromDegrees(180), false), + // new SwervePositionController(Trajectories.middle2ToShoot2_three, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.FULL_SHOOT_SPEAKER), + // new SwervePositionController(Trajectories.shoot2ToNote2_three, () -> Rotation2d.fromDegrees(138.59), false), + // CommandGroups.FULL_INTAKE, + // new SwervePositionController(Trajectories.note2ToShoot3_three, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.FULL_SHOOT_SPEAKER) ); public static final SequentialCommandGroup sixNotePath = new SequentialCommandGroup ( - new SwervePositionController(Trajectories.startToNote1_six, () -> Rotation2d.fromDegrees(180), false), - new SwervePositionController(Trajectories.note1ToShoot1_six, () -> Rotation2d.fromDegrees(180), true), - new SwervePositionController(Trajectories.shoot1ToNote2_six, () -> Rotation2d.fromDegrees(158.62), false), - new SwervePositionController(Trajectories.note2ToShoot2_six, () -> Rotation2d.fromDegrees(178.45), true), - new SwervePositionController(Trajectories.note5ToShooter_six, () -> Rotation2d.fromDegrees(-160.76), true), - new SwervePositionController(Trajectories.shoot2ToNote3_six, () -> Rotation2d.fromDegrees(180), false), - new SwervePositionController(Trajectories.note3ToShoot3_six, () -> Rotation2d.fromDegrees(-156.72), true), - new SwervePositionController(Trajectories.shoot3ToNote4_six, () -> Rotation2d.fromDegrees(180), false), - new SwervePositionController(Trajectories.note4ToShoot4_six, () -> Rotation2d.fromDegrees(-164.18), true), - new SwervePositionController(Trajectories.shoot4ToNote5_six, () -> Rotation2d.fromDegrees(180), false), - new SwervePositionController(Trajectories.note5ToShoot5_six, () -> Rotation2d.fromDegrees(-162.5), true) + // new SwervePositionController(Trajectories.startToNote1_six, () -> Rotation2d.fromDegrees(180), false), + // new SwervePositionController(Trajectories.note1ToShoot1_six, () -> Rotation2d.fromDegrees(180), true), + // new SwervePositionController(Trajectories.shoot1ToNote2_six, () -> Rotation2d.fromDegrees(158.62), false), + // new SwervePositionController(Trajectories.note2ToShoot2_six, () -> Rotation2d.fromDegrees(178.45), true), + // new SwervePositionController(Trajectories.note5ToShooter_six, () -> Rotation2d.fromDegrees(-160.76), true), + // new SwervePositionController(Trajectories.shoot2ToNote3_six, () -> Rotation2d.fromDegrees(180), false), + // new SwervePositionController(Trajectories.note3ToShoot3_six, () -> Rotation2d.fromDegrees(-156.72), true), + // new SwervePositionController(Trajectories.shoot3ToNote4_six, () -> Rotation2d.fromDegrees(180), false), + // new SwervePositionController(Trajectories.note4ToShoot4_six, () -> Rotation2d.fromDegrees(-164.18), true), + // new SwervePositionController(Trajectories.shoot4ToNote5_six, () -> Rotation2d.fromDegrees(180), false), + // new SwervePositionController(Trajectories.note5ToShoot5_six, () -> Rotation2d.fromDegrees(-162.5), true) ); } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java b/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java index 0917ea4..8e7a386 100644 --- a/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java +++ b/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotMap; +import frc.robot.subsystems.Pivot; import frc.robot.subsystems.swerve.Drivetrain; import frc.robot.OI; import frc.robot.util.Flip; @@ -48,10 +49,19 @@ public void execute() { vy *= -1; } // Scaling velocities based on multipliers - vx = scaleValues(vx, RobotMap.Drivetrain.MAX_DRIVING_SPEED); //*(RobotMap.SwerveManual.SPEED_MULTIPLIER); - vy = scaleValues(vy, RobotMap.Drivetrain.MAX_DRIVING_SPEED) ;//* (RobotMap.SwerveManual.SPEED_MULTIPLIER); - omega = scaleValues(omega, RobotMap.Drivetrain.MAX_ANGLE_VELOCITY); //* ( RobotMap.SwerveManual.SPEED_MULTIPLIER); + if (Pivot.getInstance().isExtended()) + { + vx = scaleValues(vx, RobotMap.Drivetrain.EXTENDED_MAX_DRIVING_SPEED); //*(RobotMap.SwerveManual.SPEED_MULTIPLIER); + vy = scaleValues(vy, RobotMap.Drivetrain.EXTENDED_MAX_DRIVING_SPEED) ;//* (RobotMap.SwerveManual.SPEED_MULTIPLIER); + omega = scaleValues(omega, RobotMap.Drivetrain.EXTENDED_MAX_ANGLE_VELOCITY); //* ( RobotMap.SwerveManual.SPEED_MULTIPLIER); + } + else + { + vx = scaleValues(vx, RobotMap.Drivetrain.MAX_DRIVING_SPEED); //*(RobotMap.SwerveManual.SPEED_MULTIPLIER); + vy = scaleValues(vy, RobotMap.Drivetrain.MAX_DRIVING_SPEED) ;//* (RobotMap.SwerveManual.SPEED_MULTIPLIER); + omega = scaleValues(omega, RobotMap.Drivetrain.MAX_ANGLE_VELOCITY); //* ( RobotMap.SwerveManual.SPEED_MULTIPLIER); + } omega = Drivetrain.getInstance().adjustPigeon(omega); // limits acceleration diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorDown.java b/src/main/java/frc/robot/commands/elevator/ElevatorDown.java new file mode 100644 index 0000000..8168a5d --- /dev/null +++ b/src/main/java/frc/robot/commands/elevator/ElevatorDown.java @@ -0,0 +1,25 @@ +package frc.robot.commands.elevator; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotMap; +import frc.robot.subsystems.Elevator; + +public class ElevatorDown extends Command{ + public ElevatorDown() { + addRequirements(Elevator.getInstance()); + } + + @Override + public void execute() { + Elevator.getInstance().setElevatorPower(-0.3); + } + + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + Elevator.getInstance().setElevatorPower(0); + } +} diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorManual.java b/src/main/java/frc/robot/commands/elevator/ElevatorManual.java index 37ee7b1..22f3365 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorManual.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorManual.java @@ -11,7 +11,7 @@ public ElevatorManual() { @Override public void execute() { - Elevator.getInstance().setElevatorPower(0.4); + Elevator.getInstance().setElevatorPower(0.3); } public boolean isFinished() { diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index b2c42e5..1be526f 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -136,6 +136,11 @@ public double getPivotSetpoint(double distance) { return speakerAngles.get(distance); } + public boolean isExtended() + { + return getPosition() > 90.0; + } + private final SysIdRoutine _sysId = new SysIdRoutine( new SysIdRoutine.Config(), new SysIdRoutine.Mechanism( diff --git a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java index 4456461..b6f054d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java @@ -64,9 +64,9 @@ public class Drivetrain extends SubsystemBase { private static PIDController vxAmpController = new PIDController(RobotMap.Drivetrain.VX_AMP_kP, 0, 0); private static PIDController omegaAmpController = new PIDController(RobotMap.Drivetrain.OMEGA_AMP_KP, 0, 0); // Standard deviations of pose estimate (x, y, heading) - private static Matrix stateStdDevs = VecBuilder.fill(0.3, 0.3, 0.1); // increase to trust encoder (state) + private static Matrix stateStdDevs = VecBuilder.fill(0.2, 0.2, 0.1); // increase to trust encoder (state) // measurements less - private static Matrix visionStdDevs = VecBuilder.fill(0.2, 0.2, 0.1); // increase to trust vsion + private static Matrix visionStdDevs = VecBuilder.fill(0.5, 0.5, 0.1); // increase to trust vsion // measurements less private boolean robotCentric; From 91336f1831bfcd06b5b3e70063031c275408eb68 Mon Sep 17 00:00:00 2001 From: 25ChilingH <25ChilingH@students.harker.org> Date: Wed, 6 Mar 2024 22:20:24 -0800 Subject: [PATCH 04/31] Optimize climb --- src/main/java/frc/robot/OI.java | 5 ++-- src/main/java/frc/robot/RobotMap.java | 8 +++--- .../robot/commands/elevator/ElevatorDown.java | 25 ------------------- .../commands/elevator/ElevatorManual.java | 14 ++++++++--- .../robot/commands/elevator/ZeroElevator.java | 2 +- .../java/frc/robot/subsystems/Elevator.java | 8 ++++-- 6 files changed, 24 insertions(+), 38 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/elevator/ElevatorDown.java diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 9596c08..4f12082 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.CommandGroups; import frc.robot.commands.drivetrain.AlignToStage; -import frc.robot.commands.elevator.ElevatorDown; //import frc.robot.commands.CommandGroups; // import frc.robot.commands.drivetrain.AlignToStage; import frc.robot.commands.elevator.ElevatorManual; @@ -73,8 +72,8 @@ private void initBindings() { operator.getRightBumper().onTrue(CommandGroups.FULL_INTAKE); operator.getLeftBumper().whileTrue(new OuttakeNote()); - operator.getButtonY().whileTrue(new ElevatorManual()); - operator.getButtonX().whileTrue(new ElevatorDown()); + operator.getButtonY().whileTrue(new ElevatorManual(RobotMap.Elevator.EXTEND_SPEED)); + operator.getButtonA().whileTrue(new ElevatorManual(-RobotMap.Elevator.EXTEND_SPEED)); //TESTING // operator.getUpDPadButton().onTrue(CommandGroups.PRE_ALIGN_CLIMB); diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 00e92b5..2c36f05 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -211,7 +211,7 @@ public static final class Pivot { public static final double PIVOT_GEAR_RATIO = 36; public static final double PIVOT_ROT_TO_ANGLE = 360; // motor rotations to degrees - public static final double STALLING_CURRENT = 20; // stalls when current is 50 + public static final double STALLING_CURRENT = 40; // stalls when current is 50 public static final double MAX_ERROR = 0.5; // degrees @@ -237,7 +237,7 @@ public static final class Elevator { public static final int LIMIT_SWITCH_ID = 1; public static final double ELEVATOR_kP = 1; - public static final double ELEVATOR_kG = 0; + public static final double ELEVATOR_kG = 0.05; public static final double TRAP_HEIGHT = 0; // motor rotations public static final double STAGE_HEIGHT = 20; @@ -245,9 +245,11 @@ public static final class Elevator { public static final double ELEVATOR_FORWARD_SOFT_LIMIT = 0; public static final double ELEVATOR_REVERSE_SOFT_LIMIT = 0; + public static final double ELEVATOR_STALLING_CURRENT = 50; + public static final InvertedValue MASTER_INVERT = InvertedValue.Clockwise_Positive; - public static final double ZERO_SPEED = -0.3; + public static final double EXTEND_SPEED = 0.3; public static final double MAX_ERROR = 1; // motor rotations } diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorDown.java b/src/main/java/frc/robot/commands/elevator/ElevatorDown.java deleted file mode 100644 index 8168a5d..0000000 --- a/src/main/java/frc/robot/commands/elevator/ElevatorDown.java +++ /dev/null @@ -1,25 +0,0 @@ -package frc.robot.commands.elevator; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.RobotMap; -import frc.robot.subsystems.Elevator; - -public class ElevatorDown extends Command{ - public ElevatorDown() { - addRequirements(Elevator.getInstance()); - } - - @Override - public void execute() { - Elevator.getInstance().setElevatorPower(-0.3); - } - - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean interrupted) { - Elevator.getInstance().setElevatorPower(0); - } -} diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorManual.java b/src/main/java/frc/robot/commands/elevator/ElevatorManual.java index 22f3365..4db6b12 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorManual.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorManual.java @@ -5,21 +5,27 @@ import frc.robot.subsystems.Elevator; public class ElevatorManual extends Command{ - public ElevatorManual() { + private double power; + + public ElevatorManual(double power) { + this.power = power; addRequirements(Elevator.getInstance()); } @Override public void execute() { - Elevator.getInstance().setElevatorPower(0.3); + Elevator.getInstance().setElevatorPower(power); } public boolean isFinished() { - return false; + return Elevator.getInstance().isStalling(); } @Override public void end(boolean interrupted) { - Elevator.getInstance().setElevatorPower(0); + if (power > 0) + Elevator.getInstance().setElevatorPower(RobotMap.Elevator.ELEVATOR_kG); + else + Elevator.getInstance().setElevatorPower(0); } } diff --git a/src/main/java/frc/robot/commands/elevator/ZeroElevator.java b/src/main/java/frc/robot/commands/elevator/ZeroElevator.java index 31dd887..62ea401 100644 --- a/src/main/java/frc/robot/commands/elevator/ZeroElevator.java +++ b/src/main/java/frc/robot/commands/elevator/ZeroElevator.java @@ -10,7 +10,7 @@ public ZeroElevator() { } public void execute() { - Elevator.getInstance().setElevatorPower(RobotMap.Elevator.ZERO_SPEED); + Elevator.getInstance().setElevatorPower(-RobotMap.Elevator.EXTEND_SPEED); } public boolean isFinished() { diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 8f37fa9..4582b72 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -36,8 +36,8 @@ private void configMotors() { masterConfig.MotorOutput.Inverted = RobotMap.Elevator.MASTER_INVERT; - masterConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; - followerConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; + masterConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + followerConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; masterConfig.Voltage.PeakForwardVoltage = RobotMap.MAX_VOLTAGE; masterConfig.Voltage.PeakReverseVoltage = -RobotMap.MAX_VOLTAGE; @@ -83,6 +83,10 @@ public boolean isLimitHit() { return !limitSwitch.get(); } + public boolean isStalling() { + return master.getStatorCurrent().getValue() >= RobotMap.Elevator.ELEVATOR_STALLING_CURRENT; + } + public static Elevator getInstance() { if(instance == null) { instance = new Elevator(); From e96cd0441dca969a3aa4cd3644c25a6bee5ab513 Mon Sep 17 00:00:00 2001 From: 25ChilingH <25ChilingH@students.harker.org> Date: Wed, 6 Mar 2024 22:33:56 -0800 Subject: [PATCH 05/31] Attempt to fix CommandGroup error --- src/main/java/frc/robot/OI.java | 10 +++---- src/main/java/frc/robot/auton/Autons.java | 18 +++++------ .../frc/robot/commands/CommandGroups.java | 30 ++++++++++++++----- 3 files changed, 37 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 4f12082..e36dd48 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -49,8 +49,8 @@ public XboxGamepad getOperator() { } private void initBindings() { - driver.getButtonY().onTrue(CommandGroups.FULL_SHOOT_AMP); - driver.getRightBumper().onTrue(CommandGroups.FULL_SHOOT_SPEAKER); + driver.getButtonY().onTrue(CommandGroups.getFullShootAmp()); + driver.getRightBumper().onTrue(CommandGroups.getFullShootSpeaker()); driver.getButtonB().whileTrue(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0.3))); driver.getButtonB().whileFalse(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0))); @@ -66,10 +66,10 @@ private void initBindings() { driver.getButtonX().onTrue(new InstantCommand( () -> Drivetrain.getInstance().setPose(new Pose2d(new Translation2d(Units.inchesToMeters(14), Units.inchesToMeters(121.25)), new Rotation2d(Math.toRadians(180)))))); - driver.getUpDPadButton().onTrue(CommandGroups.FULL_SHOOT_NO_ALIGN); + driver.getUpDPadButton().onTrue(CommandGroups.getFullShootSpeaker()); - operator.getDownDPadButton().onTrue(CommandGroups.FULL_ZERO); - operator.getRightBumper().onTrue(CommandGroups.FULL_INTAKE); + operator.getDownDPadButton().onTrue(CommandGroups.getFullZeroCommand()); + operator.getRightBumper().onTrue(CommandGroups.getFullIntakeCommand()); operator.getLeftBumper().whileTrue(new OuttakeNote()); operator.getButtonY().whileTrue(new ElevatorManual(RobotMap.Elevator.EXTEND_SPEED)); diff --git a/src/main/java/frc/robot/auton/Autons.java b/src/main/java/frc/robot/auton/Autons.java index 5674eda..aecad0a 100644 --- a/src/main/java/frc/robot/auton/Autons.java +++ b/src/main/java/frc/robot/auton/Autons.java @@ -13,15 +13,15 @@ public class Autons { public static final SequentialCommandGroup fourNotePath = new SequentialCommandGroup( - // new SwervePositionController(Trajectories.startToNote1_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.FULL_SHOOT_SPEAKER) - // .andThen(CommandGroups.FULL_INTAKE) - // .andThen(new SwervePositionController(Trajectories.note1ToShoot1_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.FULL_SHOOT_SPEAKER) - // .andThen(new SwervePositionController(Trajectories.shoot1ToNote2_four, () -> Rotation2d.fromDegrees(180), false)) - // .andThen(CommandGroups.FULL_INTAKE) - // .andThen(new SwervePositionController(Trajectories.note2ToShoot2_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.FULL_SHOOT_SPEAKER)) - // .andThen(new SwervePositionController(Trajectories.shoot2ToNote3_four, () -> Rotation2d.fromDegrees(-157.83), false)) - // .andThen(CommandGroups.FULL_INTAKE) - // .andThen(new SwervePositionController(Trajectories.note3ToShoot3_four, () -> Rotation2d.fromDegrees(-136.16), true).alongWith(CommandGroups.FULL_SHOOT_SPEAKER)) + new SwervePositionController(Trajectories.startToNote1_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()), + CommandGroups.getFullIntakeCommand(), + new SwervePositionController(Trajectories.note1ToShoot1_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()), + new SwervePositionController(Trajectories.shoot1ToNote2_four, () -> Rotation2d.fromDegrees(180), false), + CommandGroups.getFullIntakeCommand(), + new SwervePositionController(Trajectories.note2ToShoot2_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()), + new SwervePositionController(Trajectories.shoot2ToNote3_four, () -> Rotation2d.fromDegrees(-157.83), false), + CommandGroups.getFullIntakeCommand(), + new SwervePositionController(Trajectories.note3ToShoot3_four, () -> Rotation2d.fromDegrees(-136.16), true).alongWith(CommandGroups.getFullShootSpeaker()) ); public static final SequentialCommandGroup threeNotePath = new SequentialCommandGroup( diff --git a/src/main/java/frc/robot/commands/CommandGroups.java b/src/main/java/frc/robot/commands/CommandGroups.java index 9053ed1..75bc758 100644 --- a/src/main/java/frc/robot/commands/CommandGroups.java +++ b/src/main/java/frc/robot/commands/CommandGroups.java @@ -13,15 +13,31 @@ import frc.robot.commands.shooter.ShootNote; public class CommandGroups { - public static final Command FULL_ZERO = new ZeroPivot().alongWith(new ZeroElevator(), new ZeroIntake()); - - public static final Command FULL_INTAKE = new MoveNoteToShooter().raceWith(new IndexToShooter().alongWith(new IntakeNote())); - - public static final Command FULL_SHOOT_SPEAKER = new PivotToAngle(RobotMap.Pivot.Goal.SPEAKER).alongWith(new RevShooter(RobotMap.Shooter.Goal.SPEAKER)).andThen(new ShootNote()).andThen(new ZeroPivot()); + + public static Command getFullZeroCommand() { + return new ZeroPivot() + .alongWith(new ZeroElevator(), new ZeroIntake()); + } + + public static Command getFullIntakeCommand() { + return new MoveNoteToShooter() + .raceWith(new IndexToShooter() + .alongWith(new IntakeNote())); + } - public static final Command FULL_SHOOT_AMP = new PivotToAngle(RobotMap.Pivot.Goal.AMP).alongWith(new RevShooter(RobotMap.Shooter.Goal.AMP)).andThen(new ShootNote()).andThen(new ZeroPivot()); + public static Command getFullShootSpeaker() { + return new PivotToAngle(RobotMap.Pivot.Goal.SPEAKER) + .alongWith(new RevShooter(RobotMap.Shooter.Goal.SPEAKER)) + .andThen(new ShootNote()) + .andThen(new ZeroPivot()); + } + + public static Command getFullShootAmp() { + return new PivotToAngle(RobotMap.Pivot.Goal.AMP) + .alongWith(new RevShooter(RobotMap.Shooter.Goal.AMP)) + .andThen(new ShootNote()).andThen(new ZeroPivot()); + } - public static final Command FULL_SHOOT_NO_ALIGN = new RevShooter(RobotMap.Shooter.Goal.AMP).andThen(new ShootNote()); // public static final Command PRE_DRIVEFWD_CLIMB = new PivotToAngle(RobotMap.Pivot.Goal.TRAP1); // wait for drive forward // public static final Command PRE_DRIVEBKWD_CLIMB = new PivotToAngle(RobotMap.Pivot.Goal.TRAP2).andThen(new MoveToPosition(RobotMap.Elevator.STAGE_HEIGHT).alongWith(new ZeroPivot())); // wait for drive backwards From 5ce151290f9bb5dd741b81b59564450f4f1fd871 Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Thu, 7 Mar 2024 18:04:08 -0800 Subject: [PATCH 06/31] Add InterpolatingTreeMap data points --- src/main/java/frc/robot/OI.java | 4 ++-- src/main/java/frc/robot/RobotMap.java | 2 +- src/main/java/frc/robot/subsystems/Pivot.java | 6 +++++- src/main/java/frc/robot/util/Telemetry.java | 2 +- 4 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index e36dd48..ff43d1f 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -52,8 +52,8 @@ private void initBindings() { driver.getButtonY().onTrue(CommandGroups.getFullShootAmp()); driver.getRightBumper().onTrue(CommandGroups.getFullShootSpeaker()); - driver.getButtonB().whileTrue(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0.3))); - driver.getButtonB().whileFalse(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0))); + driver.getButtonB().whileTrue(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0.1))); + driver.getButtonB().whileFalse(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0.015))); //0.03 // driver.getButtonA().onTrue(new AlignToStage("left")); driver.getButtonSelect().onTrue(new InstantCommand(() -> { diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 2c36f05..a1ac7bf 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -180,7 +180,7 @@ public static final class Shooter { public static final double INDEXING_SPEED = 0.4; public static final double SHOOTING_SPEED = 0.8; - public static final double REVVED_RPS = 1200.0 / 60.0; + public static final double REVVED_RPS = 1000.0 / 60.0; public static final double REVVED_AMP_RPS = 250.0 / 60.0; public static enum Goal { diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 1be526f..555ab26 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -53,7 +53,11 @@ private Pivot() { limitSwitch = new DigitalInput(RobotMap.Pivot.LIMIT_SWITCH_ID); speakerAngles = new InterpolatingDoubleTreeMap(); - speakerAngles.put(0.0, 0.0); // TODO + speakerAngles.put(1.572, 40.0); + speakerAngles.put(2.410, 58.6); + speakerAngles.put(3.448, 58.8); + speakerAngles.put(4.424, 65.0); + speakerAngles.put(4.277,58.8); configMotors(); } diff --git a/src/main/java/frc/robot/util/Telemetry.java b/src/main/java/frc/robot/util/Telemetry.java index f90c753..ba793b1 100644 --- a/src/main/java/frc/robot/util/Telemetry.java +++ b/src/main/java/frc/robot/util/Telemetry.java @@ -254,7 +254,7 @@ public void vision() { tY.setDouble(Limelight.getTy()); NetworkTableEntry distance = _limelight.getEntry("Distance To Speaker"); - distance.setDouble(drive.getDistanceToSpeaker()); + distance.setDouble(Drivetrain.getInstance().getDistanceToSpeaker()); } public static void putModule(int id, String entry, double number) { From 8f546f09972e7c9053df7b6608259068e15a8c22 Mon Sep 17 00:00:00 2001 From: 25ChilingH <25ChilingH@students.harker.org> Date: Fri, 8 Mar 2024 08:24:17 -0800 Subject: [PATCH 07/31] Change CommandGroups --- src/main/java/frc/robot/commands/CommandGroups.java | 12 ++++++------ src/main/java/frc/robot/subsystems/Pivot.java | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/commands/CommandGroups.java b/src/main/java/frc/robot/commands/CommandGroups.java index 75bc758..f3f33de 100644 --- a/src/main/java/frc/robot/commands/CommandGroups.java +++ b/src/main/java/frc/robot/commands/CommandGroups.java @@ -26,15 +26,15 @@ public static Command getFullIntakeCommand() { } public static Command getFullShootSpeaker() { - return new PivotToAngle(RobotMap.Pivot.Goal.SPEAKER) - .alongWith(new RevShooter(RobotMap.Shooter.Goal.SPEAKER)) - .andThen(new ShootNote()) - .andThen(new ZeroPivot()); + return new RevShooter(RobotMap.Shooter.Goal.SPEAKER) + .raceWith(new PivotToAngle(RobotMap.Pivot.Goal.SPEAKER)) + .andThen(new ShootNote()) + .andThen(new ZeroPivot()); } public static Command getFullShootAmp() { - return new PivotToAngle(RobotMap.Pivot.Goal.AMP) - .alongWith(new RevShooter(RobotMap.Shooter.Goal.AMP)) + return new RevShooter(RobotMap.Shooter.Goal.AMP) + .raceWith(new PivotToAngle(RobotMap.Pivot.Goal.AMP)) .andThen(new ShootNote()).andThen(new ZeroPivot()); } diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 555ab26..36ff34f 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -56,8 +56,8 @@ private Pivot() { speakerAngles.put(1.572, 40.0); speakerAngles.put(2.410, 58.6); speakerAngles.put(3.448, 58.8); - speakerAngles.put(4.424, 65.0); speakerAngles.put(4.277,58.8); + speakerAngles.put(4.424, 65.0); configMotors(); } From 4d1ceeb825dd904afb39f3c9f04c668cf3f87295 Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Fri, 8 Mar 2024 15:55:24 -0800 Subject: [PATCH 08/31] Switch driver controls --- src/main/java/frc/robot/OI.java | 6 ++---- src/main/java/frc/robot/RobotMap.java | 5 +---- .../java/frc/robot/commands/drivetrain/SwerveManual.java | 4 ++-- 3 files changed, 5 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index ff43d1f..2b61837 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -49,11 +49,10 @@ public XboxGamepad getOperator() { } private void initBindings() { - driver.getButtonY().onTrue(CommandGroups.getFullShootAmp()); + driver.getLeftBumper().onTrue(CommandGroups.getFullShootAmp()); driver.getRightBumper().onTrue(CommandGroups.getFullShootSpeaker()); driver.getButtonB().whileTrue(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0.1))); - driver.getButtonB().whileFalse(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0.015))); //0.03 // driver.getButtonA().onTrue(new AlignToStage("left")); driver.getButtonSelect().onTrue(new InstantCommand(() -> { @@ -66,9 +65,8 @@ private void initBindings() { driver.getButtonX().onTrue(new InstantCommand( () -> Drivetrain.getInstance().setPose(new Pose2d(new Translation2d(Units.inchesToMeters(14), Units.inchesToMeters(121.25)), new Rotation2d(Math.toRadians(180)))))); - driver.getUpDPadButton().onTrue(CommandGroups.getFullShootSpeaker()); - operator.getDownDPadButton().onTrue(CommandGroups.getFullZeroCommand()); + operator.getRightBumper().onTrue(CommandGroups.getFullIntakeCommand()); operator.getLeftBumper().whileTrue(new OuttakeNote()); diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index a1ac7bf..9ffa7c9 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -198,10 +198,7 @@ public static final class Pivot { public static final double ZERO_SPEED = -0.2; public static final double PIVOT_kP = 47; - public static final double PIVOT_kG = 0; - public static final double PIVOT_kS = 0; //0.10574; - public static final double PIVOT_kV = 0; //69.706; - public static final double PIVOT_kA = 0; //82.618; + public static final double PIVOT_kG = 0.015 * RobotMap.MAX_VOLTAGE; // in volts public static final double TRAP1_ANGLE = 50; public static final double TRAP2_ANGLE = 40; diff --git a/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java b/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java index 8e7a386..67aa613 100644 --- a/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java +++ b/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java @@ -69,13 +69,13 @@ public void execute() { vx = vxFilter.calculate(vx); // limitAcceleration(vx, prevvx); // aligns to speaker - if (OI.getInstance().getDriver().getRightBumperState()) { + if (OI.getInstance().getDriver().getRightTrigger() > 0.5) { omega = Drivetrain.getInstance().alignToSpeaker(); Drivetrain.getInstance().setPreviousHeading(-Drivetrain.getInstance().getPoseEstimatorPose2d().getRotation().getDegrees()); } // aligns to amp - if (OI.getInstance().getDriver().getLeftBumperState()) { + if (OI.getInstance().getDriver().getLeftTrigger() > 0.5) { vx = -Drivetrain.getInstance().alignToAmp()[0]; omega = Drivetrain.getInstance().alignToAmp()[1]; } From 7dd73edebb7e860e93c22caaed6911481f6e39b5 Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Fri, 8 Mar 2024 17:57:22 -0800 Subject: [PATCH 09/31] Add new pivot angle points --- src/main/java/frc/robot/OI.java | 4 ++++ src/main/java/frc/robot/RobotMap.java | 6 +++--- src/main/java/frc/robot/auton/Autons.java | 1 + src/main/java/frc/robot/subsystems/Pivot.java | 13 +++++++------ 4 files changed, 15 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 2b61837..c58fffd 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.RobotMap.Pivot.Goal; import frc.robot.commands.CommandGroups; import frc.robot.commands.drivetrain.AlignToStage; //import frc.robot.commands.CommandGroups; @@ -52,7 +53,10 @@ private void initBindings() { driver.getLeftBumper().onTrue(CommandGroups.getFullShootAmp()); driver.getRightBumper().onTrue(CommandGroups.getFullShootSpeaker()); + driver.getButtonA().onTrue(new PivotToAngle(Goal.SPEAKER)); + driver.getButtonB().whileTrue(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0.1))); + driver.getButtonB().whileFalse(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0))); // driver.getButtonA().onTrue(new AlignToStage("left")); driver.getButtonSelect().onTrue(new InstantCommand(() -> { diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 9ffa7c9..74d1066 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -198,7 +198,7 @@ public static final class Pivot { public static final double ZERO_SPEED = -0.2; public static final double PIVOT_kP = 47; - public static final double PIVOT_kG = 0.015 * RobotMap.MAX_VOLTAGE; // in volts + public static final double PIVOT_kG = 0; // in volts public static final double TRAP1_ANGLE = 50; public static final double TRAP2_ANGLE = 40; @@ -208,7 +208,7 @@ public static final class Pivot { public static final double PIVOT_GEAR_RATIO = 36; public static final double PIVOT_ROT_TO_ANGLE = 360; // motor rotations to degrees - public static final double STALLING_CURRENT = 40; // stalls when current is 50 + public static final double STALLING_CURRENT = 50; // stalls when current is 50 public static final double MAX_ERROR = 0.5; // degrees @@ -260,7 +260,7 @@ public static final class Intake { public static final boolean ROLLER_INVERT = false; public static final double ZERO_SPEED = -0.5; - public static final double ROLLER_SPEED = 0.6; + public static final double ROLLER_SPEED = 1.0; public static final double ROLLER_OUTAKE_SPEED = -0.8; public static final double DEPLOY_kP = 0.07; diff --git a/src/main/java/frc/robot/auton/Autons.java b/src/main/java/frc/robot/auton/Autons.java index aecad0a..cc8cc48 100644 --- a/src/main/java/frc/robot/auton/Autons.java +++ b/src/main/java/frc/robot/auton/Autons.java @@ -13,6 +13,7 @@ public class Autons { public static final SequentialCommandGroup fourNotePath = new SequentialCommandGroup( + CommandGroups.getFullZeroCommand(), new SwervePositionController(Trajectories.startToNote1_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()), CommandGroups.getFullIntakeCommand(), new SwervePositionController(Trajectories.note1ToShoot1_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()), diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 36ff34f..723aaf6 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -16,6 +16,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.Angle; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; @@ -53,12 +54,12 @@ private Pivot() { limitSwitch = new DigitalInput(RobotMap.Pivot.LIMIT_SWITCH_ID); speakerAngles = new InterpolatingDoubleTreeMap(); - speakerAngles.put(1.572, 40.0); - speakerAngles.put(2.410, 58.6); - speakerAngles.put(3.448, 58.8); - speakerAngles.put(4.277,58.8); - speakerAngles.put(4.424, 65.0); - + speakerAngles.put(0.81 + Units.inchesToMeters(14), 31.729 - 10); + speakerAngles.put(1.49 + Units.inchesToMeters(14), 43.506 - 10); + speakerAngles.put(2.27 + Units.inchesToMeters(14), 46.758 - 10); + speakerAngles.put(2.44 + Units.inchesToMeters(14), 48.252 - 10); + speakerAngles.put(2.82 + Units.inchesToMeters(14), 50.712 - 10); + speakerAngles.put(3.2 + Units.inchesToMeters(14), 34.0); configMotors(); } From 712e07ae89e9c5b0d92c9aa429f042a19d4ec602 Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Sat, 9 Mar 2024 18:05:12 -0800 Subject: [PATCH 10/31] amp; stage; sub-shoot speaker --- src/main/java/frc/robot/OI.java | 12 ++--- src/main/java/frc/robot/RobotMap.java | 46 ++++++++++--------- .../frc/robot/commands/CommandGroups.java | 4 +- .../commands/drivetrain/SwerveManual.java | 36 ++++++++------- .../commands/elevator/ElevatorManual.java | 5 +- .../robot/commands/elevator/ZeroElevator.java | 2 +- .../robot/commands/pivot/PivotToAngle.java | 3 ++ .../frc/robot/commands/pivot/ZeroPivot.java | 3 +- .../java/frc/robot/subsystems/Elevator.java | 4 ++ .../java/frc/robot/subsystems/Indexer.java | 2 +- src/main/java/frc/robot/subsystems/Pivot.java | 15 +++--- .../robot/subsystems/swerve/Drivetrain.java | 19 +++++--- src/main/java/frc/robot/util/Telemetry.java | 9 ++-- 13 files changed, 94 insertions(+), 66 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index c58fffd..11ebd73 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -53,15 +53,15 @@ private void initBindings() { driver.getLeftBumper().onTrue(CommandGroups.getFullShootAmp()); driver.getRightBumper().onTrue(CommandGroups.getFullShootSpeaker()); - driver.getButtonA().onTrue(new PivotToAngle(Goal.SPEAKER)); + driver.getUpDPadButton().onTrue(new PivotToAngle(Goal.SPEAKER)); + driver.getDownDPadButton().onTrue(new PivotToAngle(Goal.SUB)); driver.getButtonB().whileTrue(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0.1))); driver.getButtonB().whileFalse(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0))); // driver.getButtonA().onTrue(new AlignToStage("left")); - - driver.getButtonSelect().onTrue(new InstantCommand(() -> { - Drivetrain.getInstance().setYaw(180); - })); + + driver.getButtonY().onTrue(new ElevatorManual(RobotMap.Elevator.EXTEND_SPEED)); + driver.getButtonA().onTrue(new ElevatorManual(-RobotMap.Elevator.EXTEND_SPEED)); driver.getButtonStart().onTrue(new InstantCommand(() -> { Drivetrain.getInstance().toggleRobotCentric(); @@ -74,8 +74,6 @@ private void initBindings() { operator.getRightBumper().onTrue(CommandGroups.getFullIntakeCommand()); operator.getLeftBumper().whileTrue(new OuttakeNote()); - operator.getButtonY().whileTrue(new ElevatorManual(RobotMap.Elevator.EXTEND_SPEED)); - operator.getButtonA().whileTrue(new ElevatorManual(-RobotMap.Elevator.EXTEND_SPEED)); //TESTING // operator.getUpDPadButton().onTrue(CommandGroups.PRE_ALIGN_CLIMB); diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 74d1066..58e1f01 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -111,14 +111,16 @@ public static final class Drivetrain { public static final double MIN_OUTPUT = 0.05; // PID for omega (turning) control - public static final double OMEGA_kP = 5; // TODO + public static final double OMEGA_kP = 3.0; // TODO public static final double OMEGA_kI = 0.0; public static final double OMEGA_kD = 0.1; public static final double MAX_ERROR_SPEAKER = 0.5; //TODO - public static final double VX_AMP_kP = 1; + public static final double VX_AMP_kP = 0.5; + public static final double VY_AMP_kP = 0.5; public static final double MAX_ERROR_VX_AMP = Units.inchesToMeters(1.0); - public static final double OMEGA_AMP_KP = 5; + public static final double MAX_ERROR_VY_AMP = Units.inchesToMeters(3.0); + public static final double OMEGA_AMP_KP = 2.5; public static final double MAX_ERROR_AMP_DEG = 1; public static final double VX_STAGE_kP = 0.19; @@ -134,7 +136,7 @@ public static final class Drivetrain { public static final double MAX_DRIVING_SPEED = 5.0; // m/s //TODO public static final double EXTENDED_MAX_DRIVING_SPEED = 0.3; - public static final double MAX_ACCELERATION = 7; + public static final double MAX_ACCELERATION = 8.5; public static final double MAX_ANGLE_VELOCITY = Math.PI; public static final double EXTENDED_MAX_ANGLE_VELOCITY = Math.PI/10; public static final double MAX_ANGLE_ACCELERATION = MAX_ANGLE_VELOCITY / 2.0; @@ -177,11 +179,11 @@ public static final class Shooter { public static final double SHOOTER_CURRENT_LIMIT_TIME = 0.1; public static final int INDEXER_CURRENT_LIMIT = 80; - public static final double INDEXING_SPEED = 0.4; + public static final double INDEXING_SPEED = 0.32; public static final double SHOOTING_SPEED = 0.8; public static final double REVVED_RPS = 1000.0 / 60.0; - public static final double REVVED_AMP_RPS = 250.0 / 60.0; + public static final double REVVED_AMP_RPS = 150.0 / 60.0; public static enum Goal { AMP, @@ -193,34 +195,36 @@ public static final class Pivot { public static final int MASTER_ID = 24; public static final int LIMIT_SWITCH_ID = 2; - public static final InvertedValue MASTER_INVERT = InvertedValue.Clockwise_Positive; + public static final InvertedValue MASTER_INVERT = InvertedValue.CounterClockwise_Positive; public static final double ZERO_SPEED = -0.2; - public static final double PIVOT_kP = 47; - public static final double PIVOT_kG = 0; // in volts + public static final double PIVOT_kP = 260; + public static final double PIVOT_kG = 0.022; - public static final double TRAP1_ANGLE = 50; + public static final double SUB_ANGLE = 25.0; + public static final double TRAP1_ANGLE = 10; public static final double TRAP2_ANGLE = 40; public static final double TRAP_SCORE_ANGLE = 30; - public static final double AMP_ANGLE = 100; + public static final double AMP_ANGLE = 77; - public static final double PIVOT_GEAR_RATIO = 36; + public static final double PIVOT_GEAR_RATIO = 37.5; public static final double PIVOT_ROT_TO_ANGLE = 360; // motor rotations to degrees public static final double STALLING_CURRENT = 50; // stalls when current is 50 - public static final double MAX_ERROR = 0.5; // degrees + public static final double MAX_ERROR = 1.5; // degrees - public static final double PIVOT_FORWARD_SOFT_LIMIT = 65 / 67.76 * 36; + public static final double PIVOT_FORWARD_SOFT_LIMIT = 65 / 67.76 * 37.5; public static final double PIVOT_REVERSE_SOFT_LIMIT = 0; - public static final double MAX_CRUISE_ACCLERATION = 388.0 / PIVOT_ROT_TO_ANGLE / 67.76 * 36; - public static final double MAX_CRUISE_VElOCITY = 388.0 / PIVOT_ROT_TO_ANGLE / 67.76 * 36; + public static final double MAX_CRUISE_ACCLERATION = 400.0 / PIVOT_ROT_TO_ANGLE / 67.76 * 37.5; + public static final double MAX_CRUISE_VElOCITY = 400.0 / PIVOT_ROT_TO_ANGLE / 67.76 * 37.5; public static enum Goal { AMP, SPEAKER, + SUB, TRAP1, TRAP2, TRAP_SCORE @@ -234,7 +238,7 @@ public static final class Elevator { public static final int LIMIT_SWITCH_ID = 1; public static final double ELEVATOR_kP = 1; - public static final double ELEVATOR_kG = 0.05; + public static final double ELEVATOR_kG = 0.01; public static final double TRAP_HEIGHT = 0; // motor rotations public static final double STAGE_HEIGHT = 20; @@ -242,11 +246,11 @@ public static final class Elevator { public static final double ELEVATOR_FORWARD_SOFT_LIMIT = 0; public static final double ELEVATOR_REVERSE_SOFT_LIMIT = 0; - public static final double ELEVATOR_STALLING_CURRENT = 50; + public static final double ELEVATOR_STALLING_CURRENT = 80; public static final InvertedValue MASTER_INVERT = InvertedValue.Clockwise_Positive; - public static final double EXTEND_SPEED = 0.3; + public static final double EXTEND_SPEED = 0.8; public static final double MAX_ERROR = 1; // motor rotations } @@ -260,7 +264,7 @@ public static final class Intake { public static final boolean ROLLER_INVERT = false; public static final double ZERO_SPEED = -0.5; - public static final double ROLLER_SPEED = 1.0; + public static final double ROLLER_SPEED = 0.5; public static final double ROLLER_OUTAKE_SPEED = -0.8; public static final double DEPLOY_kP = 0.07; @@ -268,7 +272,7 @@ public static final class Intake { public static final double INTAKE_STALLING_CURRENT = 40; - public static final int ROLLER_CURRENT_LIMIT = 90; + public static final int ROLLER_CURRENT_LIMIT = 80; } public static final class Indexer { diff --git a/src/main/java/frc/robot/commands/CommandGroups.java b/src/main/java/frc/robot/commands/CommandGroups.java index f3f33de..16054b3 100644 --- a/src/main/java/frc/robot/commands/CommandGroups.java +++ b/src/main/java/frc/robot/commands/CommandGroups.java @@ -27,14 +27,14 @@ public static Command getFullIntakeCommand() { public static Command getFullShootSpeaker() { return new RevShooter(RobotMap.Shooter.Goal.SPEAKER) - .raceWith(new PivotToAngle(RobotMap.Pivot.Goal.SPEAKER)) + .alongWith(new PivotToAngle(RobotMap.Pivot.Goal.SPEAKER)) .andThen(new ShootNote()) .andThen(new ZeroPivot()); } public static Command getFullShootAmp() { return new RevShooter(RobotMap.Shooter.Goal.AMP) - .raceWith(new PivotToAngle(RobotMap.Pivot.Goal.AMP)) + .alongWith(new PivotToAngle(RobotMap.Pivot.Goal.AMP)) .andThen(new ShootNote()).andThen(new ZeroPivot()); } diff --git a/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java b/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java index 67aa613..eac6198 100644 --- a/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java +++ b/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java @@ -50,18 +50,12 @@ public void execute() { } // Scaling velocities based on multipliers - if (Pivot.getInstance().isExtended()) - { - vx = scaleValues(vx, RobotMap.Drivetrain.EXTENDED_MAX_DRIVING_SPEED); //*(RobotMap.SwerveManual.SPEED_MULTIPLIER); - vy = scaleValues(vy, RobotMap.Drivetrain.EXTENDED_MAX_DRIVING_SPEED) ;//* (RobotMap.SwerveManual.SPEED_MULTIPLIER); - omega = scaleValues(omega, RobotMap.Drivetrain.EXTENDED_MAX_ANGLE_VELOCITY); //* ( RobotMap.SwerveManual.SPEED_MULTIPLIER); - } - else - { - vx = scaleValues(vx, RobotMap.Drivetrain.MAX_DRIVING_SPEED); //*(RobotMap.SwerveManual.SPEED_MULTIPLIER); - vy = scaleValues(vy, RobotMap.Drivetrain.MAX_DRIVING_SPEED) ;//* (RobotMap.SwerveManual.SPEED_MULTIPLIER); - omega = scaleValues(omega, RobotMap.Drivetrain.MAX_ANGLE_VELOCITY); //* ( RobotMap.SwerveManual.SPEED_MULTIPLIER); - } + + vx = scaleValues(vx, RobotMap.Drivetrain.MAX_DRIVING_SPEED); //*(RobotMap.SwerveManual.SPEED_MULTIPLIER); + vy = scaleValues(vy, RobotMap.Drivetrain.MAX_DRIVING_SPEED) ;//* (RobotMap.SwerveManual.SPEED_MULTIPLIER); + omega = scaleValues(omega, RobotMap.Drivetrain.MAX_ANGLE_VELOCITY); //* ( RobotMap.SwerveManual.SPEED_MULTIPLIER); + + omega = Drivetrain.getInstance().adjustPigeon(omega); // limits acceleration @@ -69,15 +63,25 @@ public void execute() { vx = vxFilter.calculate(vx); // limitAcceleration(vx, prevvx); // aligns to speaker - if (OI.getInstance().getDriver().getRightTrigger() > 0.5) { + if (OI.getInstance().getDriver().getRightBumperState()) { omega = Drivetrain.getInstance().alignToSpeaker(); Drivetrain.getInstance().setPreviousHeading(-Drivetrain.getInstance().getPoseEstimatorPose2d().getRotation().getDegrees()); } // aligns to amp + if (OI.getInstance().getDriver().getRightTrigger() > 0.5) { + // vx = -Drivetrain.getInstance().alignToAmp()[0]; + // vy = -Drivetrain.getInstance().alignToAmp()[1]; + vx *= RobotMap.Drivetrain.EXTENDED_MAX_DRIVING_SPEED; + vy *= RobotMap.Drivetrain.EXTENDED_MAX_DRIVING_SPEED; + omega = Drivetrain.getInstance().alignToAmp()[2]; + Drivetrain.getInstance().setPreviousHeading(-Drivetrain.getInstance().getPoseEstimatorPose2d().getRotation().getDegrees()); + } + if (OI.getInstance().getDriver().getLeftTrigger() > 0.5) { - vx = -Drivetrain.getInstance().alignToAmp()[0]; - omega = Drivetrain.getInstance().alignToAmp()[1]; + vx *= RobotMap.Drivetrain.EXTENDED_MAX_DRIVING_SPEED; + vy *= RobotMap.Drivetrain.EXTENDED_MAX_DRIVING_SPEED; + omega *= RobotMap.Drivetrain.EXTENDED_MAX_ANGLE_VELOCITY; } // if rotational velocity is very small @@ -95,7 +99,7 @@ public void execute() { Drivetrain.getInstance() .setAngleAndDrive( ChassisSpeeds.fromFieldRelativeSpeeds( - vx, vy, -omega, Rotation2d.fromDegrees(0))); + vx, vy, -omega, Rotation2d.fromDegrees(180))); else Drivetrain.getInstance() .setAngleAndDrive( diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorManual.java b/src/main/java/frc/robot/commands/elevator/ElevatorManual.java index 4db6b12..e41a6ce 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorManual.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorManual.java @@ -18,7 +18,10 @@ public void execute() { } public boolean isFinished() { - return Elevator.getInstance().isStalling(); + if (power > 0) + return Elevator.getInstance().isAtTop(); + else + return Elevator.getInstance().isStalling(); } @Override diff --git a/src/main/java/frc/robot/commands/elevator/ZeroElevator.java b/src/main/java/frc/robot/commands/elevator/ZeroElevator.java index 62ea401..3533705 100644 --- a/src/main/java/frc/robot/commands/elevator/ZeroElevator.java +++ b/src/main/java/frc/robot/commands/elevator/ZeroElevator.java @@ -14,7 +14,7 @@ public void execute() { } public boolean isFinished() { - return Elevator.getInstance().isLimitHit(); + return Elevator.getInstance().isLimitHit() || Elevator.getInstance().isStalling(); } public void end(boolean interrupted) { diff --git a/src/main/java/frc/robot/commands/pivot/PivotToAngle.java b/src/main/java/frc/robot/commands/pivot/PivotToAngle.java index 0cadb1b..28d28a5 100644 --- a/src/main/java/frc/robot/commands/pivot/PivotToAngle.java +++ b/src/main/java/frc/robot/commands/pivot/PivotToAngle.java @@ -20,6 +20,9 @@ public void execute() { case SPEAKER: ref = Pivot.getInstance().getPivotSetpoint(Drivetrain.getInstance().getDistanceToSpeaker()); break; + case SUB: + ref = RobotMap.Pivot.SUB_ANGLE; + break; case AMP: ref = RobotMap.Pivot.AMP_ANGLE; break; diff --git a/src/main/java/frc/robot/commands/pivot/ZeroPivot.java b/src/main/java/frc/robot/commands/pivot/ZeroPivot.java index bab5c87..394d4a2 100644 --- a/src/main/java/frc/robot/commands/pivot/ZeroPivot.java +++ b/src/main/java/frc/robot/commands/pivot/ZeroPivot.java @@ -14,7 +14,8 @@ public void execute() { } public boolean isFinished() { - return Pivot.getInstance().isStalling(); + return Pivot.getInstance().isLimitHit(); + // return Pivot.getInstance().isStalling(); } public void end(boolean interrupted) { diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 4582b72..9539035 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -83,6 +83,10 @@ public boolean isLimitHit() { return !limitSwitch.get(); } + public boolean isAtTop() { + return master.getPosition().getValue() >= 89; + } + public boolean isStalling() { return master.getStatorCurrent().getValue() >= RobotMap.Elevator.ELEVATOR_STALLING_CURRENT; } diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index db39f53..3a07b9a 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -23,7 +23,7 @@ public void configMotors() { master.restoreFactoryDefaults(); master.setSmartCurrentLimit(RobotMap.Indexer.CURRENT_LIMIT); - master.setIdleMode(IdleMode.kBrake); + master.setIdleMode(IdleMode.kCoast); master.burnFlash(); } diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 723aaf6..0f2087f 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -54,12 +54,15 @@ private Pivot() { limitSwitch = new DigitalInput(RobotMap.Pivot.LIMIT_SWITCH_ID); speakerAngles = new InterpolatingDoubleTreeMap(); - speakerAngles.put(0.81 + Units.inchesToMeters(14), 31.729 - 10); - speakerAngles.put(1.49 + Units.inchesToMeters(14), 43.506 - 10); - speakerAngles.put(2.27 + Units.inchesToMeters(14), 46.758 - 10); - speakerAngles.put(2.44 + Units.inchesToMeters(14), 48.252 - 10); - speakerAngles.put(2.82 + Units.inchesToMeters(14), 50.712 - 10); - speakerAngles.put(3.2 + Units.inchesToMeters(14), 34.0); + speakerAngles.put(0.0, 25.0); + speakerAngles.put(1.877, 25.0); + // speakerAngles.put(2.361, 16.787); + // speakerAngles.put(2.839, 23.643); + // speakerAngles.put(3.228, 33.574); + // speakerAngles.put(3.713, 36.914); + // speakerAngles.put(4.156, 37.969); + // speakerAngles.put(4.507, 39.463); + // speakerAngles.put(5.051, 39.990); configMotors(); } diff --git a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java index b6f054d..27fec21 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java @@ -22,6 +22,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; @@ -62,11 +63,13 @@ public class Drivetrain extends SubsystemBase { private static PIDController omegaSpeakerController = new PIDController(RobotMap.Drivetrain.OMEGA_kP, RobotMap.Drivetrain.OMEGA_kI, RobotMap.Drivetrain.OMEGA_kD); private static PIDController vxAmpController = new PIDController(RobotMap.Drivetrain.VX_AMP_kP, 0, 0); + private static PIDController vyAmpController = new PIDController(RobotMap.Drivetrain.VY_AMP_kP, 0, 0); + private static PIDController omegaAmpController = new PIDController(RobotMap.Drivetrain.OMEGA_AMP_KP, 0, 0); // Standard deviations of pose estimate (x, y, heading) - private static Matrix stateStdDevs = VecBuilder.fill(0.2, 0.2, 0.1); // increase to trust encoder (state) + private static Matrix stateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); // increase to trust encoder (state) // measurements less - private static Matrix visionStdDevs = VecBuilder.fill(0.5, 0.5, 0.1); // increase to trust vsion + private static Matrix visionStdDevs = VecBuilder.fill(0.5, 0.5, 0.3); // increase to trust vsion // measurements less private boolean robotCentric; @@ -100,6 +103,7 @@ private Drivetrain() { omegaSpeakerController.enableContinuousInput(-Math.PI, Math.PI); vxAmpController.setTolerance(RobotMap.Drivetrain.MAX_ERROR_VX_AMP); + vyAmpController.setTolerance(RobotMap.Drivetrain.MAX_ERROR_VY_AMP); omegaAmpController.setTolerance(RobotMap.Drivetrain.MAX_ERROR_AMP_DEG); omegaAmpController.setSetpoint(Math.PI / 2.0); omegaAmpController.enableContinuousInput(-Math.PI, Math.PI); @@ -285,13 +289,14 @@ public double getDistanceToSpeaker() { public double[] alignToAmp() { - double refXFieldRel = Flip.apply(RobotMap.Field.AMP) - .minus(getPoseEstimatorPose2d().getTranslation()).getX(); + Translation2d refFieldRel = Flip.apply(RobotMap.Field.AMP) + .minus(getPoseEstimatorPose2d().getTranslation()); - double vx = MathUtil.clamp(vxAmpController.calculate(refXFieldRel, 0), -1, 1); + double vx = MathUtil.clamp(vxAmpController.calculate(refFieldRel.getX(), 0), -1, 1); double omega = MathUtil.clamp(omegaAmpController.calculate(getPoseEstimatorPose2d().getRotation().getRadians()), -1, 1); - - return new double[]{vx, omega}; + double vy = MathUtil.clamp(vyAmpController.calculate(refFieldRel.getY(), Units.inchesToMeters(14 + 3)), -1, 1); + + return new double[]{vx, vy, omega}; } public boolean alignedToSpeaker() { diff --git a/src/main/java/frc/robot/util/Telemetry.java b/src/main/java/frc/robot/util/Telemetry.java index ba793b1..40a4249 100644 --- a/src/main/java/frc/robot/util/Telemetry.java +++ b/src/main/java/frc/robot/util/Telemetry.java @@ -202,6 +202,9 @@ public void elevator() { NetworkTableEntry elevatorLimitSwitchHit = _elevator.getEntry("Elevator Limit Switch Hit"); elevatorLimitSwitchHit.setBoolean(elevator.isLimitHit()); + NetworkTableEntry elevatorStalling = _elevator.getEntry("Elevator Stalling"); + elevatorStalling.setBoolean(elevator.isLimitHit()); + NetworkTableEntry elevatorSensorPosition = _elevator.getEntry("Elevator Sensor Position"); elevatorSensorPosition.setDouble(elevator.getPosition()); @@ -221,12 +224,12 @@ public void pivot() { NetworkTableEntry pivotSensorVelocity = _pivot.getEntry("Pivot Sensor Velocity"); pivotSensorVelocity.setDouble(pivot.getVelocity()); - NetworkTableEntry pivotVelocity = _pivot.getEntry("Pivot Velocity"); - pivotVelocity.setDouble(pivot.getVelocity()); - NetworkTableEntry pivotStalling = _pivot.getEntry("Pivot Stalling"); pivotStalling.setBoolean(pivot.isStalling()); + NetworkTableEntry pivotLimitSwitch = _pivot.getEntry("Pivot Limit Switch"); + pivotLimitSwitch.setBoolean(pivot.isLimitHit()); + NetworkTableEntry pivotMasterCurrent = _pivot.getEntry("Pivot Master Motor Current"); pivotMasterCurrent.setDouble(pivot.getMasterCurrent()); From 357588747a654124eb0a820f8067a0f93b147894 Mon Sep 17 00:00:00 2001 From: 25ChilingH <25ChilingH@students.harker.org> Date: Sat, 9 Mar 2024 20:32:03 -0800 Subject: [PATCH 11/31] Change std devs for pose estimation --- src/main/java/frc/robot/RobotMap.java | 2 +- src/main/java/frc/robot/subsystems/Elevator.java | 3 ++- src/main/java/frc/robot/subsystems/Pivot.java | 5 ----- src/main/java/frc/robot/subsystems/swerve/Drivetrain.java | 4 ++-- 4 files changed, 5 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 58e1f01..47d4d9b 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -241,7 +241,7 @@ public static final class Elevator { public static final double ELEVATOR_kG = 0.01; public static final double TRAP_HEIGHT = 0; // motor rotations - public static final double STAGE_HEIGHT = 20; + public static final double STAGE_HEIGHT = 89; public static final double ELEVATOR_FORWARD_SOFT_LIMIT = 0; public static final double ELEVATOR_REVERSE_SOFT_LIMIT = 0; diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 9539035..f972bde 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -8,6 +8,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; import frc.robot.RobotMap; public class Elevator extends SubsystemBase { @@ -84,7 +85,7 @@ public boolean isLimitHit() { } public boolean isAtTop() { - return master.getPosition().getValue() >= 89; + return master.getPosition().getValue() >= RobotMap.Elevator.STAGE_HEIGHT; } public boolean isStalling() { diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 0f2087f..4e6fdfd 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -144,11 +144,6 @@ public double getPivotSetpoint(double distance) { return speakerAngles.get(distance); } - public boolean isExtended() - { - return getPosition() > 90.0; - } - private final SysIdRoutine _sysId = new SysIdRoutine( new SysIdRoutine.Config(), new SysIdRoutine.Mechanism( diff --git a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java index 27fec21..60992eb 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java @@ -67,9 +67,9 @@ public class Drivetrain extends SubsystemBase { private static PIDController omegaAmpController = new PIDController(RobotMap.Drivetrain.OMEGA_AMP_KP, 0, 0); // Standard deviations of pose estimate (x, y, heading) - private static Matrix stateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); // increase to trust encoder (state) + private static Matrix stateStdDevs = VecBuilder.fill(0.01, 0.01, 0.01); // increase to trust encoder (state) // measurements less - private static Matrix visionStdDevs = VecBuilder.fill(0.5, 0.5, 0.3); // increase to trust vsion + private static Matrix visionStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); // increase to trust vsion // measurements less private boolean robotCentric; From 6c58324922dadff56d7c291bd6fd89ce98041991 Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Mon, 11 Mar 2024 20:56:21 -0700 Subject: [PATCH 12/31] Subwoofer shot working --- src/main/java/frc/robot/RobotMap.java | 13 ++++--- .../robot/commands/pivot/PivotToAngle.java | 6 ++-- .../frc/robot/commands/pivot/ZeroPivot.java | 7 ++-- src/main/java/frc/robot/subsystems/Pivot.java | 36 ++++++++++++++----- .../robot/subsystems/swerve/Drivetrain.java | 4 +-- src/main/java/frc/robot/util/Limelight.java | 3 +- 6 files changed, 49 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 58e1f01..858aea0 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -194,13 +194,17 @@ public static enum Goal { public static final class Pivot { public static final int MASTER_ID = 24; public static final int LIMIT_SWITCH_ID = 2; + public static final int CAN_CODER_ID = 30; + + public static final double CAN_CODER_OFFSET = 0.359619; //TODO public static final InvertedValue MASTER_INVERT = InvertedValue.CounterClockwise_Positive; public static final double ZERO_SPEED = -0.2; - public static final double PIVOT_kP = 260; - public static final double PIVOT_kG = 0.022; + public static final double PIVOT_kP = 50; + public static final double PIVOT_kG = 0.8; + public static final double PIVOT_kS = 0.10574; public static final double SUB_ANGLE = 25.0; public static final double TRAP1_ANGLE = 10; @@ -209,6 +213,7 @@ public static final class Pivot { public static final double AMP_ANGLE = 77; public static final double PIVOT_GEAR_RATIO = 37.5; + public static final double PIVOT_ROT_TO_ANGLE = 360; // motor rotations to degrees public static final double STALLING_CURRENT = 50; // stalls when current is 50 @@ -218,8 +223,8 @@ public static final class Pivot { public static final double PIVOT_FORWARD_SOFT_LIMIT = 65 / 67.76 * 37.5; public static final double PIVOT_REVERSE_SOFT_LIMIT = 0; - public static final double MAX_CRUISE_ACCLERATION = 400.0 / PIVOT_ROT_TO_ANGLE / 67.76 * 37.5; - public static final double MAX_CRUISE_VElOCITY = 400.0 / PIVOT_ROT_TO_ANGLE / 67.76 * 37.5; + public static final double MAX_CRUISE_ACCLERATION = 400.0 / 67.76 * 37.5 / 360; + public static final double MAX_CRUISE_VElOCITY = 400.0 / 67.76 * 37.5 / 360; public static enum Goal { AMP, diff --git a/src/main/java/frc/robot/commands/pivot/PivotToAngle.java b/src/main/java/frc/robot/commands/pivot/PivotToAngle.java index 28d28a5..86fcc8c 100644 --- a/src/main/java/frc/robot/commands/pivot/PivotToAngle.java +++ b/src/main/java/frc/robot/commands/pivot/PivotToAngle.java @@ -44,8 +44,8 @@ public boolean isFinished() { return MathUtil.compareSetpoint(Pivot.getInstance().getPosition(), ref, RobotMap.Pivot.MAX_ERROR); } - // public void end(boolean interrupted) { - // Pivot.getInstance().setPercentOutput(0); - // } + public void end(boolean interrupted) { + Pivot.getInstance().moveToPosition(ref); + } } diff --git a/src/main/java/frc/robot/commands/pivot/ZeroPivot.java b/src/main/java/frc/robot/commands/pivot/ZeroPivot.java index 394d4a2..56dd138 100644 --- a/src/main/java/frc/robot/commands/pivot/ZeroPivot.java +++ b/src/main/java/frc/robot/commands/pivot/ZeroPivot.java @@ -1,5 +1,6 @@ package frc.robot.commands.pivot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotMap; import frc.robot.subsystems.Pivot; @@ -11,16 +12,18 @@ public ZeroPivot() { public void execute() { Pivot.getInstance().setPercentOutput(RobotMap.Pivot.ZERO_SPEED); + // Pivot.getInstance().setPercentOutput(0); } public boolean isFinished() { - return Pivot.getInstance().isLimitHit(); + return Pivot.getInstance().getPosition() < 0; + // return Pivot.getInstance().isLimitHit(); // return Pivot.getInstance().isStalling(); } public void end(boolean interrupted) { Pivot.getInstance().setPercentOutput(0); - Pivot.getInstance().setSensorPosition(0); + // Pivot.getInstance().setSensorPosition(0); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 0f2087f..6017245 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -7,13 +7,18 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Volts; +import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.util.Units; @@ -38,6 +43,8 @@ public class Pivot extends SubsystemBase { private DigitalInput limitSwitch; + private CANcoder canCoder; + private InterpolatingDoubleTreeMap speakerAngles; // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. private final MutableMeasure _appliedVoltage = mutable(Volts.of(0)); @@ -53,6 +60,8 @@ private Pivot() { limitSwitch = new DigitalInput(RobotMap.Pivot.LIMIT_SWITCH_ID); + canCoder = new CANcoder(RobotMap.Pivot.CAN_CODER_ID); + speakerAngles = new InterpolatingDoubleTreeMap(); speakerAngles.put(0.0, 25.0); speakerAngles.put(1.877, 25.0); @@ -63,6 +72,7 @@ private Pivot() { // speakerAngles.put(4.156, 37.969); // speakerAngles.put(4.507, 39.463); // speakerAngles.put(5.051, 39.990); + configCANcoder(); configMotors(); } @@ -75,28 +85,38 @@ private void configMotors() { masterConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - masterConfig.Feedback.SensorToMechanismRatio = RobotMap.Pivot.PIVOT_GEAR_RATIO; - - // masterConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = RobotMap.Pivot.PIVOT_FORWARD_SOFT_LIMIT; - // masterConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = RobotMap.Pivot.PIVOT_REVERSE_SOFT_LIMIT; - // masterConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; - // masterConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; + // masterConfig.Feedback.SensorToMechanismRatio = RobotMap.Pivot.PIVOT_GEAR_RATIO; masterConfig.Slot0.kP = RobotMap.Pivot.PIVOT_kP; masterConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; masterConfig.Slot0.kG = RobotMap.Pivot.PIVOT_kG; + masterConfig.Slot0.kS = RobotMap.Pivot.PIVOT_kS; masterConfig.MotionMagic.MotionMagicCruiseVelocity = RobotMap.Pivot.MAX_CRUISE_VElOCITY; masterConfig.MotionMagic.MotionMagicAcceleration = RobotMap.Pivot.MAX_CRUISE_ACCLERATION; + masterConfig.Feedback.FeedbackRemoteSensorID = canCoder.getDeviceID(); + masterConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; + master.getConfigurator().apply(masterConfig); } + private void configCANcoder() { + canCoder.clearStickyFaults(); + + CANcoderConfiguration canCoderConfig = new CANcoderConfiguration(); + canCoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + canCoderConfig.MagnetSensor.MagnetOffset = -RobotMap.Pivot.CAN_CODER_OFFSET; // offset is ADDED, so -offset + + canCoder.getConfigurator().apply(canCoderConfig); + } + /* * Get pivot angle in degrees */ public double getPosition() { - return master.getPosition().getValue() * RobotMap.Pivot.PIVOT_ROT_TO_ANGLE; + return canCoder.getAbsolutePosition().getValue() * RobotMap.Pivot.PIVOT_ROT_TO_ANGLE; } public boolean isStalling() { @@ -111,7 +131,7 @@ public double getMasterCurrent() { * Get pivot angle in degrees per second */ public double getVelocity() { - return master.getVelocity().getValue() * RobotMap.Pivot.PIVOT_ROT_TO_ANGLE; + return canCoder.getVelocity().getValue() * RobotMap.Pivot.PIVOT_ROT_TO_ANGLE; } public double getVoltage() { diff --git a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java index 27fec21..4c46abf 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java @@ -67,9 +67,9 @@ public class Drivetrain extends SubsystemBase { private static PIDController omegaAmpController = new PIDController(RobotMap.Drivetrain.OMEGA_AMP_KP, 0, 0); // Standard deviations of pose estimate (x, y, heading) - private static Matrix stateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); // increase to trust encoder (state) + private static Matrix stateStdDevs = VecBuilder.fill(0.01, 0.01, 0.1); // increase to trust encoder (state) // measurements less - private static Matrix visionStdDevs = VecBuilder.fill(0.5, 0.5, 0.3); // increase to trust vsion + private static Matrix visionStdDevs = VecBuilder.fill(0.1, 0.1, 0.3); // increase to trust vsion // measurements less private boolean robotCentric; diff --git a/src/main/java/frc/robot/util/Limelight.java b/src/main/java/frc/robot/util/Limelight.java index 3050ca9..5b01b0c 100644 --- a/src/main/java/frc/robot/util/Limelight.java +++ b/src/main/java/frc/robot/util/Limelight.java @@ -20,7 +20,8 @@ public static Pose2d getBotPose2d() { } public static boolean isPoseValid(Pose2d botPose, Pose2d visionBot) { - return visionBot.getTranslation().getDistance(botPose.getTranslation()) < RobotMap.Camera.MAX_ERROR_VISION_POSE; + return visionBot.getTranslation().getDistance(botPose.getTranslation()) < RobotMap.Camera.MAX_ERROR_VISION_POSE + && getBotPoseVal()[6] / 1000.0 <= 1; } public static double getTimestamp() { From 39410abd90f355a7252a9c01ed8b46f17b234f95 Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Thu, 14 Mar 2024 19:56:21 -0700 Subject: [PATCH 13/31] Working speaker --- src/main/java/frc/robot/OI.java | 4 +- src/main/java/frc/robot/RobotMap.java | 26 +-- src/main/java/frc/robot/auton/Autons.java | 22 +- .../commands/elevator/MoveToPosition.java | 21 +- .../robot/commands/pivot/PivotToAngle.java | 4 + .../frc/robot/commands/pivot/ZeroPivot.java | 2 +- src/main/java/frc/robot/subsystems/Pivot.java | 20 +- .../robot/subsystems/swerve/Drivetrain.java | 4 +- src/main/java/frc/robot/util/Telemetry.java | 3 + .../util/{apriltag.vpr => limelight.vpr} | 189 +++++++++--------- 10 files changed, 153 insertions(+), 142 deletions(-) rename src/main/java/frc/robot/util/{apriltag.vpr => limelight.vpr} (82%) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 11ebd73..5052665 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -12,6 +12,7 @@ //import frc.robot.commands.CommandGroups; // import frc.robot.commands.drivetrain.AlignToStage; import frc.robot.commands.elevator.ElevatorManual; +import frc.robot.commands.elevator.MoveToPosition; import frc.robot.commands.elevator.ZeroElevator; import frc.robot.commands.indexer.IndexToShooter; import frc.robot.commands.intake.IntakeNote; @@ -67,9 +68,10 @@ private void initBindings() { Drivetrain.getInstance().toggleRobotCentric(); })); - driver.getButtonX().onTrue(new InstantCommand( () -> Drivetrain.getInstance().setPose(new Pose2d(new Translation2d(Units.inchesToMeters(14), Units.inchesToMeters(121.25)), new Rotation2d(Math.toRadians(180)))))); + driver.getButtonX().onTrue(new InstantCommand( () -> Drivetrain.getInstance().setPose(new Pose2d(1.28, 5.41, Rotation2d.fromDegrees(180))))); operator.getDownDPadButton().onTrue(CommandGroups.getFullZeroCommand()); + operator.getButtonY().whileTrue(new MoveToPosition(RobotMap.Elevator.STAGE_HEIGHT * 0.85)); operator.getRightBumper().onTrue(CommandGroups.getFullIntakeCommand()); operator.getLeftBumper().whileTrue(new OuttakeNote()); diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 11e453a..8d960c2 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -145,15 +145,15 @@ public static final class Drivetrain { * PID values for X, Y, and Rotation (THETA) */ - public static double X_kP = 0.0; // TODO + public static double X_kP = 1.0; // TODO public static double X_kI = 0.0; public static double X_kD = 0.0; - public static double Y_kP = 0.0; // TODO + public static double Y_kP = 1.0; // TODO public static double Y_kI = 0.0; public static double Y_kD = 0.0; - public static double THETA_kP = 0.0; // TODO + public static double THETA_kP = 0.3; // TODO public static double THETA_kI = 0.0; public static double THETA_kD = 0.0; @@ -179,7 +179,7 @@ public static final class Shooter { public static final double SHOOTER_CURRENT_LIMIT_TIME = 0.1; public static final int INDEXER_CURRENT_LIMIT = 80; - public static final double INDEXING_SPEED = 0.32; + public static final double INDEXING_SPEED = 0.28; public static final double SHOOTING_SPEED = 0.8; public static final double REVVED_RPS = 1000.0 / 60.0; @@ -187,7 +187,7 @@ public static final class Shooter { public static enum Goal { AMP, - SPEAKER, + SPEAKER } } @@ -196,21 +196,21 @@ public static final class Pivot { public static final int LIMIT_SWITCH_ID = 2; public static final int CAN_CODER_ID = 30; - public static final double CAN_CODER_OFFSET = 0.359619; //TODO + public static final double CAN_CODER_OFFSET = 0.022705; //TODO public static final InvertedValue MASTER_INVERT = InvertedValue.CounterClockwise_Positive; public static final double ZERO_SPEED = -0.2; - public static final double PIVOT_kP = 50; - public static final double PIVOT_kG = 0.8; + public static final double PIVOT_kP = 65; + public static final double PIVOT_kG = 0.3; public static final double PIVOT_kS = 0.10574; public static final double SUB_ANGLE = 25.0; public static final double TRAP1_ANGLE = 10; public static final double TRAP2_ANGLE = 40; public static final double TRAP_SCORE_ANGLE = 30; - public static final double AMP_ANGLE = 77; + public static final double AMP_ANGLE = 80; public static final double PIVOT_GEAR_RATIO = 37.5; @@ -255,9 +255,9 @@ public static final class Elevator { public static final InvertedValue MASTER_INVERT = InvertedValue.Clockwise_Positive; - public static final double EXTEND_SPEED = 0.8; + public static final double EXTEND_SPEED = 0.7; - public static final double MAX_ERROR = 1; // motor rotations + public static final double MAX_ERROR = 2; // motor rotations } public static final class Intake { @@ -269,7 +269,7 @@ public static final class Intake { public static final boolean ROLLER_INVERT = false; public static final double ZERO_SPEED = -0.5; - public static final double ROLLER_SPEED = 0.5; + public static final double ROLLER_SPEED = 0.8; public static final double ROLLER_OUTAKE_SPEED = -0.8; public static final double DEPLOY_kP = 0.07; @@ -285,7 +285,7 @@ public static final class Indexer { public static final boolean MASTER_INVERT = false; - public static final double INDEXING_SPEED = 0.8; + public static final double INDEXING_SPEED = 0.65; public static final int CURRENT_LIMIT = 90; diff --git a/src/main/java/frc/robot/auton/Autons.java b/src/main/java/frc/robot/auton/Autons.java index cc8cc48..aa1476e 100644 --- a/src/main/java/frc/robot/auton/Autons.java +++ b/src/main/java/frc/robot/auton/Autons.java @@ -26,17 +26,17 @@ public class Autons ); public static final SequentialCommandGroup threeNotePath = new SequentialCommandGroup( - - // new SwervePositionController(Trajectories.startToShoot1_three, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.FULL_SHOOT_SPEAKER), - // CommandGroups.FULL_INTAKE, - // new SwervePositionController(Trajectories.shoot1ToMiddle1_three, () -> Rotation2d.fromDegrees(140.1), false), - // new SwervePositionController(Trajectories.middleToNote1_three, () -> Rotation2d.fromDegrees(180), false), - // CommandGroups.FULL_INTAKE, - // new SwervePositionController(Trajectories.noteToMiddle2_three, () -> Rotation2d.fromDegrees(180), false), - // new SwervePositionController(Trajectories.middle2ToShoot2_three, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.FULL_SHOOT_SPEAKER), - // new SwervePositionController(Trajectories.shoot2ToNote2_three, () -> Rotation2d.fromDegrees(138.59), false), - // CommandGroups.FULL_INTAKE, - // new SwervePositionController(Trajectories.note2ToShoot3_three, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.FULL_SHOOT_SPEAKER) + CommandGroups.getFullZeroCommand(), + new SwervePositionController(Trajectories.startToShoot1_three, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()), + CommandGroups.getFullIntakeCommand(), + new SwervePositionController(Trajectories.shoot1ToMiddle1_three, () -> Rotation2d.fromDegrees(140.1), false), + new SwervePositionController(Trajectories.middleToNote1_three, () -> Rotation2d.fromDegrees(180), false), + CommandGroups.getFullIntakeCommand(), + new SwervePositionController(Trajectories.noteToMiddle2_three, () -> Rotation2d.fromDegrees(180), false), + new SwervePositionController(Trajectories.middle2ToShoot2_three, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()), + new SwervePositionController(Trajectories.shoot2ToNote2_three, () -> Rotation2d.fromDegrees(138.59), false), + CommandGroups.getFullIntakeCommand(), + new SwervePositionController(Trajectories.note2ToShoot3_three, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()) ); public static final SequentialCommandGroup sixNotePath = new SequentialCommandGroup diff --git a/src/main/java/frc/robot/commands/elevator/MoveToPosition.java b/src/main/java/frc/robot/commands/elevator/MoveToPosition.java index 2e0372c..accbbf1 100644 --- a/src/main/java/frc/robot/commands/elevator/MoveToPosition.java +++ b/src/main/java/frc/robot/commands/elevator/MoveToPosition.java @@ -7,25 +7,28 @@ public class MoveToPosition extends Command { - private double setpoint; + private double height; - public MoveToPosition(double desired) { - this.setpoint = desired; + public MoveToPosition(double height) { + this.height = height; addRequirements(Elevator.getInstance()); } public void execute() { - Elevator.getInstance().moveToPosition(setpoint); + if (Elevator.getInstance().getPosition() > height) + Elevator.getInstance().setElevatorPower(-0.6); + else if (Elevator.getInstance().getPosition() < height) + Elevator.getInstance().setElevatorPower(0.6); } public boolean isFinished() { - return MathUtil.compareSetpoint(Elevator.getInstance().getPosition(), setpoint, RobotMap.Elevator.MAX_ERROR); + return MathUtil.compareSetpoint(Elevator.getInstance().getPosition(), height, RobotMap.Elevator.MAX_ERROR); } - // public void end(boolean interrupted) { - // Elevator.getInstance().moveToPosition(0); - // Elevator.getInstance().setElevatorPower(0); - // } + @Override + public void end(boolean interrupted) { + Elevator.getInstance().setElevatorPower(0); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/pivot/PivotToAngle.java b/src/main/java/frc/robot/commands/pivot/PivotToAngle.java index 86fcc8c..a422ef9 100644 --- a/src/main/java/frc/robot/commands/pivot/PivotToAngle.java +++ b/src/main/java/frc/robot/commands/pivot/PivotToAngle.java @@ -40,6 +40,10 @@ public void execute() { } + public double getRef() { + return ref; + } + public boolean isFinished() { return MathUtil.compareSetpoint(Pivot.getInstance().getPosition(), ref, RobotMap.Pivot.MAX_ERROR); } diff --git a/src/main/java/frc/robot/commands/pivot/ZeroPivot.java b/src/main/java/frc/robot/commands/pivot/ZeroPivot.java index 56dd138..9790d06 100644 --- a/src/main/java/frc/robot/commands/pivot/ZeroPivot.java +++ b/src/main/java/frc/robot/commands/pivot/ZeroPivot.java @@ -16,7 +16,7 @@ public void execute() { } public boolean isFinished() { - return Pivot.getInstance().getPosition() < 0; + return Pivot.getInstance().getPosition() <= 1; // return Pivot.getInstance().isLimitHit(); // return Pivot.getInstance().isStalling(); } diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 8984353..b5e4356 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -63,19 +63,19 @@ private Pivot() { canCoder = new CANcoder(RobotMap.Pivot.CAN_CODER_ID); speakerAngles = new InterpolatingDoubleTreeMap(); - speakerAngles.put(0.0, 25.0); - speakerAngles.put(1.877, 25.0); - // speakerAngles.put(2.361, 16.787); - // speakerAngles.put(2.839, 23.643); - // speakerAngles.put(3.228, 33.574); - // speakerAngles.put(3.713, 36.914); - // speakerAngles.put(4.156, 37.969); - // speakerAngles.put(4.507, 39.463); - // speakerAngles.put(5.051, 39.990); + speakerAngles.put(0.0, 22.0); + speakerAngles.put(1.877, 22.0); + speakerAngles.put(2.361, 16.787 + 15); + speakerAngles.put(2.839, 23.643 + 15); + speakerAngles.put(3.228, 33.574 + 11); + speakerAngles.put(3.713, 36.914 + 10); + speakerAngles.put(4.156, 37.969 + 10); + speakerAngles.put(4.507, 38.463 + 10); + speakerAngles.put(5.051, 39.990 + 10); configCANcoder(); configMotors(); } - + private void configMotors() { master.clearStickyFaults(); diff --git a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java index 60992eb..a4c88cd 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java @@ -67,9 +67,9 @@ public class Drivetrain extends SubsystemBase { private static PIDController omegaAmpController = new PIDController(RobotMap.Drivetrain.OMEGA_AMP_KP, 0, 0); // Standard deviations of pose estimate (x, y, heading) - private static Matrix stateStdDevs = VecBuilder.fill(0.01, 0.01, 0.01); // increase to trust encoder (state) + private static Matrix stateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); // increase to trust encoder (state) // measurements less - private static Matrix visionStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); // increase to trust vsion + private static Matrix visionStdDevs = VecBuilder.fill(0.3, 0.3, 0.3); // increase to trust vsion // measurements less private boolean robotCentric; diff --git a/src/main/java/frc/robot/util/Telemetry.java b/src/main/java/frc/robot/util/Telemetry.java index 40a4249..4dda11a 100644 --- a/src/main/java/frc/robot/util/Telemetry.java +++ b/src/main/java/frc/robot/util/Telemetry.java @@ -221,6 +221,9 @@ public void pivot() { NetworkTableEntry pivotSensorPosition = _pivot.getEntry("Pivot Sensor Position"); pivotSensorPosition.setDouble(pivot.getPosition()); + NetworkTableEntry pivotRef = _pivot.getEntry("Pivot Speaker Setpoint"); + pivotRef.setDouble(pivot.getPivotSetpoint(Drivetrain.getInstance().getDistanceToSpeaker())); + NetworkTableEntry pivotSensorVelocity = _pivot.getEntry("Pivot Sensor Velocity"); pivotSensorVelocity.setDouble(pivot.getVelocity()); diff --git a/src/main/java/frc/robot/util/apriltag.vpr b/src/main/java/frc/robot/util/limelight.vpr similarity index 82% rename from src/main/java/frc/robot/util/apriltag.vpr rename to src/main/java/frc/robot/util/limelight.vpr index d651941..a85c56a 100644 --- a/src/main/java/frc/robot/util/apriltag.vpr +++ b/src/main/java/frc/robot/util/limelight.vpr @@ -1,95 +1,94 @@ -area_max:100 -area_min:0.0017850625000000004 -area_similarity:0 -aspect_max:20.000000 -aspect_min:0.000000 -black_level:0 -blue_balance:1975 -botfloorsnap:1 -botlength:0.7112 -bottype:swerve -botwidth:0.7112 -calibration_type:0 -classifier_conf:0.100000 -contour_grouping:0 -contour_sort_final:6 -convexity_max:100 -convexity_min:10 -corner_approx:5.000000 -crop_x_max:1.000000 -crop_x_min:-1.000000 -crop_y_max:1.000000 -crop_y_min:-1.000000 -cross_a_a:1 -cross_a_x:0 -cross_a_y:0 -cross_b_a:1 -cross_b_x:0 -cross_b_y:0 -desc:apriltag -desired_contour_region:0 -detector_conf:0.800000 -detector_idfilters: -dilation_steps:0 -direction_filter:0 -dual_close_sort_origin:0 -erosion_steps:0 -exposure:692 -fiducial_decoder_strictness:strict -fiducial_denoise:0.000000 -fiducial_idfilters: -fiducial_locfilters: -fiducial_method:sqpnp -fiducial_qualitythreshold:2.0 -fiducial_refine:1 -fiducial_resdiv:2 -fiducial_size:152.4 -fiducial_skip3d:0 -fiducial_type:aprilClassic36h11 -fiducial_vis_mode:3dbotposefieldspace -flicker_correction:0 -force_convex:1 -hue_max:85 -hue_min:60 -image_flip:0 -image_source:0 -img_to_show:0 -intersection_filter:0 -invert_hue:0 -lcgain:6.000000 -multigroup_max:7 -multigroup_min:1 -multigroup_rejector:0 -pipeline_led_enabled:0 -pipeline_led_power:40 -pipeline_res:0 -pipeline_type:3 -red_balance:1200 -roi_x:0.000000 -roi_y:0.000000 -rsf:0 -rspitch:0 -rsroll:0 -rss:0 -rsu:0 -rsyaw:0 -sat_max:255 -sat_min:70 -send_corners:0 -send_raw_contours:0 -solve3d:0 -solve3d_algo:0 -solve3d_bindtarget:1 -solve3d_conf:0.990000 -solve3d_error:8 -solve3d_guess:0 -solve3d_iterations:50 -solve3d_precise:0 -solve3d_zoffset:0.000000 -tsf:0 -tss:0 -tsu:0 -val_max:255 -val_min:70 -x_outlier_miqr:1.5 -y_outlier_miqr:1.5 +area_max:100 +area_min:0.0017850625000000004 +area_similarity:0 +aspect_max:20.000000 +aspect_min:0.000000 +black_level:6 +blue_balance:1558 +botfloorsnap:0 +botlength:0.7112 +botwidth:0.7112 +calibration_type:0 +classifier_conf:0.100000 +contour_grouping:0 +contour_sort_final:6 +convexity_max:100 +convexity_min:10 +corner_approx:5.000000 +crop_x_max:1.000000 +crop_x_min:-1.000000 +crop_y_max:1.000000 +crop_y_min:-1.000000 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:limelight +desired_contour_region:0 +detector_conf:0.800000 +detector_idfilters: +dilation_steps:0 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:327 +fiducial_decoder_strictness:strict +fiducial_denoise:0.000000 +fiducial_idfilters: +fiducial_locfilters: +fiducial_method:sqpnp +fiducial_qualitythreshold:2.0 +fiducial_refine:1 +fiducial_resdiv:2 +fiducial_size:152.4 +fiducial_skip3d:1 +fiducial_type:aprilClassic36h11 +fiducial_vis_mode:3dtargposebotspace +flicker_correction:0 +force_convex:1 +hue_max:85 +hue_min:60 +image_flip:0 +image_source:0 +img_to_show:0 +intersection_filter:0 +invert_hue:0 +lcgain:9.8 +multigroup_max:7 +multigroup_min:1 +multigroup_rejector:0 +pipeline_led_enabled:1 +pipeline_led_power:40 +pipeline_res:1 +pipeline_type:3 +red_balance:1303 +roi_x:0.000000 +roi_y:0.000000 +rsf:0 +rspitch:0 +rsroll:0 +rss:0 +rsu:0 +rsyaw:0 +sat_max:255 +sat_min:70 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precise:0 +solve3d_zoffset:0.000000 +tsf:0 +tss:0 +tsu:0 +val_max:255 +val_min:70 +x_outlier_miqr:1.5 +y_outlier_miqr:1.5 From 71385723c46f9f0b915064424ce1b1fa6ba6dbde Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Fri, 15 Mar 2024 17:56:18 -0700 Subject: [PATCH 14/31] comp qual day 1; autons; general fixes --- .../pathplanner/paths/FourNotePath.path | 25 ++++---- .../pathplanner/paths/New New Path.path | 58 +++++++++++++++++++ .../deploy/pathplanner/paths/New Path.path | 58 +++++++++++++++++++ .../pathplanner/paths/ThreeNotePath.path | 52 ++++++++--------- src/main/java/frc/robot/OI.java | 2 +- src/main/java/frc/robot/Robot.java | 21 ++++++- src/main/java/frc/robot/RobotMap.java | 32 +++++----- src/main/java/frc/robot/auton/Autons.java | 36 +++++++----- .../robot/auton/SwervePositionController.java | 6 +- .../java/frc/robot/auton/Trajectories.java | 20 +++++-- .../java/frc/robot/subsystems/Indexer.java | 26 ++++++--- src/main/java/frc/robot/subsystems/Pivot.java | 19 +++--- .../robot/subsystems/swerve/Drivetrain.java | 4 +- src/main/java/frc/robot/util/Telemetry.java | 2 +- src/main/java/frc/robot/util/limelight.vpr | 8 +-- 15 files changed, 260 insertions(+), 109 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/New New Path.path create mode 100644 src/main/deploy/pathplanner/paths/New Path.path diff --git a/src/main/deploy/pathplanner/paths/FourNotePath.path b/src/main/deploy/pathplanner/paths/FourNotePath.path index e084a2a..c12faf4 100644 --- a/src/main/deploy/pathplanner/paths/FourNotePath.path +++ b/src/main/deploy/pathplanner/paths/FourNotePath.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 2.398033988749899, - "y": 5.413895591680073 + "x": 1.7164528753779988, + "y": 4.869096333680088 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.565241563695676, - "y": 4.153622036288668 + "x": 2.573413830101682, + "y": 4.4600922416554205 }, "prevControl": { - "x": 2.5089519071853434, - "y": 4.132111461906811 + "x": 2.603543675107459, + "y": 4.407905819281239 }, "nextControl": { - "x": 3.0471837503016173, - "y": 4.33779180912854 + "x": 2.315447370819469, + "y": 4.906903255780861 }, "isLocked": false, "linkedName": null @@ -127,7 +127,7 @@ "rotationTargets": [ { "waypointRelativePos": 1, - "rotationDegrees": 180.0, + "rotationDegrees": 133.12669653785858, "rotateFast": false }, { @@ -145,11 +145,6 @@ "rotationDegrees": -143.15527430224782, "rotateFast": false }, - { - "waypointRelativePos": 0, - "rotationDegrees": 180.0, - "rotateFast": false - }, { "waypointRelativePos": 3, "rotationDegrees": 180.0, @@ -177,7 +172,7 @@ "reversed": false, "folder": null, "previewStartingState": { - "rotation": -1.473689633364434, + "rotation": 180.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/New New Path.path b/src/main/deploy/pathplanner/paths/New New Path.path new file mode 100644 index 0000000..afe4e22 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New New Path.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 6.0 + }, + "prevControl": { + "x": 3.0, + "y": 6.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 45.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 90.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..86ee0ed --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.8984446913140571, + "y": 6.446683545798883 + }, + "prevControl": null, + "nextControl": { + "x": 1.8984446913140567, + "y": 6.446683545798883 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.3068121248935265, + "y": 7.3620736565316545 + }, + "prevControl": { + "x": 3.3068121248935265, + "y": 7.3620736565316545 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0, + "rotationDegrees": -119.7448812969422, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 90.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -108.97040780848653, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ThreeNotePath.path b/src/main/deploy/pathplanner/paths/ThreeNotePath.path index 7fedba8..c608828 100644 --- a/src/main/deploy/pathplanner/paths/ThreeNotePath.path +++ b/src/main/deploy/pathplanner/paths/ThreeNotePath.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 1.51, - "y": 1.3588927224150746 + "x": 1.570379985362581, + "y": 1.3536087807644175 }, "prevControl": null, "nextControl": { - "x": 6.9530533212918275, - "y": 0.7031704202297295 + "x": 7.013433306654408, + "y": 0.6978864785790724 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.9930962948430917, - "y": 3.4618237659134143 + "x": 1.9793840773921174, + "y": 3.47653478225106 }, "prevControl": { - "x": 3.518485190594432, - "y": 2.7165654778797874 + "x": 1.8625257653836784, + "y": 3.233079965566812 }, "nextControl": { - "x": 2.7147240100272763, - "y": 3.856691746747238 + "x": 2.188448208451587, + "y": 3.9120850552916213 }, "isLocked": false, "linkedName": null @@ -33,15 +33,15 @@ { "anchor": { "x": 5.182268647489855, - "y": 1.8719319735981343 + "y": 1.52 }, "prevControl": { "x": 3.989225197806464, - "y": 2.4868913206357983 + "y": 2.134959347037664 }, "nextControl": { "x": 5.582490574055011, - "y": 1.6656358703874938 + "y": 1.3137038967893595 }, "isLocked": false, "linkedName": null @@ -65,31 +65,31 @@ { "anchor": { "x": 5.182268647489855, - "y": 1.8719319735981343 + "y": 1.5191580561097076 }, "prevControl": { "x": 6.271447434262405, - "y": 1.628337605806952 + "y": 1.2755636883185253 }, "nextControl": { "x": 4.093089860717305, - "y": 2.1155263413893164 + "y": 1.7627524239008894 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.9930962948430917, - "y": 3.4618237659134143 + "x": 1.9793840773921174, + "y": 3.47653478225106 }, "prevControl": { - "x": 3.3796657049805376, - "y": 3.1725788667136037 + "x": 2.135195160070036, + "y": 3.0772688828888937 }, "nextControl": { - "x": 2.6065268847056466, - "y": 3.751068665113225 + "x": 1.8038647561047902, + "y": 3.926303043049836 }, "isLocked": false, "linkedName": null @@ -112,12 +112,12 @@ }, { "anchor": { - "x": 2.9930962948430917, - "y": 3.4618237659134143 + "x": 1.9793840773921174, + "y": 3.47653478225106 }, "prevControl": { - "x": 3.2565818269753746, - "y": 3.757857413165056 + "x": 3.5959240601755234, + "y": 3.233079965566812 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 5052665..f7ce903 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -71,7 +71,7 @@ private void initBindings() { driver.getButtonX().onTrue(new InstantCommand( () -> Drivetrain.getInstance().setPose(new Pose2d(1.28, 5.41, Rotation2d.fromDegrees(180))))); operator.getDownDPadButton().onTrue(CommandGroups.getFullZeroCommand()); - operator.getButtonY().whileTrue(new MoveToPosition(RobotMap.Elevator.STAGE_HEIGHT * 0.85)); + operator.getButtonY().whileTrue(new MoveToPosition(RobotMap.Elevator.STAGE_HEIGHT * 0.95)); operator.getRightBumper().onTrue(CommandGroups.getFullIntakeCommand()); operator.getLeftBumper().whileTrue(new OuttakeNote()); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c8a04ee..41d2685 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; @@ -23,7 +24,10 @@ // import frc.robot.commands.CommandGroups; //import frc.robot.commands.CommandGroups; import frc.robot.commands.drivetrain.SwerveManual; +import frc.robot.subsystems.Indexer; +import frc.robot.subsystems.Intake; import frc.robot.subsystems.Pivot; +import frc.robot.subsystems.Shooter; // import frc.robot.commands.shooter.ShooterManual; // import frc.robot.subsystems.Intake; // import frc.robot.subsystems.Shooter; @@ -80,6 +84,7 @@ public void robotInit() { autonChooser.setDefaultOption("Four Note Path", "Four Note Path"); autonChooser.addOption("Six Note Path", "Six Note Path"); autonChooser.addOption("Three Note Path", "Three Note Path"); + autonChooser.addOption("One Note Path", "One Note Path"); SmartDashboard.putData("Auton Chooser", autonChooser); } @@ -104,12 +109,17 @@ public void autonomousInit() { Autons.fourNotePath.schedule(); break; case "Three Note Path": - Drivetrain.getInstance().setPose(new Pose2d(1.51, 1.36, Rotation2d.fromDegrees(180))); + Drivetrain.getInstance().setPose(new Pose2d(1.51, 1.36 + Units.inchesToMeters(3), Rotation2d.fromDegrees(180))); Autons.threeNotePath.schedule(); break; case "Six Note Path": Drivetrain.getInstance().setPose(new Pose2d(1.72, 5.56, Rotation2d.fromDegrees(180))); Autons.sixNotePath.schedule(); + break; + case "One Note Path": + Drivetrain.getInstance().setPose(new Pose2d(0.90, 6.45 + , Rotation2d.fromDegrees(-119.49))); + Autons.oneNote.schedule(); // default: // Drivetrain.getInstance().setPose(Flip.apply(new Pose2d(1.28, 5.41, Rotation2d.fromDegrees(180)))); // Autons.fourNotePath.schedule(); @@ -125,7 +135,7 @@ public void teleopInit() { Autons.fourNotePath.cancel(); Autons.threeNotePath.cancel(); Autons.sixNotePath.cancel(); - Drivetrain.getInstance().setYaw(0); + Autons.oneNote.cancel(); } @Override @@ -140,7 +150,12 @@ public void disabledInit() { } @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + Intake.getInstance().setRollerPower(0); + Intake.getInstance().setDeployPos(0); + Indexer.getInstance().setPower(0); + Shooter.getInstance().setIndexer(0); + } @Override public void testInit() {} diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 8d960c2..6d6be5d 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -65,7 +65,7 @@ public static final class SwerveModule { public static final double[] CAN_CODER_OFFSETS = (FIRST_BOT) ? new double[]{-0.155518, -0.069092, -0.360596-0.299805, -0.402588-0.193848} : new double[]{0.403320, 0.367188, 0.099609, -0.030518}; // current limit constants for translation motors - public static final double TRANS_CURRENT_LIMIT = 30; + public static final double TRANS_CURRENT_LIMIT = 40; public static final double TRANS_THRESHOLD_CURRENT = 55; public static final double TRANS_THRESHOLD_TIME= 0.1; @@ -111,10 +111,10 @@ public static final class Drivetrain { public static final double MIN_OUTPUT = 0.05; // PID for omega (turning) control - public static final double OMEGA_kP = 3.0; // TODO + public static final double OMEGA_kP = 2.5; // TODO public static final double OMEGA_kI = 0.0; public static final double OMEGA_kD = 0.1; - public static final double MAX_ERROR_SPEAKER = 0.5; //TODO + public static final double MAX_ERROR_SPEAKER = 1; public static final double VX_AMP_kP = 0.5; public static final double VY_AMP_kP = 0.5; @@ -153,7 +153,7 @@ public static final class Drivetrain { public static double Y_kI = 0.0; public static double Y_kD = 0.0; - public static double THETA_kP = 0.3; // TODO + public static double THETA_kP = 1.0; // TODO public static double THETA_kI = 0.0; public static double THETA_kD = 0.0; @@ -179,7 +179,7 @@ public static final class Shooter { public static final double SHOOTER_CURRENT_LIMIT_TIME = 0.1; public static final int INDEXER_CURRENT_LIMIT = 80; - public static final double INDEXING_SPEED = 0.28; + public static final double INDEXING_SPEED = 0.25; public static final double SHOOTING_SPEED = 0.8; public static final double REVVED_RPS = 1000.0 / 60.0; @@ -202,8 +202,8 @@ public static final class Pivot { public static final double ZERO_SPEED = -0.2; - public static final double PIVOT_kP = 65; - public static final double PIVOT_kG = 0.3; + public static final double PIVOT_kP = 105; + public static final double PIVOT_kG = 0.4; public static final double PIVOT_kS = 0.10574; public static final double SUB_ANGLE = 25.0; @@ -218,7 +218,7 @@ public static final class Pivot { public static final double STALLING_CURRENT = 50; // stalls when current is 50 - public static final double MAX_ERROR = 1.5; // degrees + public static final double MAX_ERROR = 0.9; // degrees public static final double PIVOT_FORWARD_SOFT_LIMIT = 65 / 67.76 * 37.5; public static final double PIVOT_REVERSE_SOFT_LIMIT = 0; @@ -251,7 +251,7 @@ public static final class Elevator { public static final double ELEVATOR_FORWARD_SOFT_LIMIT = 0; public static final double ELEVATOR_REVERSE_SOFT_LIMIT = 0; - public static final double ELEVATOR_STALLING_CURRENT = 80; + public static final double ELEVATOR_STALLING_CURRENT = 90; public static final InvertedValue MASTER_INVERT = InvertedValue.Clockwise_Positive; @@ -269,25 +269,27 @@ public static final class Intake { public static final boolean ROLLER_INVERT = false; public static final double ZERO_SPEED = -0.5; - public static final double ROLLER_SPEED = 0.8; + public static final double ROLLER_SPEED = 0.7; public static final double ROLLER_OUTAKE_SPEED = -0.8; public static final double DEPLOY_kP = 0.07; - public static final double INTAKE_DEPLOY = 23; // rotations + public static final double INTAKE_DEPLOY = 23.5; // rotations public static final double INTAKE_STALLING_CURRENT = 40; - public static final int ROLLER_CURRENT_LIMIT = 80; + public static final int ROLLER_CURRENT_LIMIT = 70; } public static final class Indexer { public static final int MASTER_ID = 5; - public static final boolean MASTER_INVERT = false; + public static final InvertedValue MASTER_INVERT = InvertedValue.CounterClockwise_Positive; - public static final double INDEXING_SPEED = 0.65; + public static final double INDEXING_SPEED = 1.0; - public static final int CURRENT_LIMIT = 90; + public static final int CURRENT_LIMIT = 30; + public static final int CURRENT_LIMIT_THRESHOLD = 40; + public static final double CURRENT_LIMIT_TIME = 0.1; } diff --git a/src/main/java/frc/robot/auton/Autons.java b/src/main/java/frc/robot/auton/Autons.java index aa1476e..2eb8fc1 100644 --- a/src/main/java/frc/robot/auton/Autons.java +++ b/src/main/java/frc/robot/auton/Autons.java @@ -1,44 +1,50 @@ package frc.robot.auton; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.commands.CommandGroups; +import frc.robot.subsystems.swerve.Drivetrain; // import frc.robot.commands.CommandGroups; // import frc.robot.commands.indexer.IndexToShooter; // import frc.robot.commands.shooter.MoveNoteToShooter; // import frc.robot.commands.shooter.ShooterManual; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.Trajectory; public class Autons { public static final SequentialCommandGroup fourNotePath = new SequentialCommandGroup( CommandGroups.getFullZeroCommand(), - new SwervePositionController(Trajectories.startToNote1_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()), - CommandGroups.getFullIntakeCommand(), + CommandGroups.getFullShootSpeaker(), + new SwervePositionController(Trajectories.startToNote1_four, () -> Rotation2d.fromDegrees(133.13), false).alongWith(CommandGroups.getFullIntakeCommand()), new SwervePositionController(Trajectories.note1ToShoot1_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()), - new SwervePositionController(Trajectories.shoot1ToNote2_four, () -> Rotation2d.fromDegrees(180), false), - CommandGroups.getFullIntakeCommand(), + new SwervePositionController(Trajectories.shoot1ToNote2_four, () -> Rotation2d.fromDegrees(180), false).alongWith(CommandGroups.getFullIntakeCommand()), new SwervePositionController(Trajectories.note2ToShoot2_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()), - new SwervePositionController(Trajectories.shoot2ToNote3_four, () -> Rotation2d.fromDegrees(-157.83), false), - CommandGroups.getFullIntakeCommand(), + new SwervePositionController(Trajectories.shoot2ToNote3_four, () -> Rotation2d.fromDegrees(-157.83), false).alongWith(CommandGroups.getFullIntakeCommand()), new SwervePositionController(Trajectories.note3ToShoot3_four, () -> Rotation2d.fromDegrees(-136.16), true).alongWith(CommandGroups.getFullShootSpeaker()) ); public static final SequentialCommandGroup threeNotePath = new SequentialCommandGroup( CommandGroups.getFullZeroCommand(), - new SwervePositionController(Trajectories.startToShoot1_three, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()), - CommandGroups.getFullIntakeCommand(), - new SwervePositionController(Trajectories.shoot1ToMiddle1_three, () -> Rotation2d.fromDegrees(140.1), false), - new SwervePositionController(Trajectories.middleToNote1_three, () -> Rotation2d.fromDegrees(180), false), - CommandGroups.getFullIntakeCommand(), + new SwervePositionController(Trajectories.startToShoot1_three, () -> Rotation2d.fromDegrees(140.1), true).alongWith(CommandGroups.getFullShootSpeaker()), + new SwervePositionController(Trajectories.shoot1ToMiddle1_three, () -> Rotation2d.fromDegrees(180), false), + new SwervePositionController(Trajectories.middleToNote1_three, () -> Rotation2d.fromDegrees(180), false).alongWith(CommandGroups.getFullIntakeCommand()), new SwervePositionController(Trajectories.noteToMiddle2_three, () -> Rotation2d.fromDegrees(180), false), - new SwervePositionController(Trajectories.middle2ToShoot2_three, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()), - new SwervePositionController(Trajectories.shoot2ToNote2_three, () -> Rotation2d.fromDegrees(138.59), false), - CommandGroups.getFullIntakeCommand(), - new SwervePositionController(Trajectories.note2ToShoot3_three, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()) + new SwervePositionController(Trajectories.middle2ToShoot2_three, () -> Rotation2d.fromDegrees(140.1), true).alongWith(CommandGroups.getFullShootSpeaker()), + new SwervePositionController(Trajectories.shoot2ToNote2_three, () -> Rotation2d.fromDegrees(180), false).alongWith(CommandGroups.getFullIntakeCommand()), + new SwervePositionController(Trajectories.note2ToShoot3_three, () -> Rotation2d.fromDegrees(140.1), true).alongWith(CommandGroups.getFullShootSpeaker()) ); + + + public static final SequentialCommandGroup oneNote = new SequentialCommandGroup( + CommandGroups.getFullZeroCommand(), + CommandGroups.getFullShootSpeaker(), + new SwervePositionController(Trajectories.note1_one, () -> Rotation2d.fromDegrees(180), false) + ); + public static final SequentialCommandGroup sixNotePath = new SequentialCommandGroup ( // new SwervePositionController(Trajectories.startToNote1_six, () -> Rotation2d.fromDegrees(180), false), diff --git a/src/main/java/frc/robot/auton/SwervePositionController.java b/src/main/java/frc/robot/auton/SwervePositionController.java index c5f50cd..f26c891 100644 --- a/src/main/java/frc/robot/auton/SwervePositionController.java +++ b/src/main/java/frc/robot/auton/SwervePositionController.java @@ -35,7 +35,7 @@ public SwervePositionController( this.trajectory = trajectory; this.referenceHeading = referenceHeading; this.alignToSpeaker = alignToSpeaker; - thetaController.enableContinuousInput(-Math.PI, Math.PI); + thetaController.enableContinuousInput(-0.5, 0.5); addRequirements(Drivetrain.getInstance()); } @@ -75,7 +75,7 @@ public void execute() { double xFF = desiredState.velocityMetersPerSecond * desiredState.poseMeters.getRotation().getCos(); // meters per second double yFF = desiredState.velocityMetersPerSecond * desiredState.poseMeters.getRotation().getSin(); // meters per second - double thetaFF = MathUtil.clamp((alignToSpeaker) ? Drivetrain.getInstance().alignToSpeaker() + double thetaFF = MathUtil.clamp((alignToSpeaker) ? -Drivetrain.getInstance().alignToSpeaker() : thetaController.calculate(currentRotation.getRotations(), referenceAngle.getRotations()), -clampAdd, clampAdd); // radians per second @@ -89,7 +89,7 @@ public void execute() { * Send values to Drivetrain */ ChassisSpeeds adjustedSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xFF + xFeedback, yFF + yFeedback, thetaFF, - currentPose.getRotation()); + Drivetrain.getInstance().getPoseEstimatorPose2d().getRotation()); Drivetrain.getInstance().setAngleAndDrive(adjustedSpeeds); } diff --git a/src/main/java/frc/robot/auton/Trajectories.java b/src/main/java/frc/robot/auton/Trajectories.java index d3b79e6..2608d5f 100644 --- a/src/main/java/frc/robot/auton/Trajectories.java +++ b/src/main/java/frc/robot/auton/Trajectories.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; +import edu.wpi.first.math.util.Units; import java.util.ArrayList; import java.util.List; @@ -17,15 +18,15 @@ public class Trajectories { */ public static Trajectory startToShoot1_three = generateTrajectory( List.of( - new Pose2d(1.51, 1.36, Rotation2d.fromDegrees(180)), - new Pose2d(2.99, 3.46, Rotation2d.fromDegrees(140.1))), + new Pose2d(1.51, 1.36 + Units.inchesToMeters(3), Rotation2d.fromDegrees(180)), + new Pose2d(1.98, 3.48, Rotation2d.fromDegrees(140.1))), 5.0, 2.5, 0.0, 0.0, true); public static Trajectory shoot1ToMiddle1_three = generateTrajectory( - List.of(new Pose2d(2.99, 3.46, Rotation2d.fromDegrees(140.1)), + List.of(new Pose2d(1.98, 3.48, Rotation2d.fromDegrees(140.1)), new Pose2d(5.18, 1.87, Rotation2d.fromDegrees(180))), 5.0, 2.5, @@ -50,14 +51,14 @@ public class Trajectories { false); public static Trajectory middle2ToShoot2_three = generateTrajectory( List.of(new Pose2d(5.18, 1.87, Rotation2d.fromDegrees(180)), - new Pose2d(2.99, 3.46, Rotation2d.fromDegrees(138.59))), + new Pose2d(1.98, 3.48, Rotation2d.fromDegrees(140.1))), 5.0, 2.5, 3.0, 0.0, false); public static Trajectory shoot2ToNote2_three= generateTrajectory( - List.of(new Pose2d(2.99, 3.46, Rotation2d.fromDegrees(138.59)), + List.of(new Pose2d(1.98, 3.48, Rotation2d.fromDegrees(140.1)), new Pose2d(7.69, 0.74, Rotation2d.fromDegrees(180))), 5.0, 2.5, @@ -66,7 +67,7 @@ public class Trajectories { false); public static Trajectory note2ToShoot3_three = generateTrajectory( List.of(new Pose2d(7.69, 0.74, Rotation2d.fromDegrees(180)), - new Pose2d(2.99, 3.46, Rotation2d.fromDegrees(140.0))), + new Pose2d(1.98, 3.48, Rotation2d.fromDegrees(140.0))), 5.0, 2.5, 0.0, @@ -225,6 +226,13 @@ public class Trajectories { 0.0, 0.0, false); + + public static Trajectory note1_one = generateTrajectory( + List.of(new Pose2d(0.90, 6.45, Rotation2d.fromDegrees(-119.49)), + new Pose2d(4.31, 7.36, Rotation2d.fromDegrees(180))), + 2.0, 1.0, 0.0, 0.0, true); + + /** * generates a Trajectory given a list of Pose2d points, max velocity, max * acceleration, start velocity, and end velocity, and if flipped due to diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index 3a07b9a..30125d4 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -1,6 +1,10 @@ package frc.robot.subsystems; import com.revrobotics.CANSparkLowLevel.MotorType; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkBase.IdleMode; @@ -9,23 +13,27 @@ public class Indexer extends SubsystemBase { private static Indexer instance; - private CANSparkMax master; + private TalonFX master; private Indexer() { - master = new CANSparkMax(RobotMap.Indexer.MASTER_ID, MotorType.kBrushless); - - master.setInverted(RobotMap.Indexer.MASTER_INVERT); + master = new TalonFX(RobotMap.Indexer.MASTER_ID); configMotors(); } public void configMotors() { - master.restoreFactoryDefaults(); + master.clearStickyFaults(); + + TalonFXConfiguration masterConfiguration = new TalonFXConfiguration(); + masterConfiguration.MotorOutput.Inverted = RobotMap.Indexer.MASTER_INVERT; + + masterConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Coast; + masterConfiguration.CurrentLimits.SupplyCurrentLimit = RobotMap.Indexer.CURRENT_LIMIT; + masterConfiguration.CurrentLimits.SupplyCurrentThreshold = RobotMap.Indexer.CURRENT_LIMIT_THRESHOLD; + masterConfiguration.CurrentLimits.SupplyTimeThreshold = RobotMap.Indexer.CURRENT_LIMIT_TIME; + masterConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true; - master.setSmartCurrentLimit(RobotMap.Indexer.CURRENT_LIMIT); - master.setIdleMode(IdleMode.kCoast); - - master.burnFlash(); + master.getConfigurator().apply(masterConfiguration); } public void setPower(double power) { diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index b5e4356..25b25f0 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -63,15 +63,16 @@ private Pivot() { canCoder = new CANcoder(RobotMap.Pivot.CAN_CODER_ID); speakerAngles = new InterpolatingDoubleTreeMap(); - speakerAngles.put(0.0, 22.0); - speakerAngles.put(1.877, 22.0); - speakerAngles.put(2.361, 16.787 + 15); - speakerAngles.put(2.839, 23.643 + 15); - speakerAngles.put(3.228, 33.574 + 11); - speakerAngles.put(3.713, 36.914 + 10); - speakerAngles.put(4.156, 37.969 + 10); - speakerAngles.put(4.507, 38.463 + 10); - speakerAngles.put(5.051, 39.990 + 10); + speakerAngles.put(0.0, 21.5); + speakerAngles.put(1.787, 22.5); + speakerAngles.put(2.043, 35.571); + speakerAngles.put(2.361, 16.787 + 16); + speakerAngles.put(2.839, 23.643 + 16); + speakerAngles.put(3.228, 33.574 + 12); + speakerAngles.put(3.713, 36.914 + 9); + speakerAngles.put(4.156, 37.969 + 9); + speakerAngles.put(4.507, 38.463 + 9); + speakerAngles.put(5.051, 39.990 + 9); configCANcoder(); configMotors(); } diff --git a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java index a4c88cd..1f4fcc6 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java @@ -69,7 +69,7 @@ public class Drivetrain extends SubsystemBase { // Standard deviations of pose estimate (x, y, heading) private static Matrix stateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); // increase to trust encoder (state) // measurements less - private static Matrix visionStdDevs = VecBuilder.fill(0.3, 0.3, 0.3); // increase to trust vsion + private static Matrix visionStdDevs = VecBuilder.fill(0.15, 0.15, 0.15); // increase to trust vsion // measurements less private boolean robotCentric; @@ -277,7 +277,7 @@ public double alignToSpeaker() { Telemetry.putNumber("swerve", "Desired Omega", refAngleFieldRel.getRadians()); Telemetry.putNumber("swerve", "Current Omega", getPoseEstimatorPose2d().getRotation().getRadians()); - + // return 0; return omegaSpeakerController.calculate(getPoseEstimatorPose2d().getRotation().getRadians(), refAngleFieldRel.getRadians()); } diff --git a/src/main/java/frc/robot/util/Telemetry.java b/src/main/java/frc/robot/util/Telemetry.java index 4dda11a..117583f 100644 --- a/src/main/java/frc/robot/util/Telemetry.java +++ b/src/main/java/frc/robot/util/Telemetry.java @@ -101,7 +101,7 @@ public void autons(String entry, String list) { public void odometry() { NetworkTableEntry rotation = _odometry.getEntry("Rotation"); - rotation.setDouble(drive.getRotation().getRadians()); + rotation.setDouble(drive.getRotation().getDegrees()); } public void debug() { diff --git a/src/main/java/frc/robot/util/limelight.vpr b/src/main/java/frc/robot/util/limelight.vpr index a85c56a..88db7e6 100644 --- a/src/main/java/frc/robot/util/limelight.vpr +++ b/src/main/java/frc/robot/util/limelight.vpr @@ -3,7 +3,7 @@ area_min:0.0017850625000000004 area_similarity:0 aspect_max:20.000000 aspect_min:0.000000 -black_level:6 +black_level:5 blue_balance:1558 botfloorsnap:0 botlength:0.7112 @@ -33,7 +33,7 @@ dilation_steps:0 direction_filter:0 dual_close_sort_origin:0 erosion_steps:0 -exposure:327 +exposure:473 fiducial_decoder_strictness:strict fiducial_denoise:0.000000 fiducial_idfilters: @@ -60,8 +60,8 @@ multigroup_max:7 multigroup_min:1 multigroup_rejector:0 pipeline_led_enabled:1 -pipeline_led_power:40 -pipeline_res:1 +pipeline_led_power:60 +pipeline_res:0 pipeline_type:3 red_balance:1303 roi_x:0.000000 From 6ddd5fe4b123358451f54c30504e8ea6ca8b1188 Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Tue, 19 Mar 2024 16:27:36 -0700 Subject: [PATCH 15/31] SAC changes --- src/main/java/frc/robot/OI.java | 16 ++--- src/main/java/frc/robot/RobotMap.java | 18 ++--- src/main/java/frc/robot/auton/Autons.java | 5 ++ .../robot/auton/SwervePositionController.java | 2 +- .../frc/robot/commands/CommandGroups.java | 4 +- .../commands/elevator/ElevatorManual.java | 56 ++++++++-------- ...OuttakeNote.java => OuttakeStuckNote.java} | 12 ++-- .../java/frc/robot/subsystems/Intake.java | 2 +- .../robot/subsystems/swerve/Drivetrain.java | 53 +++++++++++++-- .../robot/subsystems/swerve/SwerveModule.java | 3 + src/main/java/frc/robot/util/Flip.java | 4 +- src/main/java/frc/robot/util/Limelight.java | 66 ++++++++++++++----- src/main/java/frc/robot/util/Telemetry.java | 4 ++ src/main/java/frc/robot/util/limelight.vpr | 4 +- 14 files changed, 166 insertions(+), 83 deletions(-) rename src/main/java/frc/robot/commands/intake/{OuttakeNote.java => OuttakeStuckNote.java} (57%) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index f7ce903..dab5d14 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -3,7 +3,6 @@ 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.util.Units; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.RobotMap.Pivot.Goal; @@ -11,13 +10,9 @@ import frc.robot.commands.drivetrain.AlignToStage; //import frc.robot.commands.CommandGroups; // import frc.robot.commands.drivetrain.AlignToStage; -import frc.robot.commands.elevator.ElevatorManual; +// import frc.robot.commands.elevator.ElevatorManual; import frc.robot.commands.elevator.MoveToPosition; -import frc.robot.commands.elevator.ZeroElevator; -import frc.robot.commands.indexer.IndexToShooter; -import frc.robot.commands.intake.IntakeNote; -import frc.robot.commands.intake.OuttakeNote; -import frc.robot.commands.intake.ZeroIntake; +import frc.robot.commands.intake.OuttakeStuckNote; import frc.robot.commands.pivot.PivotToAngle; import frc.robot.commands.pivot.ZeroPivot; import frc.robot.commands.shooter.MoveNoteToShooter; @@ -61,8 +56,8 @@ private void initBindings() { driver.getButtonB().whileFalse(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0))); // driver.getButtonA().onTrue(new AlignToStage("left")); - driver.getButtonY().onTrue(new ElevatorManual(RobotMap.Elevator.EXTEND_SPEED)); - driver.getButtonA().onTrue(new ElevatorManual(-RobotMap.Elevator.EXTEND_SPEED)); + driver.getButtonY().onTrue(new MoveToPosition(RobotMap.Elevator.STAGE_HEIGHT)); + driver.getButtonA().onTrue(new MoveToPosition(0)); driver.getButtonStart().onTrue(new InstantCommand(() -> { Drivetrain.getInstance().toggleRobotCentric(); @@ -70,11 +65,10 @@ private void initBindings() { driver.getButtonX().onTrue(new InstantCommand( () -> Drivetrain.getInstance().setPose(new Pose2d(1.28, 5.41, Rotation2d.fromDegrees(180))))); - operator.getDownDPadButton().onTrue(CommandGroups.getFullZeroCommand()); + operator.getLeftBumper().onTrue(CommandGroups.getFullZeroCommand()); operator.getButtonY().whileTrue(new MoveToPosition(RobotMap.Elevator.STAGE_HEIGHT * 0.95)); operator.getRightBumper().onTrue(CommandGroups.getFullIntakeCommand()); - operator.getLeftBumper().whileTrue(new OuttakeNote()); //TESTING diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 6d6be5d..ecb7705 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -164,7 +164,7 @@ public static final class Drivetrain { } public static final class Shooter { - public static final int MASTER_ID = 3; + public static final int MASTER_ID = 38; public static final int FOLLOWER_ID = 6; public static final int INDEXER_ID = 4; @@ -202,8 +202,8 @@ public static final class Pivot { public static final double ZERO_SPEED = -0.2; - public static final double PIVOT_kP = 105; - public static final double PIVOT_kG = 0.4; + public static final double PIVOT_kP = 100; + public static final double PIVOT_kG = 0.40; public static final double PIVOT_kS = 0.10574; public static final double SUB_ANGLE = 25.0; @@ -281,15 +281,15 @@ public static final class Intake { } public static final class Indexer { - public static final int MASTER_ID = 5; + public static final int MASTER_ID = 43; public static final InvertedValue MASTER_INVERT = InvertedValue.CounterClockwise_Positive; - public static final double INDEXING_SPEED = 1.0; + public static final double INDEXING_SPEED = 0.795; - public static final int CURRENT_LIMIT = 30; - public static final int CURRENT_LIMIT_THRESHOLD = 40; - public static final double CURRENT_LIMIT_TIME = 0.1; + public static final int CURRENT_LIMIT = 50; + public static final int CURRENT_LIMIT_THRESHOLD = 70; + public static final double CURRENT_LIMIT_TIME = 0.2; } @@ -305,7 +305,7 @@ public static final class Camera { public static final int[] ID_STAGE_BLUE = {14, 15, 16}; public static final int[] ID_STAGE_RED = {11, 12, 13}; - public static final double MAX_ERROR_VISION_POSE = 1.0; // meters + public static final double MAX_ERROR_VISION_POSE = 5.8; // meters } } \ No newline at end of file diff --git a/src/main/java/frc/robot/auton/Autons.java b/src/main/java/frc/robot/auton/Autons.java index 2eb8fc1..d8ca452 100644 --- a/src/main/java/frc/robot/auton/Autons.java +++ b/src/main/java/frc/robot/auton/Autons.java @@ -3,7 +3,10 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.RobotMap; import frc.robot.commands.CommandGroups; +import frc.robot.commands.elevator.MoveToPosition; +import frc.robot.subsystems.Elevator; import frc.robot.subsystems.swerve.Drivetrain; // import frc.robot.commands.CommandGroups; // import frc.robot.commands.indexer.IndexToShooter; @@ -42,6 +45,8 @@ public class Autons public static final SequentialCommandGroup oneNote = new SequentialCommandGroup( CommandGroups.getFullZeroCommand(), CommandGroups.getFullShootSpeaker(), + new MoveToPosition(RobotMap.Elevator.STAGE_HEIGHT), + new MoveToPosition(0), new SwervePositionController(Trajectories.note1_one, () -> Rotation2d.fromDegrees(180), false) ); diff --git a/src/main/java/frc/robot/auton/SwervePositionController.java b/src/main/java/frc/robot/auton/SwervePositionController.java index f26c891..2b53098 100644 --- a/src/main/java/frc/robot/auton/SwervePositionController.java +++ b/src/main/java/frc/robot/auton/SwervePositionController.java @@ -76,7 +76,7 @@ public void execute() { double yFF = desiredState.velocityMetersPerSecond * desiredState.poseMeters.getRotation().getSin(); // meters per second double thetaFF = MathUtil.clamp((alignToSpeaker) ? -Drivetrain.getInstance().alignToSpeaker() - : thetaController.calculate(currentRotation.getRotations(), referenceAngle.getRotations()), + : -thetaController.calculate(currentRotation.getRotations(), referenceAngle.getRotations()), -clampAdd, clampAdd); // radians per second /** diff --git a/src/main/java/frc/robot/commands/CommandGroups.java b/src/main/java/frc/robot/commands/CommandGroups.java index 16054b3..c51da55 100644 --- a/src/main/java/frc/robot/commands/CommandGroups.java +++ b/src/main/java/frc/robot/commands/CommandGroups.java @@ -33,8 +33,8 @@ public static Command getFullShootSpeaker() { } public static Command getFullShootAmp() { - return new RevShooter(RobotMap.Shooter.Goal.AMP) - .alongWith(new PivotToAngle(RobotMap.Pivot.Goal.AMP)) + return new PivotToAngle(RobotMap.Pivot.Goal.AMP) + .andThen(new RevShooter(RobotMap.Shooter.Goal.AMP)) .andThen(new ShootNote()).andThen(new ZeroPivot()); } diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorManual.java b/src/main/java/frc/robot/commands/elevator/ElevatorManual.java index e41a6ce..ce4571d 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorManual.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorManual.java @@ -1,34 +1,34 @@ -package frc.robot.commands.elevator; +// package frc.robot.commands.elevator; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.RobotMap; -import frc.robot.subsystems.Elevator; +// import edu.wpi.first.wpilibj2.command.Command; +// import frc.robot.RobotMap; +// import frc.robot.subsystems.Elevator; -public class ElevatorManual extends Command{ - private double power; +// public class ElevatorManual extends Command{ +// private double power; - public ElevatorManual(double power) { - this.power = power; - addRequirements(Elevator.getInstance()); - } +// public ElevatorManual(double power) { +// this.power = power; +// addRequirements(Elevator.getInstance()); +// } - @Override - public void execute() { - Elevator.getInstance().setElevatorPower(power); - } +// @Override +// public void execute() { +// Elevator.getInstance().setElevatorPower(power); +// } - public boolean isFinished() { - if (power > 0) - return Elevator.getInstance().isAtTop(); - else - return Elevator.getInstance().isStalling(); - } +// public boolean isFinished() { +// if (power > 0) +// return Elevator.getInstance().isAtTop(); +// else +// return Elevator.getInstance().isStalling(); +// } - @Override - public void end(boolean interrupted) { - if (power > 0) - Elevator.getInstance().setElevatorPower(RobotMap.Elevator.ELEVATOR_kG); - else - Elevator.getInstance().setElevatorPower(0); - } -} +// @Override +// public void end(boolean interrupted) { +// if (power > 0) +// Elevator.getInstance().setElevatorPower(RobotMap.Elevator.ELEVATOR_kG); +// else +// Elevator.getInstance().setElevatorPower(0); +// } +// } diff --git a/src/main/java/frc/robot/commands/intake/OuttakeNote.java b/src/main/java/frc/robot/commands/intake/OuttakeStuckNote.java similarity index 57% rename from src/main/java/frc/robot/commands/intake/OuttakeNote.java rename to src/main/java/frc/robot/commands/intake/OuttakeStuckNote.java index 0754fb7..bcc5a3e 100644 --- a/src/main/java/frc/robot/commands/intake/OuttakeNote.java +++ b/src/main/java/frc/robot/commands/intake/OuttakeStuckNote.java @@ -1,20 +1,24 @@ package frc.robot.commands.intake; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.OI; import frc.robot.RobotMap; import frc.robot.subsystems.Intake; -public class OuttakeNote extends Command { +public class OuttakeStuckNote extends Command { - public OuttakeNote() + public OuttakeStuckNote() { addRequirements(Intake.getInstance()); } @Override public void execute() { - Intake.getInstance().setDeployPos(0); - Intake.getInstance().setRollerPower(RobotMap.Intake.ROLLER_OUTAKE_SPEED); + if (OI.getInstance().getOperator().getRightTrigger() > 0.5) + { + // Intake.getInstance().setDeployPos(0); + Intake.getInstance().setRollerPower(RobotMap.Intake.ROLLER_OUTAKE_SPEED); + } } public boolean isFinished() { diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 0579ada..eadb8e3 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -57,7 +57,7 @@ public void setSensorPosition(double pos) { } public void setDeployPos(double rots) { - deployPositionPID.setReference(rots, ControlType.kPosition); + deployPositionPID.setReference(rots, ControlType.kPosition, RobotMap.PID.SLOT_INDEX, 2.5); } public boolean limitSwitchHit() { diff --git a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java index 1f4fcc6..b647be0 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java @@ -30,6 +30,7 @@ import edu.wpi.first.units.Velocity; import edu.wpi.first.units.Voltage; import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -136,6 +137,8 @@ private void initPigeon() { pigeonConfigs.MountPose.MountPoseRoll = 0; pigeonConfigs.MountPose.MountPoseYaw = 0; pigeonConfigs.Pigeon2Features.EnableCompass = false; + pigeon.getYaw().setUpdateFrequency(250); + pigeon.getConfigurator().apply(pigeonConfigs); pigeon.setYaw(0); } @@ -414,17 +417,55 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return _sysId.dynamic(direction); } + public Pose2d getLLPose2d() { + return Limelight.getBotPose2d(); + } + + public void updatePoseEstimatorWithVisionBotPose() { + double xyStds = 0.15; + double degStds = 0.15; + + if (Limelight.getBotPose2d().getX() == 0.0) { + return; + } + + double poseDifference = poseEstimator.getEstimatedPosition().getTranslation() + .getDistance(Limelight.getBotPose2d().getTranslation()); + + if (Limelight.hasTargets()) { + if (Limelight.getNumTargets() >= 2) { + xyStds = 0.5; + degStds = 6; + } + else if (Limelight.getBestTargetArea() > 0.8 && poseDifference < 0.5) { + xyStds = 1.0; + degStds = 12; + } + else if (Limelight.getBestTargetArea() > 0.1 && poseDifference < 0.3) { + xyStds = 2.0; + degStds = 30; + } + else { + return; + } + } + + poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds))); + poseEstimator.addVisionMeasurement(Limelight.getBotPose2d(), Limelight.getTimestamp()); + } + @Override public void periodic() { updatePose(); + updatePoseEstimatorWithVisionBotPose(); SmartDashboard.putData(omegaAmpController); - if (Limelight.hasTargets()) { - Pose2d visionBot = Limelight.getBotPose2d(); - if (Limelight.isPoseValid(visionBot, getPoseEstimatorPose2d())) { - poseEstimator.addVisionMeasurement(visionBot, Limelight.getTimestamp()); - } - } + // if (Limelight.hasTargets()) { + // Pose2d visionBot = Limelight.getBotPose2d(); + // if (Limelight.isPoseValid()) { + // poseEstimator.addVisionMeasurement(visionBot, Limelight.getTimestamp()); + // } + // } } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index 4e93d44..859ee0a 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -86,6 +86,7 @@ private void configTranslation() { transConfig.Slot0.kI = RobotMap.SwerveModule.TRANSLATION_kI; transConfig.Slot0.kD = RobotMap.SwerveModule.TRANSLATION_kD; + translation.getPosition().setUpdateFrequency(250); translation.getConfigurator().apply(transConfig); } @@ -110,6 +111,8 @@ private void configRotation() { rotConfig.Slot0.kI = RobotMap.SwerveModule.ROTATION_kI; rotConfig.Slot0.kD = RobotMap.SwerveModule.ROTATION_kD; + rotation.getPosition().setUpdateFrequency(250); + rotation.getConfigurator().apply(rotConfig); } diff --git a/src/main/java/frc/robot/util/Flip.java b/src/main/java/frc/robot/util/Flip.java index d14973c..2def527 100644 --- a/src/main/java/frc/robot/util/Flip.java +++ b/src/main/java/frc/robot/util/Flip.java @@ -61,6 +61,8 @@ public static Trajectory.State apply(Trajectory.State state) { * field flipped along y-axis */ public static boolean isFlipped() { - return DriverStation.getAlliance().get() == Alliance.Red; + if (DriverStation.getAlliance().isPresent()) + return DriverStation.getAlliance().get() == Alliance.Red; + return false; } } diff --git a/src/main/java/frc/robot/util/Limelight.java b/src/main/java/frc/robot/util/Limelight.java index 5b01b0c..fa131e4 100644 --- a/src/main/java/frc/robot/util/Limelight.java +++ b/src/main/java/frc/robot/util/Limelight.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; @@ -19,9 +20,18 @@ public static Pose2d getBotPose2d() { return toPose2D(getBotPoseVal()); } - public static boolean isPoseValid(Pose2d botPose, Pose2d visionBot) { - return visionBot.getTranslation().getDistance(botPose.getTranslation()) < RobotMap.Camera.MAX_ERROR_VISION_POSE - && getBotPoseVal()[6] / 1000.0 <= 1; + public static double getBestTargetArea() { + return TABLE.getEntry("ta").getDouble(0.0); + } + + public static boolean isPoseValid() { + double[] targetPose = TABLE.getEntry("targetpose_robotspace").getDoubleArray(new double[6]); + if (!hasTargets() || targetPose.length <= 1) { + return false; + } + double x = targetPose[0]; + double y = targetPose[1]; + return Math.sqrt((x * x) + (y * y)) > RobotMap.Camera.MAX_ERROR_VISION_POSE; } public static double getTimestamp() { @@ -29,12 +39,12 @@ public static double getTimestamp() { } public static boolean hasTargets() { - return MathUtil.compareDouble(TABLE.getEntry("tv").getDouble(0.0), 1.0); + return getApriltagId() > -1; } public static boolean atAmp() { if (hasTargets()) { - if (DriverStation.getAlliance().get() == Alliance.Red) { + if (Flip.isFlipped()) { return MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_AMP_RED); } else { return MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_AMP_BLUE); @@ -45,28 +55,48 @@ public static boolean atAmp() { public static boolean atSpeaker() { if (hasTargets()) { - if (DriverStation.getAlliance().get() == Alliance.Red) { - return MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_SPEAKER_RED[0]) || MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_SPEAKER_RED[1]); - } else { - return MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_SPEAKER_BLUE[0]) || MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_SPEAKER_BLUE[1]); - } + + if (Flip.isFlipped()) { + return MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_SPEAKER_RED[0]) || MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_SPEAKER_RED[1]); + } else { + return MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_SPEAKER_BLUE[0]) || MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_SPEAKER_BLUE[1]); + } } return false; } public static boolean atStage() { if (hasTargets()) { - if (DriverStation.getAlliance().get() == Alliance.Red) { - return MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_RED[0]) || MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_RED[1]) || - MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_RED[2]); - } else { - return MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_BLUE[0]) || MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_BLUE[1]) || - MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_BLUE[2]); - } + if (Flip.isFlipped()) { + return MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_RED[0]) || MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_RED[1]) || + MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_RED[2]); + } else { + return MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_BLUE[0]) || MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_BLUE[1]) || + MathUtil.compareDouble(getApriltagId(), RobotMap.Camera.ID_STAGE_BLUE[2]); + } + } return false; } + + public static int getNumTargets() { + return countStringOccurrences(NetworkTableInstance.getDefault().getTable(LIMELIGHT_TABLE_KEY).getEntry("json").getString(""), "pts"); + } + + public static int countStringOccurrences(String str, String substr) { + int occ = 0; + for (int i = 0; i < str.length() - substr.length() + 1; i++) { + if (str.substring(i, i + substr.length()).equals(substr)) { + occ++; + } + } + + return occ; + } + + + /* entries[0] = forward; * entries[1] = side; * entries[2] = up; @@ -94,7 +124,7 @@ public static double getTy() { return TABLE.getEntry("ty").getDouble(0.0); } - private static double[] getBotPoseVal() { + public static double[] getBotPoseVal() { return TABLE.getEntry("botpose_wpiblue").getDoubleArray(new double[6]); } diff --git a/src/main/java/frc/robot/util/Telemetry.java b/src/main/java/frc/robot/util/Telemetry.java index 117583f..1f41f7c 100644 --- a/src/main/java/frc/robot/util/Telemetry.java +++ b/src/main/java/frc/robot/util/Telemetry.java @@ -102,6 +102,9 @@ public void autons(String entry, String list) { public void odometry() { NetworkTableEntry rotation = _odometry.getEntry("Rotation"); rotation.setDouble(drive.getRotation().getDegrees()); + + NetworkTableEntry llPose = _drive.getEntry("llPose"); + llPose.setDoubleArray(new double[]{drive.getLLPose2d().getX(), drive.getLLPose2d().getY(), drive.getLLPose2d().getRotation().getDegrees()}); } public void debug() { @@ -261,6 +264,7 @@ public void vision() { NetworkTableEntry distance = _limelight.getEntry("Distance To Speaker"); distance.setDouble(Drivetrain.getInstance().getDistanceToSpeaker()); + } public static void putModule(int id, String entry, double number) { diff --git a/src/main/java/frc/robot/util/limelight.vpr b/src/main/java/frc/robot/util/limelight.vpr index 88db7e6..4084969 100644 --- a/src/main/java/frc/robot/util/limelight.vpr +++ b/src/main/java/frc/robot/util/limelight.vpr @@ -33,7 +33,7 @@ dilation_steps:0 direction_filter:0 dual_close_sort_origin:0 erosion_steps:0 -exposure:473 +exposure:582 fiducial_decoder_strictness:strict fiducial_denoise:0.000000 fiducial_idfilters: @@ -61,7 +61,7 @@ multigroup_min:1 multigroup_rejector:0 pipeline_led_enabled:1 pipeline_led_power:60 -pipeline_res:0 +pipeline_res:2 pipeline_type:3 red_balance:1303 roi_x:0.000000 From 8929b471a90c05a91a29e9c074e614bccc027e75 Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Wed, 20 Mar 2024 18:27:56 -0700 Subject: [PATCH 16/31] Correct odometry day 1 --- src/main/java/frc/robot/OI.java | 8 +- src/main/java/frc/robot/Robot.java | 6 +- src/main/java/frc/robot/RobotMap.java | 14 +- .../commands/drivetrain/SwerveManual.java | 2 +- src/main/java/frc/robot/subsystems/Pivot.java | 2 +- .../robot/subsystems/swerve/Drivetrain.java | 32 ++- .../robot/subsystems/swerve/SwerveModule.java | 10 +- src/main/java/frc/robot/util/Limelight.java | 51 +++-- src/main/java/frc/robot/util/Telemetry.java | 185 ++++++++++-------- src/main/java/frc/robot/util/limelight.vpr | 29 +-- 10 files changed, 189 insertions(+), 150 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index dab5d14..e72b08d 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -3,6 +3,7 @@ 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.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.RobotMap.Pivot.Goal; @@ -46,8 +47,11 @@ public XboxGamepad getOperator() { } private void initBindings() { - driver.getLeftBumper().onTrue(CommandGroups.getFullShootAmp()); - driver.getRightBumper().onTrue(CommandGroups.getFullShootSpeaker()); + if (RobotMap.FIRST_BOT) + { + driver.getLeftBumper().onTrue(CommandGroups.getFullShootAmp()); + driver.getRightBumper().onTrue(CommandGroups.getFullShootSpeaker()); + } driver.getUpDPadButton().onTrue(new PivotToAngle(Goal.SPEAKER)); driver.getDownDPadButton().onTrue(new PivotToAngle(Goal.SUB)); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 41d2685..90cff60 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -62,7 +62,7 @@ public void robotInit() { // DriverStation.startDataLog(DataLogManager.getLog()); SmartDashboard.putData(RobotMap.Field.FIELD); - Limelight.setCameraPose(RobotMap.Camera.FORWARD, RobotMap.Camera.UP, RobotMap.Camera.PITCH); + // Limelight.setCameraPose(RobotMap.Camera.FORWARD, RobotMap.Camera.UP, RobotMap.Camera.PITCH); CommandScheduler.getInstance().setDefaultCommand(Drivetrain.getInstance(), new SwerveManual()); @@ -97,8 +97,8 @@ public void robotPeriodic() { telemetry.publish(); - // NetworkTableInstance.().flushLocal(); - // NetworkTableInstance.getDefault().flush(); + NetworkTableInstance.getDefault().flushLocal(); + NetworkTableInstance.getDefault().flush(); } @Override diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index ecb7705..ef13062 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -2,6 +2,9 @@ import com.ctre.phoenix6.signals.InvertedValue; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Field2d; @@ -9,7 +12,7 @@ public class RobotMap { // Global Robot Constants - public static final boolean FIRST_BOT = true; + public static final boolean FIRST_BOT = false; public static final double MAX_VOLTAGE = 12; public static final double ROBOT_LOOP = 0.02; public static final String CAN_CHAIN = "rio"; @@ -50,7 +53,8 @@ public static final class SwerveModule { public static final int[] TRANSLATION_IDS = {22, 5, 28, 10}; // translation motors inverted - public static final InvertedValue[] TRANSLATION_INVERTS = {InvertedValue.CounterClockwise_Positive, InvertedValue.CounterClockwise_Positive, InvertedValue.CounterClockwise_Positive, InvertedValue.CounterClockwise_Positive}; + public static final InvertedValue[] TRANSLATION_INVERTS = (FIRST_BOT) ? new InvertedValue[]{InvertedValue.CounterClockwise_Positive, InvertedValue.CounterClockwise_Positive, InvertedValue.CounterClockwise_Positive, InvertedValue.CounterClockwise_Positive} + : new InvertedValue[]{InvertedValue.Clockwise_Positive, InvertedValue.Clockwise_Positive, InvertedValue.Clockwise_Positive, InvertedValue.Clockwise_Positive}; // ids for rotation motors public static final int[] ROTATION_IDS = {15, 18, 27, 25}; @@ -63,7 +67,7 @@ public static final class SwerveModule { // offsets of cancoders of each swerve module (in rotations) public static final double[] CAN_CODER_OFFSETS = (FIRST_BOT) ? new double[]{-0.155518, -0.069092, -0.360596-0.299805, -0.402588-0.193848} - : new double[]{0.403320, 0.367188, 0.099609, -0.030518}; + : new double[]{-0.141357+0.5, -0.115479+0.5, 0.099609, -0.030518}; // current limit constants for translation motors public static final double TRANS_CURRENT_LIMIT = 40; public static final double TRANS_THRESHOLD_CURRENT = 55; @@ -298,6 +302,8 @@ public static final class Camera { public static final double UP = Units.inchesToMeters(21.47); // meters public static final double PITCH = -30.494; // degrees + public static final Pose3d CAMERA_POSE_ROBOT_SPACE = new Pose3d(FORWARD, 0, UP, new Rotation3d(0, PITCH, 0)); + public static final int[] ID_SPEAKER_BLUE = {7, 8}; public static final int[] ID_SPEAKER_RED = {3, 4}; public static final int ID_AMP_BLUE = 6; @@ -305,7 +311,7 @@ public static final class Camera { public static final int[] ID_STAGE_BLUE = {14, 15, 16}; public static final int[] ID_STAGE_RED = {11, 12, 13}; - public static final double MAX_ERROR_VISION_POSE = 5.8; // meters + public static final double MAX_ERROR_VISION_POSE = 5; // meters } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java b/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java index eac6198..a0b316f 100644 --- a/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java +++ b/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java @@ -99,7 +99,7 @@ public void execute() { Drivetrain.getInstance() .setAngleAndDrive( ChassisSpeeds.fromFieldRelativeSpeeds( - vx, vy, -omega, Rotation2d.fromDegrees(180))); + vx, vy, -omega, Rotation2d.fromDegrees(0))); else Drivetrain.getInstance() .setAngleAndDrive( diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 25b25f0..ede4050 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -34,7 +34,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.RobotMap; import frc.robot.util.MathUtil; -import frc.robot.util.Telemetry; +// import frc.robot.util.Telemetry; public class Pivot extends SubsystemBase { private static Pivot instance; diff --git a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java index b647be0..a4548e2 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java @@ -38,7 +38,7 @@ import frc.robot.RobotMap; import frc.robot.util.Flip; import frc.robot.util.Limelight; -import frc.robot.util.Telemetry; +// import frc.robot.util.Telemetry; public class Drivetrain extends SubsystemBase { private static Drivetrain instance; @@ -68,9 +68,9 @@ public class Drivetrain extends SubsystemBase { private static PIDController omegaAmpController = new PIDController(RobotMap.Drivetrain.OMEGA_AMP_KP, 0, 0); // Standard deviations of pose estimate (x, y, heading) - private static Matrix stateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1); // increase to trust encoder (state) + private static Matrix stateStdDevs = VecBuilder.fill(0.15, 0.15, 0.1); // increase to trust encoder (state) // measurements less - private static Matrix visionStdDevs = VecBuilder.fill(0.15, 0.15, 0.15); // increase to trust vsion + private static Matrix visionStdDevs = VecBuilder.fill(0.2, 0.2, 0.2); // increase to trust vsion // measurements less private boolean robotCentric; @@ -252,7 +252,7 @@ public void setPose(Pose2d pose) { swerveModules[2].zeroTranslation(); swerveModules[3].zeroTranslation(); setYaw(pose.getRotation().getDegrees()); - poseEstimator.resetPosition(pose.getRotation(), getModulePositions(), pose); + poseEstimator.resetPosition(getRotation(), getModulePositions(), pose); } /** @@ -278,8 +278,8 @@ public double alignToSpeaker() { Rotation2d refAngleFieldRel = Flip.apply(RobotMap.Field.SPEAKER) .minus(getPoseEstimatorPose2d().getTranslation()).getAngle(); - Telemetry.putNumber("swerve", "Desired Omega", refAngleFieldRel.getRadians()); - Telemetry.putNumber("swerve", "Current Omega", getPoseEstimatorPose2d().getRotation().getRadians()); + // Telemetry.putNumber("swerve", "Desired Omega", refAngleFieldRel.getRadians()); + // Telemetry.putNumber("swerve", "Current Omega", getPoseEstimatorPose2d().getRotation().getRadians()); // return 0; return omegaSpeakerController.calculate(getPoseEstimatorPose2d().getRotation().getRadians(), refAngleFieldRel.getRadians()); @@ -417,8 +417,8 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return _sysId.dynamic(direction); } - public Pose2d getLLPose2d() { - return Limelight.getBotPose2d(); + public boolean isPoseNear() { + return Limelight.isPoseNear(getPoseEstimatorPose2d(), Limelight.getBotPose2d()); } public void updatePoseEstimatorWithVisionBotPose() { @@ -457,15 +457,13 @@ else if (Limelight.getBestTargetArea() > 0.1 && poseDifference < 0.3) { @Override public void periodic() { updatePose(); - updatePoseEstimatorWithVisionBotPose(); + // updatePoseEstimatorWithVisionBotPose(); - SmartDashboard.putData(omegaAmpController); - - // if (Limelight.hasTargets()) { - // Pose2d visionBot = Limelight.getBotPose2d(); - // if (Limelight.isPoseValid()) { - // poseEstimator.addVisionMeasurement(visionBot, Limelight.getTimestamp()); - // } - // } + if (Limelight.hasTargets()) { + Pose2d visionBot = Limelight.getBotPose2d(); + if (Limelight.isPoseValid() && Limelight.isPoseNear(getPoseEstimatorPose2d(), visionBot)) { + poseEstimator.addVisionMeasurement(visionBot, Limelight.getTimestamp()); + } + } } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index 859ee0a..b00faac 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -16,7 +16,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import frc.robot.RobotMap; -import frc.robot.util.Telemetry; +// import frc.robot.util.Telemetry; public class SwerveModule { //motors on the swerve modules @@ -139,11 +139,11 @@ public void setAngleAndDrive(SwerveModuleState state) { driveVelocity.FeedForward = feedforward.calculate(state.speedMetersPerSecond); translation.setControl(driveVelocity); - Telemetry.putModule(ID, "Desired Velocity", state.speedMetersPerSecond); - Telemetry.putModule(ID, "Current Velocity", getSpeed()); + // Telemetry.putModule(ID, "Desired Velocity", state.speedMetersPerSecond); + // Telemetry.putModule(ID, "Current Velocity", getSpeed()); - Telemetry.putModule(ID, "Desired Angle", state.angle.getDegrees()); - Telemetry.putModule(ID, "Current Angle", getAngle()); + // Telemetry.putModule(ID, "Desired Angle", state.angle.getDegrees()); + // Telemetry.putModule(ID, "Current Angle", getAngle()); // SmartDashboard.putNumber("Desired Velocity " + ID, state.speedMetersPerSecond); // SmartDashboard.putNumber("Current Velocity " + ID, getSpeed()); diff --git a/src/main/java/frc/robot/util/Limelight.java b/src/main/java/frc/robot/util/Limelight.java index fa131e4..c2feb83 100644 --- a/src/main/java/frc/robot/util/Limelight.java +++ b/src/main/java/frc/robot/util/Limelight.java @@ -4,38 +4,46 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.RobotMap; public final class Limelight { + private static NetworkTableInstance table; public static final String LIMELIGHT_TABLE_KEY = "limelight"; - public static final NetworkTable TABLE = NetworkTableInstance.getDefault().getTable(LIMELIGHT_TABLE_KEY); public static Pose2d getBotPose2d() { return toPose2D(getBotPoseVal()); } public static double getBestTargetArea() { - return TABLE.getEntry("ta").getDouble(0.0); + return getValue("ta").getDouble(0.0); } public static boolean isPoseValid() { - double[] targetPose = TABLE.getEntry("targetpose_robotspace").getDoubleArray(new double[6]); - if (!hasTargets() || targetPose.length <= 1) { - return false; - } + return getDistanceToTag() <= RobotMap.Camera.MAX_ERROR_VISION_POSE; + } + + public static double getDistanceToTag() { + double[] targetPose = getValue("targetpose_robotspace").getDoubleArray(new double[6]); double x = targetPose[0]; double y = targetPose[1]; - return Math.sqrt((x * x) + (y * y)) > RobotMap.Camera.MAX_ERROR_VISION_POSE; + return Math.sqrt((x * x) + (y * y)); + } + + public static boolean isPoseNear(Pose2d pose, Pose2d visionPose) { + return getDistanceBetweenPose(pose, visionPose) < 1.0 + && MathUtil.compareDouble(visionPose.getTranslation().getNorm(), 0.0); + + } + + public static double getDistanceBetweenPose(Pose2d pose, Pose2d visionPose) { + return pose.getTranslation().getDistance(visionPose.getTranslation()); } public static double getTimestamp() { - return Timer.getFPGATimestamp() - getBotPoseVal()[6] / 1000.0; + return Timer.getFPGATimestamp() - getBotPoseVal()[5] / 1000.0; } public static boolean hasTargets() { @@ -104,28 +112,28 @@ public static int countStringOccurrences(String str, String substr) { * entries[4] = pitch; * entries[5] = yaw; */ public static void setCameraPose(double forward, double up, double pitch) { - TABLE.getEntry("camerapose_robotspace_set").setDoubleArray(new double[]{forward, 0, up, 0, pitch, 0}); + getValue("camerapose_robotspace").setDoubleArray(new double[]{forward, 0, up, 0, pitch, 0}); } public static void setPipeline(double idx) { - TABLE.getEntry("pipeline").setDouble(idx); + getValue("pipeline").setDouble(idx); } public static double getApriltagId() { - return TABLE.getEntry("tid").getDouble(0.0); + return getValue("tid").getDouble(0.0); } public static double getTx() { - return TABLE.getEntry("tx").getDouble(0.0); + return getValue("tx").getDouble(0.0); } public static double getTy() { - return TABLE.getEntry("ty").getDouble(0.0); + return getValue("ty").getDouble(0.0); } public static double[] getBotPoseVal() { - return TABLE.getEntry("botpose_wpiblue").getDoubleArray(new double[6]); + return getValue("botpose_wpiblue").getDoubleArray(new double[7]); } private static Pose2d toPose2D(double[] inData){ @@ -139,4 +147,13 @@ private static Pose2d toPose2D(double[] inData){ return new Pose2d(tran2d, r2d); } + public static NetworkTableEntry getValue(String key) { + if (table == null) { + table = NetworkTableInstance.getDefault(); + table.getTable(LIMELIGHT_TABLE_KEY).getEntry("pipeline").setNumber(0); + } + + return table.getTable(LIMELIGHT_TABLE_KEY).getEntry(key); + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/Telemetry.java b/src/main/java/frc/robot/util/Telemetry.java index 1f41f7c..ae3daca 100644 --- a/src/main/java/frc/robot/util/Telemetry.java +++ b/src/main/java/frc/robot/util/Telemetry.java @@ -1,5 +1,6 @@ package frc.robot.util; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; @@ -100,144 +101,156 @@ public void autons(String entry, String list) { } public void odometry() { - NetworkTableEntry rotation = _odometry.getEntry("Rotation"); - rotation.setDouble(drive.getRotation().getDegrees()); + // NetworkTableEntry rotation = _odometry.getEntry("Rotation"); + // rotation.setDouble(drive.getRotation().getDegrees()); + Pose2d visionPose = Limelight.getBotPose2d(); + NetworkTableEntry isPoseNear = _odometry.getEntry("Pose Nearness"); + isPoseNear.setBoolean(drive.isPoseNear()); - NetworkTableEntry llPose = _drive.getEntry("llPose"); - llPose.setDoubleArray(new double[]{drive.getLLPose2d().getX(), drive.getLLPose2d().getY(), drive.getLLPose2d().getRotation().getDegrees()}); + NetworkTableEntry isPoseValid = _odometry.getEntry("Pose Validity"); + isPoseValid.setBoolean(Limelight.isPoseValid()); + + NetworkTableEntry distanceToTag = _odometry.getEntry("Tag Distance"); + distanceToTag.setDouble(Limelight.getDistanceToTag()); + + NetworkTableEntry distanceBetweenPoses = _odometry.getEntry("Pose Distance"); + distanceBetweenPoses.setDouble(Limelight.getDistanceBetweenPose(drive.getPoseEstimatorPose2d(), visionPose)); + + NetworkTableEntry llPose = _odometry.getEntry("llPose"); + llPose.setDoubleArray(new double[]{visionPose.getX(), visionPose.getY(), visionPose.getRotation().getDegrees()}); } public void debug() { - NetworkTableEntry a_D = _driver.getEntry("Button A"); - a_D.setBoolean(oiDriver.getButtonAState()); + // NetworkTableEntry a_D = _driver.getEntry("Button A"); + // a_D.setBoolean(oiDriver.getButtonAState()); - NetworkTableEntry b_D = _driver.getEntry("Button B"); - b_D.setBoolean(oiDriver.getButtonBState()); + // NetworkTableEntry b_D = _driver.getEntry("Button B"); + // b_D.setBoolean(oiDriver.getButtonBState()); - NetworkTableEntry x_D = _driver.getEntry("Button X"); - x_D.setBoolean(oiDriver.getButtonXState()); + // NetworkTableEntry x_D = _driver.getEntry("Button X"); + // x_D.setBoolean(oiDriver.getButtonXState()); - NetworkTableEntry y_D = _driver.getEntry("Button Y"); - y_D.setBoolean(oiDriver.getButtonYState()); + // NetworkTableEntry y_D = _driver.getEntry("Button Y"); + // y_D.setBoolean(oiDriver.getButtonYState()); - NetworkTableEntry leftBumper_D = _driver.getEntry("Left Bumper"); - leftBumper_D.setBoolean(oiDriver.getLeftBumperState()); + // NetworkTableEntry leftBumper_D = _driver.getEntry("Left Bumper"); + // leftBumper_D.setBoolean(oiDriver.getLeftBumperState()); - NetworkTableEntry rightBumper_D = _driver.getEntry("Right Bumper"); - rightBumper_D.setBoolean(oiDriver.getRightBumperState()); + // NetworkTableEntry rightBumper_D = _driver.getEntry("Right Bumper"); + // rightBumper_D.setBoolean(oiDriver.getRightBumperState()); - NetworkTableEntry leftTrigger_D = _driver.getEntry("Left Trigger"); - leftTrigger_D.setDouble(oiDriver.getLeftTrigger()); + // NetworkTableEntry leftTrigger_D = _driver.getEntry("Left Trigger"); + // leftTrigger_D.setDouble(oiDriver.getLeftTrigger()); - NetworkTableEntry rightTrigger_D = _driver.getEntry("Right Trigger"); - rightTrigger_D.setDouble(oiDriver.getRightTrigger()); + // NetworkTableEntry rightTrigger_D = _driver.getEntry("Right Trigger"); + // rightTrigger_D.setDouble(oiDriver.getRightTrigger()); - NetworkTableEntry upD_D = _driver.getEntry("Up DPad"); - upD_D.setBoolean(oiDriver.getUpDPadButtonState()); + // NetworkTableEntry upD_D = _driver.getEntry("Up DPad"); + // upD_D.setBoolean(oiDriver.getUpDPadButtonState()); - NetworkTableEntry leftD_D = _driver.getEntry("Left DPad"); - leftD_D.setBoolean(oiDriver.getLeftDPadButtonState()); + // NetworkTableEntry leftD_D = _driver.getEntry("Left DPad"); + // leftD_D.setBoolean(oiDriver.getLeftDPadButtonState()); - NetworkTableEntry downD_D = _driver.getEntry("Down DPad"); - downD_D.setBoolean(oiDriver.getDownDPadButtonState()); + // NetworkTableEntry downD_D = _driver.getEntry("Down DPad"); + // downD_D.setBoolean(oiDriver.getDownDPadButtonState()); - NetworkTableEntry rightD_D = _driver.getEntry("Right DPad"); - rightD_D.setBoolean(oiDriver.getDownDPadButtonState()); + // NetworkTableEntry rightD_D = _driver.getEntry("Right DPad"); + // rightD_D.setBoolean(oiDriver.getDownDPadButtonState()); - NetworkTableEntry select_D = _driver.getEntry("Select"); - select_D.setBoolean(oiDriver.getButtonSelectState()); + // NetworkTableEntry select_D = _driver.getEntry("Select"); + // select_D.setBoolean(oiDriver.getButtonSelectState()); - NetworkTableEntry start_D = _driver.getEntry("Start"); - start_D.setBoolean(oiDriver.getButtonStartState()); + // NetworkTableEntry start_D = _driver.getEntry("Start"); + // start_D.setBoolean(oiDriver.getButtonStartState()); - NetworkTableEntry a = _operator.getEntry("Button A"); - a.setBoolean(oiOperator.getButtonAState()); + // NetworkTableEntry a = _operator.getEntry("Button A"); + // a.setBoolean(oiOperator.getButtonAState()); - NetworkTableEntry b = _operator.getEntry("Button B"); - b.setBoolean(oiOperator.getButtonBState()); + // NetworkTableEntry b = _operator.getEntry("Button B"); + // b.setBoolean(oiOperator.getButtonBState()); - NetworkTableEntry x = _operator.getEntry("Button X"); - x.setBoolean(oiOperator.getButtonXState()); + // NetworkTableEntry x = _operator.getEntry("Button X"); + // x.setBoolean(oiOperator.getButtonXState()); - NetworkTableEntry y = _operator.getEntry("Button Y"); - y.setBoolean(oiOperator.getButtonYState()); + // NetworkTableEntry y = _operator.getEntry("Button Y"); + // y.setBoolean(oiOperator.getButtonYState()); - NetworkTableEntry leftBumper = _operator.getEntry("Left Bumper"); - leftBumper.setBoolean(oiOperator.getLeftBumperState()); + // NetworkTableEntry leftBumper = _operator.getEntry("Left Bumper"); + // leftBumper.setBoolean(oiOperator.getLeftBumperState()); - NetworkTableEntry rightBumper = _operator.getEntry("Right Bumper"); - rightBumper.setBoolean(oiOperator.getRightBumperState()); + // NetworkTableEntry rightBumper = _operator.getEntry("Right Bumper"); + // rightBumper.setBoolean(oiOperator.getRightBumperState()); - NetworkTableEntry leftTrigger = _operator.getEntry("Left Trigger"); - leftTrigger.setDouble(oiOperator.getLeftTrigger()); + // NetworkTableEntry leftTrigger = _operator.getEntry("Left Trigger"); + // leftTrigger.setDouble(oiOperator.getLeftTrigger()); - NetworkTableEntry rightTrigger = _operator.getEntry("Right Trigger"); - rightTrigger.setDouble(oiOperator.getRightTrigger()); + // NetworkTableEntry rightTrigger = _operator.getEntry("Right Trigger"); + // rightTrigger.setDouble(oiOperator.getRightTrigger()); - NetworkTableEntry upD = _operator.getEntry("Up DPad"); - upD.setBoolean(oiOperator.getUpDPadButtonState()); + // NetworkTableEntry upD = _operator.getEntry("Up DPad"); + // upD.setBoolean(oiOperator.getUpDPadButtonState()); - NetworkTableEntry leftD = _operator.getEntry("Left DPad"); - leftD.setBoolean(oiOperator.getLeftDPadButtonState()); + // NetworkTableEntry leftD = _operator.getEntry("Left DPad"); + // leftD.setBoolean(oiOperator.getLeftDPadButtonState()); - NetworkTableEntry downD = _operator.getEntry("Down DPad"); - downD.setBoolean(oiOperator.getDownDPadButtonState()); + // NetworkTableEntry downD = _operator.getEntry("Down DPad"); + // downD.setBoolean(oiOperator.getDownDPadButtonState()); - NetworkTableEntry rightD = _operator.getEntry("Right DPad"); - rightD.setBoolean(oiOperator.getDownDPadButtonState()); + // NetworkTableEntry rightD = _operator.getEntry("Right DPad"); + // rightD.setBoolean(oiOperator.getDownDPadButtonState()); - NetworkTableEntry select = _operator.getEntry("Select"); - select.setBoolean(oiOperator.getButtonSelectState()); + // NetworkTableEntry select = _operator.getEntry("Select"); + // select.setBoolean(oiOperator.getButtonSelectState()); - NetworkTableEntry start = _operator.getEntry("Start"); - start.setBoolean(oiOperator.getButtonStartState()); + // NetworkTableEntry start = _operator.getEntry("Start"); + // start.setBoolean(oiOperator.getButtonStartState()); - NetworkTableEntry isRobotCentric = _debug.getEntry("Robot Centric"); - isRobotCentric.setBoolean(drive.robotCentric()); + // NetworkTableEntry isRobotCentric = _debug.getEntry("Robot Centric"); + // isRobotCentric.setBoolean(drive.robotCentric()); } public void shooter() { - NetworkTableEntry shooterIndexProxSensor = _shooter.getEntry("Shooter Index Occupied"); - shooterIndexProxSensor.setBoolean(shooter.shooterIndexerOccupied()); + // NetworkTableEntry shooterIndexProxSensor = _shooter.getEntry("Shooter Index Occupied"); + // shooterIndexProxSensor.setBoolean(shooter.shooterIndexerOccupied()); } public void elevator() { - NetworkTableEntry elevatorLimitSwitchHit = _elevator.getEntry("Elevator Limit Switch Hit"); - elevatorLimitSwitchHit.setBoolean(elevator.isLimitHit()); + // NetworkTableEntry elevatorLimitSwitchHit = _elevator.getEntry("Elevator Limit Switch Hit"); + // elevatorLimitSwitchHit.setBoolean(elevator.isLimitHit()); - NetworkTableEntry elevatorStalling = _elevator.getEntry("Elevator Stalling"); - elevatorStalling.setBoolean(elevator.isLimitHit()); + // NetworkTableEntry elevatorStalling = _elevator.getEntry("Elevator Stalling"); + // elevatorStalling.setBoolean(elevator.isLimitHit()); - NetworkTableEntry elevatorSensorPosition = _elevator.getEntry("Elevator Sensor Position"); - elevatorSensorPosition.setDouble(elevator.getPosition()); + // NetworkTableEntry elevatorSensorPosition = _elevator.getEntry("Elevator Sensor Position"); + // elevatorSensorPosition.setDouble(elevator.getPosition()); - NetworkTableEntry elevatorSensorVelocity = _elevator.getEntry("Sensor Velocity"); - elevatorSensorVelocity.setDouble(elevator.getVelocity()); + // NetworkTableEntry elevatorSensorVelocity = _elevator.getEntry("Sensor Velocity"); + // elevatorSensorVelocity.setDouble(elevator.getVelocity()); } public void intake() { - NetworkTableEntry intakeStalling = _intake.getEntry("Intake Stalling"); - intakeStalling.setBoolean(intake.isStalling()); + // NetworkTableEntry intakeStalling = _intake.getEntry("Intake Stalling"); + // intakeStalling.setBoolean(intake.isStalling()); } public void pivot() { - NetworkTableEntry pivotSensorPosition = _pivot.getEntry("Pivot Sensor Position"); - pivotSensorPosition.setDouble(pivot.getPosition()); + // NetworkTableEntry pivotSensorPosition = _pivot.getEntry("Pivot Sensor Position"); + // pivotSensorPosition.setDouble(pivot.getPosition()); - NetworkTableEntry pivotRef = _pivot.getEntry("Pivot Speaker Setpoint"); - pivotRef.setDouble(pivot.getPivotSetpoint(Drivetrain.getInstance().getDistanceToSpeaker())); + // NetworkTableEntry pivotRef = _pivot.getEntry("Pivot Speaker Setpoint"); + // pivotRef.setDouble(pivot.getPivotSetpoint(Drivetrain.getInstance().getDistanceToSpeaker())); - NetworkTableEntry pivotSensorVelocity = _pivot.getEntry("Pivot Sensor Velocity"); - pivotSensorVelocity.setDouble(pivot.getVelocity()); + // NetworkTableEntry pivotSensorVelocity = _pivot.getEntry("Pivot Sensor Velocity"); + // pivotSensorVelocity.setDouble(pivot.getVelocity()); - NetworkTableEntry pivotStalling = _pivot.getEntry("Pivot Stalling"); - pivotStalling.setBoolean(pivot.isStalling()); + // NetworkTableEntry pivotStalling = _pivot.getEntry("Pivot Stalling"); + // pivotStalling.setBoolean(pivot.isStalling()); - NetworkTableEntry pivotLimitSwitch = _pivot.getEntry("Pivot Limit Switch"); - pivotLimitSwitch.setBoolean(pivot.isLimitHit()); + // NetworkTableEntry pivotLimitSwitch = _pivot.getEntry("Pivot Limit Switch"); + // pivotLimitSwitch.setBoolean(pivot.isLimitHit()); - NetworkTableEntry pivotMasterCurrent = _pivot.getEntry("Pivot Master Motor Current"); - pivotMasterCurrent.setDouble(pivot.getMasterCurrent()); + // NetworkTableEntry pivotMasterCurrent = _pivot.getEntry("Pivot Master Motor Current"); + // pivotMasterCurrent.setDouble(pivot.getMasterCurrent()); // NetworkTableEntry setPivotAngle = _pivot.getEntry("Pivot Angle"); // setPivotAngle.setDouble(pivot.getPivotSetpoint(0)) diff --git a/src/main/java/frc/robot/util/limelight.vpr b/src/main/java/frc/robot/util/limelight.vpr index 4084969..2e92b6a 100644 --- a/src/main/java/frc/robot/util/limelight.vpr +++ b/src/main/java/frc/robot/util/limelight.vpr @@ -1,12 +1,13 @@ -area_max:100 -area_min:0.0017850625000000004 +area_max:0.0017850625000000004 +area_min:0 area_similarity:0 aspect_max:20.000000 aspect_min:0.000000 -black_level:5 +black_level:4 blue_balance:1558 -botfloorsnap:0 +botfloorsnap:1 botlength:0.7112 +bottype:swerve botwidth:0.7112 calibration_type:0 classifier_conf:0.100000 @@ -33,7 +34,7 @@ dilation_steps:0 direction_filter:0 dual_close_sort_origin:0 erosion_steps:0 -exposure:582 +exposure:302 fiducial_decoder_strictness:strict fiducial_denoise:0.000000 fiducial_idfilters: @@ -43,9 +44,9 @@ fiducial_qualitythreshold:2.0 fiducial_refine:1 fiducial_resdiv:2 fiducial_size:152.4 -fiducial_skip3d:1 +fiducial_skip3d:0 fiducial_type:aprilClassic36h11 -fiducial_vis_mode:3dtargposebotspace +fiducial_vis_mode:3dbotposefieldspace flicker_correction:0 force_convex:1 hue_max:85 @@ -55,22 +56,22 @@ image_source:0 img_to_show:0 intersection_filter:0 invert_hue:0 -lcgain:9.8 +lcgain:9.6 multigroup_max:7 multigroup_min:1 multigroup_rejector:0 -pipeline_led_enabled:1 -pipeline_led_power:60 -pipeline_res:2 +pipeline_led_enabled:0 +pipeline_led_power:0 +pipeline_res:3 pipeline_type:3 red_balance:1303 roi_x:0.000000 roi_y:0.000000 -rsf:0 -rspitch:0 +rsf:1.55 +rspitch:-30.494 rsroll:0 rss:0 -rsu:0 +rsu:0.545 rsyaw:0 sat_max:255 sat_min:70 From 64f8cecff3ce4d99713fd5843af410da8612da0b Mon Sep 17 00:00:00 2001 From: Benjamin Xia Date: Thu, 21 Mar 2024 16:56:29 -0700 Subject: [PATCH 17/31] limelight april tag poses --- src/main/java/frc/robot/util/Limelight.java | 80 +++++++++++++++++++-- 1 file changed, 76 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/util/Limelight.java b/src/main/java/frc/robot/util/Limelight.java index c2feb83..3144a86 100644 --- a/src/main/java/frc/robot/util/Limelight.java +++ b/src/main/java/frc/robot/util/Limelight.java @@ -8,6 +8,7 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Timer; import frc.robot.RobotMap; +import frc.robot.subsystems.swerve.Drivetrain; public final class Limelight { private static NetworkTableInstance table; @@ -25,11 +26,82 @@ public static boolean isPoseValid() { return getDistanceToTag() <= RobotMap.Camera.MAX_ERROR_VISION_POSE; } + // public static double getDistanceToTag() { - double[] targetPose = getValue("targetpose_robotspace").getDoubleArray(new double[6]); - double x = targetPose[0]; - double y = targetPose[1]; - return Math.sqrt((x * x) + (y * y)); + Pose2d targetPose = new Pose2d(); + if (MathUtil.compareDouble(getApriltagId(), 1)) + { + targetPose = new Pose2d(Units.inchesToMeters(593.68), Units.inchesToMeters(9.68), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 2)) + { + targetPose = new Pose2d(Units.inchesToMeters(637.21), Units.inchesToMeters(34.79), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 3)) + { + targetPose = new Pose2d(Units.inchesToMeters(652.73), Units.inchesToMeters(196.17), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 4)) + { + targetPose = new Pose2d(Units.inchesToMeters(652.73), Units.inchesToMeters(218.42), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 5)) + { + targetPose = new Pose2d(Units.inchesToMeters(578.77), Units.inchesToMeters(323.00), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 6)) + { + targetPose = new Pose2d(Units.inchesToMeters(72.5), Units.inchesToMeters(323.00), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 7)) + { + targetPose = new Pose2d(Units.inchesToMeters(-1.50), Units.inchesToMeters(218.42), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 8)) + { + targetPose = new Pose2d(Units.inchesToMeters(-1.50), Units.inchesToMeters(196.17), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 9)) + { + targetPose = new Pose2d(Units.inchesToMeters(14.02), Units.inchesToMeters(34.79), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 10)) + { + targetPose = new Pose2d(Units.inchesToMeters(57.54), Units.inchesToMeters(9.68), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 11)) + { + targetPose = new Pose2d(Units.inchesToMeters(468.69), Units.inchesToMeters(146.19), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 12)) + { + targetPose = new Pose2d(Units.inchesToMeters(468.69), Units.inchesToMeters(177.10), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 13)) + { + targetPose = new Pose2d(Units.inchesToMeters(441.74), Units.inchesToMeters(161.62), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 14)) + { + targetPose = new Pose2d(Units.inchesToMeters(209.48), Units.inchesToMeters(161.62), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 15)) + { + targetPose = new Pose2d(Units.inchesToMeters(182.73), Units.inchesToMeters(177.10), new Rotation2d()); + } + else if (MathUtil.compareDouble(getApriltagId(), 16)) + { + targetPose = new Pose2d(Units.inchesToMeters(182.73), Units.inchesToMeters(146.19), new Rotation2d()); + } + + if (Flip.isFlipped()) + { + targetPose = Flip.apply(targetPose); + } + + Pose2d robotPose = Drivetrain.getInstance().getPoseEstimatorPose2d(); + + return robotPose.getTranslation().getDistance(targetPose.getTranslation()); } public static boolean isPoseNear(Pose2d pose, Pose2d visionPose) { From 668842a93c277a46a79e9941735f71164801b5ee Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Thu, 21 Mar 2024 18:07:01 -0700 Subject: [PATCH 18/31] fix odometry day 2 --- .../robot/subsystems/swerve/Drivetrain.java | 46 ++++--------------- src/main/java/frc/robot/util/Limelight.java | 11 ++--- 2 files changed, 11 insertions(+), 46 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java index a4548e2..48b3743 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java @@ -68,7 +68,7 @@ public class Drivetrain extends SubsystemBase { private static PIDController omegaAmpController = new PIDController(RobotMap.Drivetrain.OMEGA_AMP_KP, 0, 0); // Standard deviations of pose estimate (x, y, heading) - private static Matrix stateStdDevs = VecBuilder.fill(0.15, 0.15, 0.1); // increase to trust encoder (state) + private static Matrix stateStdDevs = VecBuilder.fill(0.15, 0.15, 0.01); // increase to trust encoder (state) // measurements less private static Matrix visionStdDevs = VecBuilder.fill(0.2, 0.2, 0.2); // increase to trust vsion // measurements less @@ -421,48 +421,18 @@ public boolean isPoseNear() { return Limelight.isPoseNear(getPoseEstimatorPose2d(), Limelight.getBotPose2d()); } - public void updatePoseEstimatorWithVisionBotPose() { - double xyStds = 0.15; - double degStds = 0.15; - - if (Limelight.getBotPose2d().getX() == 0.0) { - return; - } - - double poseDifference = poseEstimator.getEstimatedPosition().getTranslation() - .getDistance(Limelight.getBotPose2d().getTranslation()); - - if (Limelight.hasTargets()) { - if (Limelight.getNumTargets() >= 2) { - xyStds = 0.5; - degStds = 6; - } - else if (Limelight.getBestTargetArea() > 0.8 && poseDifference < 0.5) { - xyStds = 1.0; - degStds = 12; - } - else if (Limelight.getBestTargetArea() > 0.1 && poseDifference < 0.3) { - xyStds = 2.0; - degStds = 30; - } - else { - return; - } - } - - poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds))); - poseEstimator.addVisionMeasurement(Limelight.getBotPose2d(), Limelight.getTimestamp()); - } - @Override public void periodic() { updatePose(); // updatePoseEstimatorWithVisionBotPose(); - if (Limelight.hasTargets()) { - Pose2d visionBot = Limelight.getBotPose2d(); - if (Limelight.isPoseValid() && Limelight.isPoseNear(getPoseEstimatorPose2d(), visionBot)) { - poseEstimator.addVisionMeasurement(visionBot, Limelight.getTimestamp()); + Pose2d visionBot = Limelight.getBotPose2d(); + if (Limelight.isPoseValid() && Limelight.isPoseNear(getPoseEstimatorPose2d(), visionBot)) { + double x = Limelight.getDistanceToTag(); + double stdXY = .773 * x; + double stdTheta = .025 + .277 * x; + if (Limelight.hasTargets() && !frc.robot.util.MathUtil.compareDouble(visionBot.getTranslation().getNorm(), 0)) { + poseEstimator.addVisionMeasurement(visionBot, Limelight.getTimestamp(), VecBuilder.fill(x, stdXY, stdTheta)); } } } diff --git a/src/main/java/frc/robot/util/Limelight.java b/src/main/java/frc/robot/util/Limelight.java index 3144a86..9c91747 100644 --- a/src/main/java/frc/robot/util/Limelight.java +++ b/src/main/java/frc/robot/util/Limelight.java @@ -94,19 +94,14 @@ else if (MathUtil.compareDouble(getApriltagId(), 16)) targetPose = new Pose2d(Units.inchesToMeters(182.73), Units.inchesToMeters(146.19), new Rotation2d()); } - if (Flip.isFlipped()) - { - targetPose = Flip.apply(targetPose); - } - Pose2d robotPose = Drivetrain.getInstance().getPoseEstimatorPose2d(); return robotPose.getTranslation().getDistance(targetPose.getTranslation()); } public static boolean isPoseNear(Pose2d pose, Pose2d visionPose) { - return getDistanceBetweenPose(pose, visionPose) < 1.0 - && MathUtil.compareDouble(visionPose.getTranslation().getNorm(), 0.0); + return getDistanceBetweenPose(pose, visionPose) <= 1.0 + && !MathUtil.compareDouble(visionPose.getTranslation().getNorm(), 0.0); } @@ -115,7 +110,7 @@ public static double getDistanceBetweenPose(Pose2d pose, Pose2d visionPose) { } public static double getTimestamp() { - return Timer.getFPGATimestamp() - getBotPoseVal()[5] / 1000.0; + return Timer.getFPGATimestamp() - (getValue("tl").getDouble(0.0) + getValue("cl").getDouble(0.0)); } public static boolean hasTargets() { From de1e2216de82c93959e262f44e53b7ad0d9802e6 Mon Sep 17 00:00:00 2001 From: 25ChilingH <25ChilingH@students.harker.org> Date: Thu, 21 Mar 2024 19:04:37 -0700 Subject: [PATCH 19/31] Correct errors on vision pose estimation --- src/main/java/frc/robot/OI.java | 1 - .../java/frc/robot/subsystems/swerve/Drivetrain.java | 10 ++++++---- src/main/java/frc/robot/util/Limelight.java | 10 +++++++--- 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index e72b08d..27089f2 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -13,7 +13,6 @@ // import frc.robot.commands.drivetrain.AlignToStage; // import frc.robot.commands.elevator.ElevatorManual; import frc.robot.commands.elevator.MoveToPosition; -import frc.robot.commands.intake.OuttakeStuckNote; import frc.robot.commands.pivot.PivotToAngle; import frc.robot.commands.pivot.ZeroPivot; import frc.robot.commands.shooter.MoveNoteToShooter; diff --git a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java index 48b3743..2b94a5c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java @@ -426,14 +426,16 @@ public void periodic() { updatePose(); // updatePoseEstimatorWithVisionBotPose(); - Pose2d visionBot = Limelight.getBotPose2d(); - if (Limelight.isPoseValid() && Limelight.isPoseNear(getPoseEstimatorPose2d(), visionBot)) { + if (Limelight.hasTargets()) { + Pose2d visionBot = Limelight.getBotPose2d(); double x = Limelight.getDistanceToTag(); double stdXY = .773 * x; double stdTheta = .025 + .277 * x; - if (Limelight.hasTargets() && !frc.robot.util.MathUtil.compareDouble(visionBot.getTranslation().getNorm(), 0)) { - poseEstimator.addVisionMeasurement(visionBot, Limelight.getTimestamp(), VecBuilder.fill(x, stdXY, stdTheta)); + if (Limelight.isPoseValid() && Limelight.isPoseNear(getPoseEstimatorPose2d(), visionBot)) { + poseEstimator.addVisionMeasurement(visionBot, Limelight.getTimestamp(), VecBuilder.fill(stdXY, stdXY, stdTheta)); } } + } } + \ No newline at end of file diff --git a/src/main/java/frc/robot/util/Limelight.java b/src/main/java/frc/robot/util/Limelight.java index 9c91747..245fc91 100644 --- a/src/main/java/frc/robot/util/Limelight.java +++ b/src/main/java/frc/robot/util/Limelight.java @@ -1,11 +1,13 @@ package frc.robot.util; +import edu.wpi.first.math.filter.Debouncer; 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.util.Units; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.Timer; import frc.robot.RobotMap; import frc.robot.subsystems.swerve.Drivetrain; @@ -13,6 +15,8 @@ public final class Limelight { private static NetworkTableInstance table; public static final String LIMELIGHT_TABLE_KEY = "limelight"; + public static Debouncer m_debouncer = new Debouncer(0.1, Debouncer.DebounceType.kRising); + public static Pose2d getBotPose2d() { return toPose2D(getBotPoseVal()); @@ -23,7 +27,7 @@ public static double getBestTargetArea() { } public static boolean isPoseValid() { - return getDistanceToTag() <= RobotMap.Camera.MAX_ERROR_VISION_POSE; + return m_debouncer.calculate(getDistanceToTag() <= RobotMap.Camera.MAX_ERROR_VISION_POSE); } // @@ -100,8 +104,8 @@ else if (MathUtil.compareDouble(getApriltagId(), 16)) } public static boolean isPoseNear(Pose2d pose, Pose2d visionPose) { - return getDistanceBetweenPose(pose, visionPose) <= 1.0 - && !MathUtil.compareDouble(visionPose.getTranslation().getNorm(), 0.0); + return m_debouncer.calculate(getDistanceBetweenPose(pose, visionPose) <= 1.0 + && !MathUtil.compareDouble(visionPose.getTranslation().getNorm(), 0.0)); } From 802057a9237310ea24a56f5de95ab668b4c39734 Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Fri, 22 Mar 2024 17:21:18 -0700 Subject: [PATCH 20/31] Finish fixing odometry - Day 3 --- .../robot/subsystems/swerve/Drivetrain.java | 18 +++++++++--------- src/main/java/frc/robot/util/Limelight.java | 13 ++++++++----- 2 files changed, 17 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java index 48b3743..2a77af8 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java @@ -68,9 +68,9 @@ public class Drivetrain extends SubsystemBase { private static PIDController omegaAmpController = new PIDController(RobotMap.Drivetrain.OMEGA_AMP_KP, 0, 0); // Standard deviations of pose estimate (x, y, heading) - private static Matrix stateStdDevs = VecBuilder.fill(0.15, 0.15, 0.01); // increase to trust encoder (state) + private static Matrix stateStdDevs = VecBuilder.fill(0.2, 0.2, 0.01); // increase to trust encoder (state) // measurements less - private static Matrix visionStdDevs = VecBuilder.fill(0.2, 0.2, 0.2); // increase to trust vsion + private static Matrix visionStdDevs = VecBuilder.fill(0.5, 0.5, 0.5); // increase to trust vsion // measurements less private boolean robotCentric; @@ -424,15 +424,15 @@ public boolean isPoseNear() { @Override public void periodic() { updatePose(); - // updatePoseEstimatorWithVisionBotPose(); - Pose2d visionBot = Limelight.getBotPose2d(); - if (Limelight.isPoseValid() && Limelight.isPoseNear(getPoseEstimatorPose2d(), visionBot)) { + if (Limelight.hasTargets()) { + Pose2d visionBot = Limelight.getBotPose2d(); double x = Limelight.getDistanceToTag(); - double stdXY = .773 * x; - double stdTheta = .025 + .277 * x; - if (Limelight.hasTargets() && !frc.robot.util.MathUtil.compareDouble(visionBot.getTranslation().getNorm(), 0)) { - poseEstimator.addVisionMeasurement(visionBot, Limelight.getTimestamp(), VecBuilder.fill(x, stdXY, stdTheta)); + + double stdX = .056 * x; + double stdTheta = .025 + .085 * x; + if (Limelight.isPoseValid() && Limelight.isPoseNear(getPoseEstimatorPose2d(), visionBot)) { + poseEstimator.addVisionMeasurement(visionBot, Limelight.getTimestamp(), VecBuilder.fill(stdX, stdX, stdTheta)); } } } diff --git a/src/main/java/frc/robot/util/Limelight.java b/src/main/java/frc/robot/util/Limelight.java index 9c91747..45048da 100644 --- a/src/main/java/frc/robot/util/Limelight.java +++ b/src/main/java/frc/robot/util/Limelight.java @@ -1,5 +1,7 @@ package frc.robot.util; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -13,6 +15,7 @@ public final class Limelight { private static NetworkTableInstance table; public static final String LIMELIGHT_TABLE_KEY = "limelight"; + private static Debouncer debounce = new Debouncer(0.1, DebounceType.kRising); public static Pose2d getBotPose2d() { return toPose2D(getBotPoseVal()); @@ -23,7 +26,7 @@ public static double getBestTargetArea() { } public static boolean isPoseValid() { - return getDistanceToTag() <= RobotMap.Camera.MAX_ERROR_VISION_POSE; + return debounce.calculate(getDistanceToTag() <= RobotMap.Camera.MAX_ERROR_VISION_POSE); } // @@ -94,14 +97,14 @@ else if (MathUtil.compareDouble(getApriltagId(), 16)) targetPose = new Pose2d(Units.inchesToMeters(182.73), Units.inchesToMeters(146.19), new Rotation2d()); } - Pose2d robotPose = Drivetrain.getInstance().getPoseEstimatorPose2d(); + Pose2d robotPose = getBotPose2d(); return robotPose.getTranslation().getDistance(targetPose.getTranslation()); } public static boolean isPoseNear(Pose2d pose, Pose2d visionPose) { - return getDistanceBetweenPose(pose, visionPose) <= 1.0 - && !MathUtil.compareDouble(visionPose.getTranslation().getNorm(), 0.0); + return debounce.calculate(getDistanceBetweenPose(pose, visionPose) <= 1.0 + && !MathUtil.compareDouble(visionPose.getTranslation().getNorm(), 0.0)); } @@ -110,7 +113,7 @@ public static double getDistanceBetweenPose(Pose2d pose, Pose2d visionPose) { } public static double getTimestamp() { - return Timer.getFPGATimestamp() - (getValue("tl").getDouble(0.0) + getValue("cl").getDouble(0.0)); + return Timer.getFPGATimestamp() - (getValue("tl").getDouble(0.0) + getValue("cl").getDouble(0.0)) / 1000.0; } public static boolean hasTargets() { From 236295511ec34ea584d660f29a3bb24d420809b2 Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Fri, 22 Mar 2024 18:00:19 -0700 Subject: [PATCH 21/31] Try fixing align but setpoints in opposite direction --- src/main/java/frc/robot/Robot.java | 3 +++ src/main/java/frc/robot/auton/Autons.java | 6 ++++++ .../commands/drivetrain/SwerveManual.java | 4 ++-- .../robot/subsystems/swerve/Drivetrain.java | 21 ++++++------------- 4 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 90cff60..cc3c3a1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -96,6 +96,9 @@ public void robotPeriodic() { telemetry.autons("Current Auton", autonChooser.getSelected()); telemetry.publish(); + SmartDashboard.putBoolean("Alliance Red", Flip.isFlipped()); + + SmartDashboard.putNumber("Setpoint for Amp", Drivetrain.omegaAmpController.getSetpoint()); NetworkTableInstance.getDefault().flushLocal(); NetworkTableInstance.getDefault().flush(); diff --git a/src/main/java/frc/robot/auton/Autons.java b/src/main/java/frc/robot/auton/Autons.java index d8ca452..b8f17b9 100644 --- a/src/main/java/frc/robot/auton/Autons.java +++ b/src/main/java/frc/robot/auton/Autons.java @@ -52,6 +52,12 @@ public class Autons public static final SequentialCommandGroup sixNotePath = new SequentialCommandGroup ( + new SwervePositionController(Trajectories.startToNote1_four, () -> Rotation2d.fromDegrees(133.13), false), + new SwervePositionController(Trajectories.note1ToShoot1_four, () -> Rotation2d.fromDegrees(180), true), + new SwervePositionController(Trajectories.shoot1ToNote2_four, () -> Rotation2d.fromDegrees(180), false), + new SwervePositionController(Trajectories.note2ToShoot2_four, () -> Rotation2d.fromDegrees(180), true), + new SwervePositionController(Trajectories.shoot2ToNote3_four, () -> Rotation2d.fromDegrees(-157.83), false), + new SwervePositionController(Trajectories.note3ToShoot3_four, () -> Rotation2d.fromDegrees(-136.16), true) // new SwervePositionController(Trajectories.startToNote1_six, () -> Rotation2d.fromDegrees(180), false), // new SwervePositionController(Trajectories.note1ToShoot1_six, () -> Rotation2d.fromDegrees(180), true), // new SwervePositionController(Trajectories.shoot1ToNote2_six, () -> Rotation2d.fromDegrees(158.62), false), diff --git a/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java b/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java index a0b316f..024b72d 100644 --- a/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java +++ b/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java @@ -65,7 +65,7 @@ public void execute() { // aligns to speaker if (OI.getInstance().getDriver().getRightBumperState()) { omega = Drivetrain.getInstance().alignToSpeaker(); - Drivetrain.getInstance().setPreviousHeading(-Drivetrain.getInstance().getPoseEstimatorPose2d().getRotation().getDegrees()); + Drivetrain.getInstance().setPreviousHeading(Drivetrain.getInstance().getPoseEstimatorPose2d().getRotation().getDegrees()); } // aligns to amp @@ -75,7 +75,7 @@ public void execute() { vx *= RobotMap.Drivetrain.EXTENDED_MAX_DRIVING_SPEED; vy *= RobotMap.Drivetrain.EXTENDED_MAX_DRIVING_SPEED; omega = Drivetrain.getInstance().alignToAmp()[2]; - Drivetrain.getInstance().setPreviousHeading(-Drivetrain.getInstance().getPoseEstimatorPose2d().getRotation().getDegrees()); + Drivetrain.getInstance().setPreviousHeading(Drivetrain.getInstance().getPoseEstimatorPose2d().getRotation().getDegrees()); } if (OI.getInstance().getDriver().getLeftTrigger() > 0.5) { diff --git a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java index 2a77af8..4fb829b 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java @@ -66,7 +66,7 @@ public class Drivetrain extends SubsystemBase { private static PIDController vxAmpController = new PIDController(RobotMap.Drivetrain.VX_AMP_kP, 0, 0); private static PIDController vyAmpController = new PIDController(RobotMap.Drivetrain.VY_AMP_kP, 0, 0); - private static PIDController omegaAmpController = new PIDController(RobotMap.Drivetrain.OMEGA_AMP_KP, 0, 0); + public static PIDController omegaAmpController = new PIDController(RobotMap.Drivetrain.OMEGA_AMP_KP, 0, 0); // Standard deviations of pose estimate (x, y, heading) private static Matrix stateStdDevs = VecBuilder.fill(0.2, 0.2, 0.01); // increase to trust encoder (state) // measurements less @@ -140,7 +140,6 @@ private void initPigeon() { pigeon.getYaw().setUpdateFrequency(250); pigeon.getConfigurator().apply(pigeonConfigs); - pigeon.setYaw(0); } /** @@ -151,24 +150,16 @@ private void initPigeon() { * @return adjusted rotational speed */ public double adjustPigeon(double omega) { + double currHeading = poseEstimator.getEstimatedPosition().getRotation().getDegrees(); if (Math.abs(omega) <= RobotMap.Drivetrain.MIN_OUTPUT) { - omega = -RobotMap.Drivetrain.PIGEON_kP * (prevHeading - getHeading()); + omega = -RobotMap.Drivetrain.PIGEON_kP * (prevHeading - currHeading); } else { - prevHeading = getHeading(); + prevHeading = currHeading; } return omega; } - /* - * Returns yaw of pigeon in degrees (heading of robot) - */ - public double getHeading() { - double yaw = pigeon.getYaw().getValue(); - // SmartDashboard.putNumber("pigeon heading", pigeon.getYaw()); - return yaw; - } - /** * @return pitch of pigeon in degrees */ @@ -225,7 +216,7 @@ public void toggleRobotCentric() { * @return heading of pigeon as a Rotation2d */ public Rotation2d getRotation() { - return Rotation2d.fromDegrees(getHeading()); + return pigeon.getRotation2d(); } /** @@ -429,7 +420,7 @@ public void periodic() { Pose2d visionBot = Limelight.getBotPose2d(); double x = Limelight.getDistanceToTag(); - double stdX = .056 * x; + double stdX = .06 * x; double stdTheta = .025 + .085 * x; if (Limelight.isPoseValid() && Limelight.isPoseNear(getPoseEstimatorPose2d(), visionBot)) { poseEstimator.addVisionMeasurement(visionBot, Limelight.getTimestamp(), VecBuilder.fill(stdX, stdX, stdTheta)); From 3c24dc6eff824e4d44038059ff20ccfaff5ada6d Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Fri, 22 Mar 2024 18:02:07 -0700 Subject: [PATCH 22/31] Merge branches --- src/main/java/frc/robot/subsystems/swerve/Drivetrain.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java index b614a08..d54b41f 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java @@ -416,8 +416,6 @@ public boolean isPoseNear() { public void periodic() { updatePose(); - if (Limelight.hasTargets()) { - Pose2d visionBot = Limelight.getBotPose2d(); if (Limelight.hasTargets()) { Pose2d visionBot = Limelight.getBotPose2d(); double x = Limelight.getDistanceToTag(); From bda2cc0e88c8b413050f5077a4a6ca4f815842aa Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Sat, 23 Mar 2024 10:46:26 -0700 Subject: [PATCH 23/31] STG fixes --- src/main/java/frc/robot/RobotMap.java | 2 +- src/main/java/frc/robot/subsystems/Elevator.java | 12 ++++++++++-- src/main/java/frc/robot/util/Telemetry.java | 10 ++++++++-- 3 files changed, 19 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index ef13062..d3f3654 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -206,7 +206,7 @@ public static final class Pivot { public static final double ZERO_SPEED = -0.2; - public static final double PIVOT_kP = 100; + public static final double PIVOT_kP = 41.939; public static final double PIVOT_kG = 0.40; public static final double PIVOT_kS = 0.10574; diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index f972bde..0c5af19 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -38,7 +38,7 @@ private void configMotors() { masterConfig.MotorOutput.Inverted = RobotMap.Elevator.MASTER_INVERT; masterConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - followerConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + followerConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; masterConfig.Voltage.PeakForwardVoltage = RobotMap.MAX_VOLTAGE; masterConfig.Voltage.PeakReverseVoltage = -RobotMap.MAX_VOLTAGE; @@ -51,10 +51,12 @@ private void configMotors() { masterConfig.Slot0.kP = RobotMap.Elevator.ELEVATOR_kP; masterConfig.Slot0.kG = RobotMap.Elevator.ELEVATOR_kG; + follower.setControl(new Follower(RobotMap.Elevator.MASTER_ID, false)); + master.getConfigurator().apply(masterConfig); follower.getConfigurator().apply(followerConfig); - follower.setControl(new Follower(RobotMap.Elevator.MASTER_ID, false)); + } @@ -99,4 +101,10 @@ public static Elevator getInstance() { return instance; } + public TalonFX getMaster() { + return master; + } + public TalonFX getFollower() { + return follower; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/Telemetry.java b/src/main/java/frc/robot/util/Telemetry.java index ae3daca..a7370a8 100644 --- a/src/main/java/frc/robot/util/Telemetry.java +++ b/src/main/java/frc/robot/util/Telemetry.java @@ -218,8 +218,14 @@ public void elevator() { // NetworkTableEntry elevatorLimitSwitchHit = _elevator.getEntry("Elevator Limit Switch Hit"); // elevatorLimitSwitchHit.setBoolean(elevator.isLimitHit()); - // NetworkTableEntry elevatorStalling = _elevator.getEntry("Elevator Stalling"); - // elevatorStalling.setBoolean(elevator.isLimitHit()); + NetworkTableEntry elevatorStalling = _elevator.getEntry("Elevator Stalling"); + elevatorStalling.setBoolean(elevator.isLimitHit()); + + NetworkTableEntry elevatorMasterCurrent = _elevator.getEntry("Elevator Master Current"); + elevatorMasterCurrent.setDouble(elevator.getMaster().getStatorCurrent().getValue()); + + NetworkTableEntry elevatorFollowerCurrent = _elevator.getEntry("Elevator Follower Current"); + elevatorFollowerCurrent.setDouble(elevator.getFollower().getStatorCurrent().getValue()); // NetworkTableEntry elevatorSensorPosition = _elevator.getEntry("Elevator Sensor Position"); // elevatorSensorPosition.setDouble(elevator.getPosition()); From b7a3bbb50b4e50a3cb36e3dcd0b1d21224718f4f Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Sat, 23 Mar 2024 18:42:34 -0700 Subject: [PATCH 24/31] pivot tuning; auton pathing --- src/main/java/frc/robot/OI.java | 7 +- src/main/java/frc/robot/Robot.java | 2 - src/main/java/frc/robot/RobotMap.java | 48 +++++++------ .../java/frc/robot/auton/Trajectories.java | 70 +++++++++---------- .../frc/robot/commands/CommandGroups.java | 21 ++++++ .../robot/commands/pivot/PivotToAngle.java | 24 +++---- .../java/frc/robot/subsystems/Elevator.java | 6 ++ src/main/java/frc/robot/subsystems/Pivot.java | 38 +++++----- .../robot/subsystems/swerve/Drivetrain.java | 4 +- src/main/java/frc/robot/util/Limelight.java | 1 - src/main/java/frc/robot/util/Telemetry.java | 28 ++++---- src/main/java/frc/robot/util/limelight.vpr | 14 ++-- 12 files changed, 146 insertions(+), 117 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 27089f2..b39204e 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -53,14 +53,15 @@ private void initBindings() { } driver.getUpDPadButton().onTrue(new PivotToAngle(Goal.SPEAKER)); - driver.getDownDPadButton().onTrue(new PivotToAngle(Goal.SUB)); + driver.getDownDPadButton().onTrue(new PivotToAngle(Goal.AMP)); driver.getButtonB().whileTrue(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0.1))); driver.getButtonB().whileFalse(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0))); // driver.getButtonA().onTrue(new AlignToStage("left")); - driver.getButtonY().onTrue(new MoveToPosition(RobotMap.Elevator.STAGE_HEIGHT)); - driver.getButtonA().onTrue(new MoveToPosition(0)); + + driver.getButtonY().onTrue(CommandGroups.getFullClimb()); + driver.getButtonA().whileTrue(CommandGroups.getFullRetractClimb()); driver.getButtonStart().onTrue(new InstantCommand(() -> { Drivetrain.getInstance().toggleRobotCentric(); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cc3c3a1..c7409cf 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -98,8 +98,6 @@ public void robotPeriodic() { telemetry.publish(); SmartDashboard.putBoolean("Alliance Red", Flip.isFlipped()); - SmartDashboard.putNumber("Setpoint for Amp", Drivetrain.omegaAmpController.getSetpoint()); - NetworkTableInstance.getDefault().flushLocal(); NetworkTableInstance.getDefault().flush(); } diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index d3f3654..78d2da4 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -12,7 +12,7 @@ public class RobotMap { // Global Robot Constants - public static final boolean FIRST_BOT = false; + public static final boolean FIRST_BOT = true; public static final double MAX_VOLTAGE = 12; public static final double ROBOT_LOOP = 0.02; public static final String CAN_CHAIN = "rio"; @@ -135,8 +135,8 @@ public static final class Drivetrain { public static final double VERTICAL_DEG_STAGE = 10; // Robot Dimensions - public static final double ROBOT_LENGTH = Units.inchesToMeters(28); - public static final double ROBOT_WIDTH = Units.inchesToMeters(28); + public static final double ROBOT_LENGTH = Units.inchesToMeters(22.75); + public static final double ROBOT_WIDTH = Units.inchesToMeters(22.75); public static final double MAX_DRIVING_SPEED = 5.0; // m/s //TODO public static final double EXTENDED_MAX_DRIVING_SPEED = 0.3; @@ -149,7 +149,7 @@ public static final class Drivetrain { * PID values for X, Y, and Rotation (THETA) */ - public static double X_kP = 1.0; // TODO + public static double X_kP = 1.8; // TODO public static double X_kI = 0.0; public static double X_kD = 0.0; @@ -157,7 +157,7 @@ public static final class Drivetrain { public static double Y_kI = 0.0; public static double Y_kD = 0.0; - public static double THETA_kP = 1.0; // TODO + public static double THETA_kP = 1.8; // TODO public static double THETA_kI = 0.0; public static double THETA_kD = 0.0; @@ -187,7 +187,7 @@ public static final class Shooter { public static final double SHOOTING_SPEED = 0.8; public static final double REVVED_RPS = 1000.0 / 60.0; - public static final double REVVED_AMP_RPS = 150.0 / 60.0; + public static final double REVVED_AMP_RPS = 300.0 / 60.0; public static enum Goal { AMP, @@ -200,21 +200,26 @@ public static final class Pivot { public static final int LIMIT_SWITCH_ID = 2; public static final int CAN_CODER_ID = 30; - public static final double CAN_CODER_OFFSET = 0.022705; //TODO + public static final double CAN_CODER_OFFSET = 0.032717; public static final InvertedValue MASTER_INVERT = InvertedValue.CounterClockwise_Positive; public static final double ZERO_SPEED = -0.2; - public static final double PIVOT_kP = 41.939; - public static final double PIVOT_kG = 0.40; - public static final double PIVOT_kS = 0.10574; + public static final double PIVOT_AMP_kP = 16; + public static final double PIVOT_AMP_kS = 0; - public static final double SUB_ANGLE = 25.0; - public static final double TRAP1_ANGLE = 10; - public static final double TRAP2_ANGLE = 40; - public static final double TRAP_SCORE_ANGLE = 30; - public static final double AMP_ANGLE = 80; + public static final double PIVOT_kG = 0.17; + + + public static final double PIVOT_kP = 67; + public static final double PIVOT_kI = 0.4; + public static final double PIVOT_kD = 0.7; //0.2; + public static final double PIVOT_kS = 0.4; // 0.69673; + public static final double OFFSET_ANGLE = 21; + + public static final double CLIMB_ANGLE = 100; + public static final double AMP_ANGLE = 79; public static final double PIVOT_GEAR_RATIO = 37.5; @@ -222,21 +227,18 @@ public static final class Pivot { public static final double STALLING_CURRENT = 50; // stalls when current is 50 - public static final double MAX_ERROR = 0.9; // degrees + public static final double MAX_ERROR = 1.5; // degrees public static final double PIVOT_FORWARD_SOFT_LIMIT = 65 / 67.76 * 37.5; public static final double PIVOT_REVERSE_SOFT_LIMIT = 0; - public static final double MAX_CRUISE_ACCLERATION = 400.0 / 67.76 * 37.5 / 360; - public static final double MAX_CRUISE_VElOCITY = 400.0 / 67.76 * 37.5 / 360; + public static final double MAX_CRUISE_ACCLERATION = 817.03 / 2; + public static final double MAX_CRUISE_VElOCITY = 817.03 / 2; public static enum Goal { AMP, SPEAKER, - SUB, - TRAP1, - TRAP2, - TRAP_SCORE + CLIMB } } @@ -255,7 +257,7 @@ public static final class Elevator { public static final double ELEVATOR_FORWARD_SOFT_LIMIT = 0; public static final double ELEVATOR_REVERSE_SOFT_LIMIT = 0; - public static final double ELEVATOR_STALLING_CURRENT = 90; + public static final double ELEVATOR_STALLING_CURRENT = 80; public static final InvertedValue MASTER_INVERT = InvertedValue.Clockwise_Positive; diff --git a/src/main/java/frc/robot/auton/Trajectories.java b/src/main/java/frc/robot/auton/Trajectories.java index 2608d5f..7d90cce 100644 --- a/src/main/java/frc/robot/auton/Trajectories.java +++ b/src/main/java/frc/robot/auton/Trajectories.java @@ -80,57 +80,57 @@ public class Trajectories { public static Trajectory startToNote1_four = generateTrajectory( List.of(new Pose2d(1.28, 5.41, Rotation2d.fromDegrees(180)), new Pose2d(2.57, 4.15, Rotation2d.fromDegrees(180))), - 2.0, - 1.0, + 5.0, + 4.0, 0.0, 0.0, true); public static Trajectory note1ToShoot1_four = generateTrajectory( List.of(new Pose2d(2.57, 4.15, Rotation2d.fromDegrees(180)), - new Pose2d(1.72, 5.41, Rotation2d.fromDegrees(180))), - 2.0, - 1.0, + new Pose2d(1.90, 5.41, Rotation2d.fromDegrees(180))), + 5.0, + 4.0, 0, 0, false); public static Trajectory shoot1ToNote2_four = generateTrajectory( - List.of(new Pose2d(1.72, 5.41, Rotation2d.fromDegrees(180)), + List.of(new Pose2d(1.90, 5.41, Rotation2d.fromDegrees(180)), new Pose2d(2.73, 5.54, Rotation2d.fromDegrees(180))), - 2.0, - 1.0, + 5.0, + 4.0, 0, 0, false); public static Trajectory note2ToShoot2_four = generateTrajectory( List.of(new Pose2d(2.73, 5.54, Rotation2d.fromDegrees(180)), - new Pose2d(1.84, 5.96, Rotation2d.fromDegrees(-157.83))), - 2.0, - 1.0, + new Pose2d(1.90, 5.96, Rotation2d.fromDegrees(-157.83))), + 5.0, + 4.0, 0.0, 0.0, false); public static Trajectory shoot2ToNote3_four = generateTrajectory( - List.of(new Pose2d(1.84, 5.96, Rotation2d.fromDegrees(-157.83)), + List.of(new Pose2d(1.90, 5.96, Rotation2d.fromDegrees(-157.83)), new Pose2d(2.73, 7.06, Rotation2d.fromDegrees(-136.16))), - 2.0, - 1.0, + 5.0, + 4.0, 0.0, 0.0, false); public static Trajectory note3ToShoot3_four = generateTrajectory( List.of(new Pose2d(2.73, 7.06, Rotation2d.fromDegrees(-136.16)), - new Pose2d(1.84, 6.43, Rotation2d.fromDegrees(-143.16))), - 2.0, - 1.0, + new Pose2d(1.90, 6.43, Rotation2d.fromDegrees(-143.16))), + 5.0, + 4.0, 0.0, 0.0, false); public static Trajectory shoot3ToEnd_four = generateTrajectory( - List.of(new Pose2d(1.84, 6.43, Rotation2d.fromDegrees(-143.16)), + List.of(new Pose2d(1.90, 6.43, Rotation2d.fromDegrees(-143.16)), new Pose2d(1.79, 5.43, Rotation2d.fromDegrees(180))), - 2.0, - 1.0, + 5.0, + 4.0, 0.0, 0.0, false); @@ -142,31 +142,31 @@ public class Trajectories { List.of(new Pose2d(1.72, 5.56, Rotation2d.fromDegrees(180)), new Pose2d(2.56, 4.16, Rotation2d.fromDegrees(180))), 5.0, - 2.5, + 4.0, 0.0, 0.0, true); public static Trajectory note1ToShoot1_six = generateTrajectory( List.of(new Pose2d(2.56, 4.16, Rotation2d.fromDegrees(180)), - new Pose2d(2.28, 4.89, Rotation2d.fromDegrees(158.62))), + new Pose2d(2.40, 4.89, Rotation2d.fromDegrees(158.62))), 5.0, - 2.5, + 4.0, 0.0, 3.0, true); public static Trajectory shoot1ToNote2_six = generateTrajectory( - List.of(new Pose2d(2.28, 4.89, Rotation2d.fromDegrees(158.62)), - new Pose2d(2.41, 5.56, Rotation2d.fromDegrees(178.45))), + List.of(new Pose2d(2.40, 4.89, Rotation2d.fromDegrees(158.62)), + new Pose2d(2.47, 5.56, Rotation2d.fromDegrees(178.45))), 5.0, - 2.5, + 4.0, 3.0, 0.0, false); public static Trajectory note2ToShoot2_six = generateTrajectory( - List.of(new Pose2d(2.41, 5.56, Rotation2d.fromDegrees(178.45)), + List.of(new Pose2d(2.47, 5.56, Rotation2d.fromDegrees(178.45)), new Pose2d(2.10, 6.15, Rotation2d.fromDegrees(-160.76))), 5.0, - 2.5, + 4.0, 0.0, 2.0, true); @@ -174,7 +174,7 @@ public class Trajectories { List.of(new Pose2d(2.10, 6.15, Rotation2d.fromDegrees(-160.76)), new Pose2d(2.56, 7.01, Rotation2d.fromDegrees(180))), 5.0, - 2.5, + 4.0, 2.0, 2.0, false); @@ -182,7 +182,7 @@ public class Trajectories { List.of(new Pose2d(2.56, 7.01, Rotation2d.fromDegrees(180)), new Pose2d(5.37, 6.58, Rotation2d.fromDegrees(-156.72))), 5.0, - 2.5, + 4.0, 2.0, 0.0, false); @@ -190,7 +190,7 @@ public class Trajectories { List.of(new Pose2d(5.37, 6.58, Rotation2d.fromDegrees(-156.72)), new Pose2d(5.79, 6.34, Rotation2d.fromDegrees(180))), 5.0, - 2.5, + 4.0, 0.0, 3.0, false); @@ -198,7 +198,7 @@ public class Trajectories { List.of(new Pose2d(5.79, 6.34, Rotation2d.fromDegrees(180)), new Pose2d(7.97,5.79, Rotation2d.fromDegrees(-164.18))), 5.0, - 2.5, + 4.0, 3.0, 0.0, false); @@ -206,7 +206,7 @@ public class Trajectories { List.of(new Pose2d(7.97, 5.79, Rotation2d.fromDegrees(-164.18)), new Pose2d(5.82,6.41, Rotation2d.fromDegrees(180))), 5.0, - 2.5, + 4.0, 0.0, 3.0, false); @@ -214,7 +214,7 @@ public class Trajectories { List.of(new Pose2d(5.82, 6.41, Rotation2d.fromDegrees(180)), new Pose2d(7.97,7.47, Rotation2d.fromDegrees(-162.5))), 5.0, - 2.5, + 4.0, 3.0, 0.0, false); @@ -222,7 +222,7 @@ public class Trajectories { List.of(new Pose2d(7.97,7.47, Rotation2d.fromDegrees(-162.5)), new Pose2d(5.37, 6.58, Rotation2d.fromDegrees(180))), 5.0, - 2.5, + 4.0, 0.0, 0.0, false); diff --git a/src/main/java/frc/robot/commands/CommandGroups.java b/src/main/java/frc/robot/commands/CommandGroups.java index c51da55..3ec9a7b 100644 --- a/src/main/java/frc/robot/commands/CommandGroups.java +++ b/src/main/java/frc/robot/commands/CommandGroups.java @@ -1,7 +1,13 @@ package frc.robot.commands; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.signals.NeutralModeValue; + import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.RobotMap; +import frc.robot.RobotMap.Pivot; +import frc.robot.commands.elevator.MoveToPosition; import frc.robot.commands.elevator.ZeroElevator; import frc.robot.commands.indexer.IndexToShooter; import frc.robot.commands.intake.IntakeNote; @@ -11,6 +17,7 @@ import frc.robot.commands.shooter.MoveNoteToShooter; import frc.robot.commands.shooter.RevShooter; import frc.robot.commands.shooter.ShootNote; +import frc.robot.subsystems.Elevator; public class CommandGroups { @@ -28,6 +35,7 @@ public static Command getFullIntakeCommand() { public static Command getFullShootSpeaker() { return new RevShooter(RobotMap.Shooter.Goal.SPEAKER) .alongWith(new PivotToAngle(RobotMap.Pivot.Goal.SPEAKER)) + // .alongWith(new IndexToShooter()) .andThen(new ShootNote()) .andThen(new ZeroPivot()); } @@ -37,6 +45,19 @@ public static Command getFullShootAmp() { .andThen(new RevShooter(RobotMap.Shooter.Goal.AMP)) .andThen(new ShootNote()).andThen(new ZeroPivot()); } + + public static Command getFullClimb() { + return new InstantCommand(() -> Elevator.getInstance().setFollowerNeutralMode( + new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast))) + .andThen(new PivotToAngle(Pivot.Goal.CLIMB) + .alongWith(new MoveToPosition(RobotMap.Elevator.STAGE_HEIGHT))); + } + + public static Command getFullRetractClimb() { + return new MoveToPosition(0) + .andThen(new InstantCommand(() -> Elevator.getInstance().setFollowerNeutralMode( + new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake)))); + } // public static final Command PRE_DRIVEFWD_CLIMB = new PivotToAngle(RobotMap.Pivot.Goal.TRAP1); // wait for drive forward diff --git a/src/main/java/frc/robot/commands/pivot/PivotToAngle.java b/src/main/java/frc/robot/commands/pivot/PivotToAngle.java index a422ef9..bcf0653 100644 --- a/src/main/java/frc/robot/commands/pivot/PivotToAngle.java +++ b/src/main/java/frc/robot/commands/pivot/PivotToAngle.java @@ -19,24 +19,17 @@ public void execute() { switch (setpoint) { case SPEAKER: ref = Pivot.getInstance().getPivotSetpoint(Drivetrain.getInstance().getDistanceToSpeaker()); - break; - case SUB: - ref = RobotMap.Pivot.SUB_ANGLE; + Pivot.getInstance().moveToPosition(ref); break; case AMP: ref = RobotMap.Pivot.AMP_ANGLE; + Pivot.getInstance().moveToPositionAmp(ref); break; - case TRAP1: - ref = RobotMap.Pivot.TRAP1_ANGLE; - break; - case TRAP2: - ref = RobotMap.Pivot.TRAP2_ANGLE; - break; - case TRAP_SCORE: - ref = RobotMap.Pivot.TRAP_SCORE_ANGLE; + case CLIMB: + ref = RobotMap.Pivot.CLIMB_ANGLE; + Pivot.getInstance().moveToPosition(ref); break; } - Pivot.getInstance().moveToPosition(ref); } @@ -45,7 +38,12 @@ public double getRef() { } public boolean isFinished() { - return MathUtil.compareSetpoint(Pivot.getInstance().getPosition(), ref, RobotMap.Pivot.MAX_ERROR); + switch(setpoint) { + case SPEAKER: + return MathUtil.compareSetpoint(Pivot.getInstance().getPosition(), ref, RobotMap.Pivot.MAX_ERROR); + default: + return MathUtil.compareSetpoint(Pivot.getInstance().getPosition(), ref, RobotMap.Pivot.MAX_ERROR + 1); + } } public void end(boolean interrupted) { diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 0c5af19..99eff52 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -1,6 +1,8 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.PositionVoltage; @@ -60,6 +62,10 @@ private void configMotors() { } + public void setFollowerNeutralMode(MotorOutputConfigs motorConfig) { + follower.getConfigurator().apply(motorConfig); + } + public double getPosition() { return master.getPosition().getValue(); } diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index ede4050..9823252 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -3,27 +3,21 @@ import static edu.wpi.first.units.MutableMeasure.mutable; import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.DegreesPerSecond; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.math.util.Units; import edu.wpi.first.units.Angle; -import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import edu.wpi.first.units.MutableMeasure; import edu.wpi.first.units.Velocity; @@ -63,16 +57,16 @@ private Pivot() { canCoder = new CANcoder(RobotMap.Pivot.CAN_CODER_ID); speakerAngles = new InterpolatingDoubleTreeMap(); - speakerAngles.put(0.0, 21.5); + speakerAngles.put(0.0, 22.5); speakerAngles.put(1.787, 22.5); speakerAngles.put(2.043, 35.571); - speakerAngles.put(2.361, 16.787 + 16); - speakerAngles.put(2.839, 23.643 + 16); - speakerAngles.put(3.228, 33.574 + 12); - speakerAngles.put(3.713, 36.914 + 9); - speakerAngles.put(4.156, 37.969 + 9); - speakerAngles.put(4.507, 38.463 + 9); - speakerAngles.put(5.051, 39.990 + 9); + speakerAngles.put(2.361, 37.5); + speakerAngles.put(2.839, 39.643); + speakerAngles.put(3.228, 42.574); + speakerAngles.put(3.713, 45.914); + speakerAngles.put(4.156, 46.5); + speakerAngles.put(4.507, 47.463); + speakerAngles.put(5.051, 48.990); configCANcoder(); configMotors(); } @@ -89,10 +83,13 @@ private void configMotors() { // masterConfig.Feedback.SensorToMechanismRatio = RobotMap.Pivot.PIVOT_GEAR_RATIO; masterConfig.Slot0.kP = RobotMap.Pivot.PIVOT_kP; - masterConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - masterConfig.Slot0.kG = RobotMap.Pivot.PIVOT_kG; + masterConfig.Slot0.kI = RobotMap.Pivot.PIVOT_kI; + masterConfig.Slot0.kD = RobotMap.Pivot.PIVOT_kD; masterConfig.Slot0.kS = RobotMap.Pivot.PIVOT_kS; + masterConfig.Slot1.kP = RobotMap.Pivot.PIVOT_AMP_kP; + masterConfig.Slot1.kS = RobotMap.Pivot.PIVOT_AMP_kS; + masterConfig.MotionMagic.MotionMagicCruiseVelocity = RobotMap.Pivot.MAX_CRUISE_VElOCITY; masterConfig.MotionMagic.MotionMagicAcceleration = RobotMap.Pivot.MAX_CRUISE_ACCLERATION; @@ -140,8 +137,15 @@ public double getVoltage() { } public void moveToPosition(double desiredAngle) { + double kG = RobotMap.Pivot.PIVOT_kG * Math.cos(Math.toRadians(getPosition() + RobotMap.Pivot.OFFSET_ANGLE)); + MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(desiredAngle / RobotMap.Pivot.PIVOT_ROT_TO_ANGLE); + master.setControl(motionMagicVoltage.withFeedForward(kG)); + } + + public void moveToPositionAmp(double desiredAngle) { + double kG = RobotMap.Pivot.PIVOT_kG * Math.cos(Math.toRadians(getPosition() + RobotMap.Pivot.OFFSET_ANGLE)); MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(desiredAngle / RobotMap.Pivot.PIVOT_ROT_TO_ANGLE); - master.setControl(motionMagicVoltage); + master.setControl(motionMagicVoltage.withFeedForward(kG).withSlot(1)); } public void setPercentOutput(double power) { diff --git a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java index d54b41f..3d8edeb 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java @@ -420,8 +420,8 @@ public void periodic() { Pose2d visionBot = Limelight.getBotPose2d(); double x = Limelight.getDistanceToTag(); - double stdX = .06 * x; - double stdTheta = .025 + .085 * x; + double stdX = .085 * x; + double stdTheta = .1 + .085 * x; if (Limelight.isPoseValid() && Limelight.isPoseNear(getPoseEstimatorPose2d(), visionBot)) { poseEstimator.addVisionMeasurement(visionBot, Limelight.getTimestamp(), VecBuilder.fill(stdX, stdX, stdTheta)); } diff --git a/src/main/java/frc/robot/util/Limelight.java b/src/main/java/frc/robot/util/Limelight.java index 9307e9a..4de619f 100644 --- a/src/main/java/frc/robot/util/Limelight.java +++ b/src/main/java/frc/robot/util/Limelight.java @@ -8,7 +8,6 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.Timer; import frc.robot.RobotMap; import frc.robot.subsystems.swerve.Drivetrain; diff --git a/src/main/java/frc/robot/util/Telemetry.java b/src/main/java/frc/robot/util/Telemetry.java index a7370a8..4793f4f 100644 --- a/src/main/java/frc/robot/util/Telemetry.java +++ b/src/main/java/frc/robot/util/Telemetry.java @@ -240,26 +240,26 @@ public void intake() { } public void pivot() { - // NetworkTableEntry pivotSensorPosition = _pivot.getEntry("Pivot Sensor Position"); - // pivotSensorPosition.setDouble(pivot.getPosition()); + NetworkTableEntry pivotSensorPosition = _pivot.getEntry("Pivot Sensor Position"); + pivotSensorPosition.setDouble(pivot.getPosition()); - // NetworkTableEntry pivotRef = _pivot.getEntry("Pivot Speaker Setpoint"); - // pivotRef.setDouble(pivot.getPivotSetpoint(Drivetrain.getInstance().getDistanceToSpeaker())); + NetworkTableEntry pivotRef = _pivot.getEntry("Pivot Speaker Setpoint"); + pivotRef.setDouble(pivot.getPivotSetpoint(Drivetrain.getInstance().getDistanceToSpeaker())); - // NetworkTableEntry pivotSensorVelocity = _pivot.getEntry("Pivot Sensor Velocity"); - // pivotSensorVelocity.setDouble(pivot.getVelocity()); + NetworkTableEntry pivotSensorVelocity = _pivot.getEntry("Pivot Sensor Velocity"); + pivotSensorVelocity.setDouble(pivot.getVelocity()); - // NetworkTableEntry pivotStalling = _pivot.getEntry("Pivot Stalling"); - // pivotStalling.setBoolean(pivot.isStalling()); + NetworkTableEntry pivotStalling = _pivot.getEntry("Pivot Stalling"); + pivotStalling.setBoolean(pivot.isStalling()); - // NetworkTableEntry pivotLimitSwitch = _pivot.getEntry("Pivot Limit Switch"); - // pivotLimitSwitch.setBoolean(pivot.isLimitHit()); + NetworkTableEntry pivotLimitSwitch = _pivot.getEntry("Pivot Limit Switch"); + pivotLimitSwitch.setBoolean(pivot.isLimitHit()); - // NetworkTableEntry pivotMasterCurrent = _pivot.getEntry("Pivot Master Motor Current"); - // pivotMasterCurrent.setDouble(pivot.getMasterCurrent()); + NetworkTableEntry pivotMasterCurrent = _pivot.getEntry("Pivot Master Motor Current"); + pivotMasterCurrent.setDouble(pivot.getMasterCurrent()); - // NetworkTableEntry setPivotAngle = _pivot.getEntry("Pivot Angle"); - // setPivotAngle.setDouble(pivot.getPivotSetpoint(0)) + NetworkTableEntry setPivotAngle = _pivot.getEntry("Pivot Angle"); + setPivotAngle.setDouble(pivot.getPivotSetpoint(0)); } public void vision() { diff --git a/src/main/java/frc/robot/util/limelight.vpr b/src/main/java/frc/robot/util/limelight.vpr index 2e92b6a..f39df22 100644 --- a/src/main/java/frc/robot/util/limelight.vpr +++ b/src/main/java/frc/robot/util/limelight.vpr @@ -1,9 +1,9 @@ -area_max:0.0017850625000000004 -area_min:0 +area_max:100 +area_min:0.0577200625 area_similarity:0 aspect_max:20.000000 aspect_min:0.000000 -black_level:4 +black_level:5 blue_balance:1558 botfloorsnap:1 botlength:0.7112 @@ -34,7 +34,7 @@ dilation_steps:0 direction_filter:0 dual_close_sort_origin:0 erosion_steps:0 -exposure:302 +exposure:899 fiducial_decoder_strictness:strict fiducial_denoise:0.000000 fiducial_idfilters: @@ -56,18 +56,18 @@ image_source:0 img_to_show:0 intersection_filter:0 invert_hue:0 -lcgain:9.6 +lcgain:7.3 multigroup_max:7 multigroup_min:1 multigroup_rejector:0 pipeline_led_enabled:0 pipeline_led_power:0 -pipeline_res:3 +pipeline_res:2 pipeline_type:3 red_balance:1303 roi_x:0.000000 roi_y:0.000000 -rsf:1.55 +rsf:1.35 rspitch:-30.494 rsroll:0 rss:0 From 23c8248a72e3091ba34cfb226cacd9f3db39b7aa Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Mon, 25 Mar 2024 18:07:34 -0700 Subject: [PATCH 25/31] pivot tuning --- src/main/java/frc/robot/RobotMap.java | 34 +++++++++++-------- .../frc/robot/commands/CommandGroups.java | 3 +- .../commands/drivetrain/SwerveManual.java | 4 +-- .../robot/commands/pivot/PivotToAngle.java | 13 +++++-- src/main/java/frc/robot/subsystems/Pivot.java | 6 ++-- .../robot/subsystems/swerve/Drivetrain.java | 4 +-- src/main/java/frc/robot/util/Telemetry.java | 4 +-- 7 files changed, 42 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 78d2da4..a2378bd 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -206,20 +206,21 @@ public static final class Pivot { public static final double ZERO_SPEED = -0.2; - public static final double PIVOT_AMP_kP = 16; + public static final double PIVOT_AMP_kP = 37; // 11 public static final double PIVOT_AMP_kS = 0; - - public static final double PIVOT_kG = 0.17; - - - public static final double PIVOT_kP = 67; - public static final double PIVOT_kI = 0.4; - public static final double PIVOT_kD = 0.7; //0.2; - public static final double PIVOT_kS = 0.4; // 0.69673; + public static final double PIVOT_AMP_kD = 4; + public static final double PIVOT_AMP_kG = 0.4; + + public static final double PIVOT_kG = 0.65; + public static final double PIVOT_kP = 60; // 13 + public static final double PIVOT_kI = 0; //2.7 + public static final double PIVOT_kD = 6.5; //0; + public static final double PIVOT_kS = 5.0; // 0.69673; public static final double OFFSET_ANGLE = 21; public static final double CLIMB_ANGLE = 100; - public static final double AMP_ANGLE = 79; + public static final double AMP_ANGLE = 80; + public static final double QUICK_ANGLE = 50; public static final double PIVOT_GEAR_RATIO = 37.5; @@ -227,18 +228,21 @@ public static final class Pivot { public static final double STALLING_CURRENT = 50; // stalls when current is 50 - public static final double MAX_ERROR = 1.5; // degrees + public static final double MAX_ERROR_SPEAKER = 1.5; // degrees + public static final double MAX_ERROR_AMP = 2; + public static final double MAX_ERROR_CLOSE = 3; public static final double PIVOT_FORWARD_SOFT_LIMIT = 65 / 67.76 * 37.5; public static final double PIVOT_REVERSE_SOFT_LIMIT = 0; public static final double MAX_CRUISE_ACCLERATION = 817.03 / 2; - public static final double MAX_CRUISE_VElOCITY = 817.03 / 2; + public static final double MAX_CRUISE_VElOCITY = 817.03 * 0.8; public static enum Goal { AMP, SPEAKER, - CLIMB + CLIMB, + QUICK_PIVOT } } @@ -261,7 +265,7 @@ public static final class Elevator { public static final InvertedValue MASTER_INVERT = InvertedValue.Clockwise_Positive; - public static final double EXTEND_SPEED = 0.7; + public static final double EXTEND_SPEED = 0.5; public static final double MAX_ERROR = 2; // motor rotations } @@ -281,7 +285,7 @@ public static final class Intake { public static final double DEPLOY_kP = 0.07; public static final double INTAKE_DEPLOY = 23.5; // rotations - public static final double INTAKE_STALLING_CURRENT = 40; + public static final double INTAKE_STALLING_CURRENT = 70; public static final int ROLLER_CURRENT_LIMIT = 70; } diff --git a/src/main/java/frc/robot/commands/CommandGroups.java b/src/main/java/frc/robot/commands/CommandGroups.java index 3ec9a7b..8050c17 100644 --- a/src/main/java/frc/robot/commands/CommandGroups.java +++ b/src/main/java/frc/robot/commands/CommandGroups.java @@ -29,7 +29,8 @@ public static Command getFullZeroCommand() { public static Command getFullIntakeCommand() { return new MoveNoteToShooter() .raceWith(new IndexToShooter() - .alongWith(new IntakeNote())); + .alongWith(new IntakeNote())) + .andThen(new PivotToAngle(RobotMap.Pivot.Goal.QUICK_PIVOT), new ZeroPivot()); } public static Command getFullShootSpeaker() { diff --git a/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java b/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java index 024b72d..e566790 100644 --- a/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java +++ b/src/main/java/frc/robot/commands/drivetrain/SwerveManual.java @@ -70,9 +70,9 @@ public void execute() { // aligns to amp if (OI.getInstance().getDriver().getRightTrigger() > 0.5) { - // vx = -Drivetrain.getInstance().alignToAmp()[0]; + vx = -Drivetrain.getInstance().alignToAmp()[0]; // vy = -Drivetrain.getInstance().alignToAmp()[1]; - vx *= RobotMap.Drivetrain.EXTENDED_MAX_DRIVING_SPEED; + // vx *= RobotMap.Drivetrain.EXTENDED_MAX_DRIVING_SPEED; vy *= RobotMap.Drivetrain.EXTENDED_MAX_DRIVING_SPEED; omega = Drivetrain.getInstance().alignToAmp()[2]; Drivetrain.getInstance().setPreviousHeading(Drivetrain.getInstance().getPoseEstimatorPose2d().getRotation().getDegrees()); diff --git a/src/main/java/frc/robot/commands/pivot/PivotToAngle.java b/src/main/java/frc/robot/commands/pivot/PivotToAngle.java index bcf0653..4e39960 100644 --- a/src/main/java/frc/robot/commands/pivot/PivotToAngle.java +++ b/src/main/java/frc/robot/commands/pivot/PivotToAngle.java @@ -9,12 +9,17 @@ public class PivotToAngle extends Command { private RobotMap.Pivot.Goal setpoint; private double ref; + private boolean hasPivoted; public PivotToAngle(RobotMap.Pivot.Goal goal) { setpoint = goal; addRequirements(Pivot.getInstance()); } + public void initialize() { + hasPivoted = false; + } + public void execute() { switch (setpoint) { case SPEAKER: @@ -29,6 +34,10 @@ public void execute() { ref = RobotMap.Pivot.CLIMB_ANGLE; Pivot.getInstance().moveToPosition(ref); break; + case QUICK_PIVOT: + ref = RobotMap.Pivot.QUICK_ANGLE; + Pivot.getInstance().moveToPosition(ref); + break; } } @@ -40,9 +49,9 @@ public double getRef() { public boolean isFinished() { switch(setpoint) { case SPEAKER: - return MathUtil.compareSetpoint(Pivot.getInstance().getPosition(), ref, RobotMap.Pivot.MAX_ERROR); + return MathUtil.compareSetpoint(Pivot.getInstance().getPosition(), ref, RobotMap.Pivot.MAX_ERROR_SPEAKER); default: - return MathUtil.compareSetpoint(Pivot.getInstance().getPosition(), ref, RobotMap.Pivot.MAX_ERROR + 1); + return MathUtil.compareSetpoint(Pivot.getInstance().getPosition(), ref, RobotMap.Pivot.MAX_ERROR_AMP); } } diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 9823252..7aa62af 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Robot; import frc.robot.RobotMap; import frc.robot.util.MathUtil; // import frc.robot.util.Telemetry; @@ -89,6 +90,7 @@ private void configMotors() { masterConfig.Slot1.kP = RobotMap.Pivot.PIVOT_AMP_kP; masterConfig.Slot1.kS = RobotMap.Pivot.PIVOT_AMP_kS; + masterConfig.Slot1.kD = RobotMap.Pivot.PIVOT_AMP_kD; masterConfig.MotionMagic.MotionMagicCruiseVelocity = RobotMap.Pivot.MAX_CRUISE_VElOCITY; masterConfig.MotionMagic.MotionMagicAcceleration = RobotMap.Pivot.MAX_CRUISE_ACCLERATION; @@ -143,9 +145,9 @@ public void moveToPosition(double desiredAngle) { } public void moveToPositionAmp(double desiredAngle) { - double kG = RobotMap.Pivot.PIVOT_kG * Math.cos(Math.toRadians(getPosition() + RobotMap.Pivot.OFFSET_ANGLE)); + double kG = RobotMap.Pivot.PIVOT_AMP_kG * Math.cos(Math.toRadians(getPosition() + RobotMap.Pivot.OFFSET_ANGLE)); MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(desiredAngle / RobotMap.Pivot.PIVOT_ROT_TO_ANGLE); - master.setControl(motionMagicVoltage.withFeedForward(kG).withSlot(1)); + master.setControl(motionMagicVoltage.withFeedForward(kG).withSlot(RobotMap.PID.PID_AUXILIARY)); } public void setPercentOutput(double power) { diff --git a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java index 3d8edeb..61b8599 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java @@ -288,7 +288,7 @@ public double[] alignToAmp() { double vx = MathUtil.clamp(vxAmpController.calculate(refFieldRel.getX(), 0), -1, 1); double omega = MathUtil.clamp(omegaAmpController.calculate(getPoseEstimatorPose2d().getRotation().getRadians()), -1, 1); - double vy = MathUtil.clamp(vyAmpController.calculate(refFieldRel.getY(), Units.inchesToMeters(14 + 3)), -1, 1); + double vy = MathUtil.clamp(vyAmpController.calculate(refFieldRel.getY(), Units.inchesToMeters(14 + 7)), -1, 1); return new double[]{vx, vy, omega}; } @@ -421,7 +421,7 @@ public void periodic() { double x = Limelight.getDistanceToTag(); double stdX = .085 * x; - double stdTheta = .1 + .085 * x; + double stdTheta = .2 + .085 * x; if (Limelight.isPoseValid() && Limelight.isPoseNear(getPoseEstimatorPose2d(), visionBot)) { poseEstimator.addVisionMeasurement(visionBot, Limelight.getTimestamp(), VecBuilder.fill(stdX, stdX, stdTheta)); } diff --git a/src/main/java/frc/robot/util/Telemetry.java b/src/main/java/frc/robot/util/Telemetry.java index 4793f4f..7c4bd06 100644 --- a/src/main/java/frc/robot/util/Telemetry.java +++ b/src/main/java/frc/robot/util/Telemetry.java @@ -210,8 +210,8 @@ public void debug() { } public void shooter() { - // NetworkTableEntry shooterIndexProxSensor = _shooter.getEntry("Shooter Index Occupied"); - // shooterIndexProxSensor.setBoolean(shooter.shooterIndexerOccupied()); + NetworkTableEntry shooterIndexProxSensor = _shooter.getEntry("Shooter Index Occupied"); + shooterIndexProxSensor.setBoolean(shooter.shooterIndexerOccupied()); } public void elevator() { From 56af16a399ad90a7b61b7fee2fa0e20cab038df0 Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Mon, 25 Mar 2024 19:30:27 -0700 Subject: [PATCH 26/31] Tune semi-working four note auton --- src/main/java/frc/robot/RobotMap.java | 14 +++-- src/main/java/frc/robot/auton/Autons.java | 2 +- .../java/frc/robot/auton/Trajectories.java | 52 +++++++++---------- .../frc/robot/commands/CommandGroups.java | 4 +- .../robot/commands/pivot/PivotToAngle.java | 20 ++++--- .../frc/robot/commands/pivot/QuickPivot.java | 24 +++++++++ 6 files changed, 68 insertions(+), 48 deletions(-) create mode 100644 src/main/java/frc/robot/commands/pivot/QuickPivot.java diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index a2378bd..fdafefa 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -157,7 +157,7 @@ public static final class Drivetrain { public static double Y_kI = 0.0; public static double Y_kD = 0.0; - public static double THETA_kP = 1.8; // TODO + public static double THETA_kP = 2.3; // TODO public static double THETA_kI = 0.0; public static double THETA_kD = 0.0; @@ -186,7 +186,7 @@ public static final class Shooter { public static final double INDEXING_SPEED = 0.25; public static final double SHOOTING_SPEED = 0.8; - public static final double REVVED_RPS = 1000.0 / 60.0; + public static final double REVVED_RPS = 500.0 / 60.0; public static final double REVVED_AMP_RPS = 300.0 / 60.0; public static enum Goal { @@ -204,7 +204,7 @@ public static final class Pivot { public static final InvertedValue MASTER_INVERT = InvertedValue.CounterClockwise_Positive; - public static final double ZERO_SPEED = -0.2; + public static final double ZERO_SPEED = -0.35; public static final double PIVOT_AMP_kP = 37; // 11 public static final double PIVOT_AMP_kS = 0; @@ -212,15 +212,15 @@ public static final class Pivot { public static final double PIVOT_AMP_kG = 0.4; public static final double PIVOT_kG = 0.65; - public static final double PIVOT_kP = 60; // 13 + public static final double PIVOT_kP = 61; // 13 public static final double PIVOT_kI = 0; //2.7 public static final double PIVOT_kD = 6.5; //0; - public static final double PIVOT_kS = 5.0; // 0.69673; + public static final double PIVOT_kS = 0; // 0.69673; public static final double OFFSET_ANGLE = 21; public static final double CLIMB_ANGLE = 100; public static final double AMP_ANGLE = 80; - public static final double QUICK_ANGLE = 50; + public static final double QUICK_ANGLE = 45; public static final double PIVOT_GEAR_RATIO = 37.5; @@ -230,7 +230,6 @@ public static final class Pivot { public static final double MAX_ERROR_SPEAKER = 1.5; // degrees public static final double MAX_ERROR_AMP = 2; - public static final double MAX_ERROR_CLOSE = 3; public static final double PIVOT_FORWARD_SOFT_LIMIT = 65 / 67.76 * 37.5; public static final double PIVOT_REVERSE_SOFT_LIMIT = 0; @@ -242,7 +241,6 @@ public static enum Goal { AMP, SPEAKER, CLIMB, - QUICK_PIVOT } } diff --git a/src/main/java/frc/robot/auton/Autons.java b/src/main/java/frc/robot/auton/Autons.java index b8f17b9..43f43d5 100644 --- a/src/main/java/frc/robot/auton/Autons.java +++ b/src/main/java/frc/robot/auton/Autons.java @@ -25,7 +25,7 @@ public class Autons new SwervePositionController(Trajectories.note1ToShoot1_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()), new SwervePositionController(Trajectories.shoot1ToNote2_four, () -> Rotation2d.fromDegrees(180), false).alongWith(CommandGroups.getFullIntakeCommand()), new SwervePositionController(Trajectories.note2ToShoot2_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()), - new SwervePositionController(Trajectories.shoot2ToNote3_four, () -> Rotation2d.fromDegrees(-157.83), false).alongWith(CommandGroups.getFullIntakeCommand()), + new SwervePositionController(Trajectories.shoot2ToNote3_four, () -> Rotation2d.fromDegrees(-133.83), false).alongWith(CommandGroups.getFullIntakeCommand()), new SwervePositionController(Trajectories.note3ToShoot3_four, () -> Rotation2d.fromDegrees(-136.16), true).alongWith(CommandGroups.getFullShootSpeaker()) ); diff --git a/src/main/java/frc/robot/auton/Trajectories.java b/src/main/java/frc/robot/auton/Trajectories.java index 7d90cce..2addec4 100644 --- a/src/main/java/frc/robot/auton/Trajectories.java +++ b/src/main/java/frc/robot/auton/Trajectories.java @@ -79,61 +79,61 @@ public class Trajectories { */ public static Trajectory startToNote1_four = generateTrajectory( List.of(new Pose2d(1.28, 5.41, Rotation2d.fromDegrees(180)), - new Pose2d(2.57, 4.15, Rotation2d.fromDegrees(180))), - 5.0, - 4.0, + new Pose2d(2.45, 4.15, Rotation2d.fromDegrees(180))), + 3.0, + 1.5, 0.0, 0.0, true); public static Trajectory note1ToShoot1_four = generateTrajectory( - List.of(new Pose2d(2.57, 4.15, Rotation2d.fromDegrees(180)), - new Pose2d(1.90, 5.41, Rotation2d.fromDegrees(180))), + List.of(new Pose2d(2.45, 4.15, Rotation2d.fromDegrees(180)), + new Pose2d(2.1, 5.41, Rotation2d.fromDegrees(180))), 5.0, - 4.0, + 2.5, 0, 0, false); public static Trajectory shoot1ToNote2_four = generateTrajectory( - List.of(new Pose2d(1.90, 5.41, Rotation2d.fromDegrees(180)), - new Pose2d(2.73, 5.54, Rotation2d.fromDegrees(180))), - 5.0, + List.of(new Pose2d(2.1, 5.41, Rotation2d.fromDegrees(180)), + new Pose2d(2.6, 5.59, Rotation2d.fromDegrees(180))), + 5.0, 4.0, 0, 0, false); public static Trajectory note2ToShoot2_four = generateTrajectory( - List.of(new Pose2d(2.73, 5.54, Rotation2d.fromDegrees(180)), - new Pose2d(1.90, 5.96, Rotation2d.fromDegrees(-157.83))), + List.of(new Pose2d(2.6, 5.59, Rotation2d.fromDegrees(180)), + new Pose2d(2.1, 5.36, Rotation2d.fromDegrees(-157.83))), 5.0, - 4.0, + 2.5, 0.0, 0.0, false); public static Trajectory shoot2ToNote3_four = generateTrajectory( - List.of(new Pose2d(1.90, 5.96, Rotation2d.fromDegrees(-157.83)), - new Pose2d(2.73, 7.06, Rotation2d.fromDegrees(-136.16))), + List.of(new Pose2d(2.1, 5.36, Rotation2d.fromDegrees(-157.83)), + new Pose2d(2.6, 7.06, Rotation2d.fromDegrees(-133.83))), 5.0, 4.0, 0.0, 0.0, - false); + true); public static Trajectory note3ToShoot3_four = generateTrajectory( - List.of(new Pose2d(2.73, 7.06, Rotation2d.fromDegrees(-136.16)), - new Pose2d(1.90, 6.43, Rotation2d.fromDegrees(-143.16))), + List.of(new Pose2d(2.6, 7.06, Rotation2d.fromDegrees(-133.83)), + new Pose2d(2.1, 6.43, Rotation2d.fromDegrees(-136.16))), 5.0, - 4.0, + 2.5, 0.0, 0.0, false); - public static Trajectory shoot3ToEnd_four = generateTrajectory( - List.of(new Pose2d(1.90, 6.43, Rotation2d.fromDegrees(-143.16)), - new Pose2d(1.79, 5.43, Rotation2d.fromDegrees(180))), - 5.0, - 4.0, - 0.0, - 0.0, - false); + // public static Trajectory shoot3ToEnd_four = generateTrajectory( + // List.of(new Pose2d(1.90, 6.43, Rotation2d.fromDegrees(-136.16)), + // new Pose2d(1.79, 5.43, Rotation2d.fromDegrees(180))), + // 5.0, + // 4.0, + // 0.0, + // 0.0, + // false); /** * Six Note Path (Top) diff --git a/src/main/java/frc/robot/commands/CommandGroups.java b/src/main/java/frc/robot/commands/CommandGroups.java index 8050c17..9780d37 100644 --- a/src/main/java/frc/robot/commands/CommandGroups.java +++ b/src/main/java/frc/robot/commands/CommandGroups.java @@ -13,6 +13,7 @@ import frc.robot.commands.intake.IntakeNote; import frc.robot.commands.intake.ZeroIntake; import frc.robot.commands.pivot.PivotToAngle; +import frc.robot.commands.pivot.QuickPivot; import frc.robot.commands.pivot.ZeroPivot; import frc.robot.commands.shooter.MoveNoteToShooter; import frc.robot.commands.shooter.RevShooter; @@ -30,13 +31,12 @@ public static Command getFullIntakeCommand() { return new MoveNoteToShooter() .raceWith(new IndexToShooter() .alongWith(new IntakeNote())) - .andThen(new PivotToAngle(RobotMap.Pivot.Goal.QUICK_PIVOT), new ZeroPivot()); + .andThen(new QuickPivot(), new ZeroPivot()); } public static Command getFullShootSpeaker() { return new RevShooter(RobotMap.Shooter.Goal.SPEAKER) .alongWith(new PivotToAngle(RobotMap.Pivot.Goal.SPEAKER)) - // .alongWith(new IndexToShooter()) .andThen(new ShootNote()) .andThen(new ZeroPivot()); } diff --git a/src/main/java/frc/robot/commands/pivot/PivotToAngle.java b/src/main/java/frc/robot/commands/pivot/PivotToAngle.java index 4e39960..80baee2 100644 --- a/src/main/java/frc/robot/commands/pivot/PivotToAngle.java +++ b/src/main/java/frc/robot/commands/pivot/PivotToAngle.java @@ -1,5 +1,7 @@ package frc.robot.commands.pivot; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotMap; import frc.robot.subsystems.Pivot; @@ -9,17 +11,14 @@ public class PivotToAngle extends Command { private RobotMap.Pivot.Goal setpoint; private double ref; - private boolean hasPivoted; + private Debouncer debouncer; public PivotToAngle(RobotMap.Pivot.Goal goal) { setpoint = goal; + debouncer = new Debouncer(0.5, DebounceType.kRising); addRequirements(Pivot.getInstance()); } - public void initialize() { - hasPivoted = false; - } - public void execute() { switch (setpoint) { case SPEAKER: @@ -34,10 +33,6 @@ public void execute() { ref = RobotMap.Pivot.CLIMB_ANGLE; Pivot.getInstance().moveToPosition(ref); break; - case QUICK_PIVOT: - ref = RobotMap.Pivot.QUICK_ANGLE; - Pivot.getInstance().moveToPosition(ref); - break; } } @@ -47,12 +42,15 @@ public double getRef() { } public boolean isFinished() { + double error; switch(setpoint) { case SPEAKER: - return MathUtil.compareSetpoint(Pivot.getInstance().getPosition(), ref, RobotMap.Pivot.MAX_ERROR_SPEAKER); + error = RobotMap.Pivot.MAX_ERROR_SPEAKER; + break; default: - return MathUtil.compareSetpoint(Pivot.getInstance().getPosition(), ref, RobotMap.Pivot.MAX_ERROR_AMP); + error = RobotMap.Pivot.MAX_ERROR_AMP; } + return debouncer.calculate(MathUtil.compareSetpoint(Pivot.getInstance().getPosition(), ref, error)); } public void end(boolean interrupted) { diff --git a/src/main/java/frc/robot/commands/pivot/QuickPivot.java b/src/main/java/frc/robot/commands/pivot/QuickPivot.java new file mode 100644 index 0000000..c09c2cd --- /dev/null +++ b/src/main/java/frc/robot/commands/pivot/QuickPivot.java @@ -0,0 +1,24 @@ +package frc.robot.commands.pivot; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotMap; +import frc.robot.subsystems.Pivot; + +public class QuickPivot extends Command { + public QuickPivot() { + addRequirements(Pivot.getInstance()); + } + + public void execute() { + Pivot.getInstance().setPercentOutput(0.3); + } + + public boolean isFinished() { + return Pivot.getInstance().getPosition() >= RobotMap.Pivot.QUICK_ANGLE; + } + + public void end(boolean interrupted) { + Pivot.getInstance().setPercentOutput(0); + } + +} From e4aac00f1683f4ab1e5c4378bad480347aa4dffe Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Tue, 26 Mar 2024 18:01:21 -0700 Subject: [PATCH 27/31] Working autons! --- src/main/java/frc/robot/RobotMap.java | 17 ++++++------- src/main/java/frc/robot/auton/Autons.java | 7 +++--- .../java/frc/robot/auton/Trajectories.java | 24 +++++++++---------- .../robot/commands/pivot/PivotToAngle.java | 2 +- .../frc/robot/commands/pivot/QuickPivot.java | 2 +- .../robot/commands/shooter/RevShooter.java | 3 +++ src/main/java/frc/robot/subsystems/Pivot.java | 20 ++++++++-------- .../java/frc/robot/subsystems/Shooter.java | 4 ++++ 8 files changed, 44 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index fdafefa..e52dc1d 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -115,7 +115,7 @@ public static final class Drivetrain { public static final double MIN_OUTPUT = 0.05; // PID for omega (turning) control - public static final double OMEGA_kP = 2.5; // TODO + public static final double OMEGA_kP = 3.5; // TODO public static final double OMEGA_kI = 0.0; public static final double OMEGA_kD = 0.1; public static final double MAX_ERROR_SPEAKER = 1; @@ -149,7 +149,7 @@ public static final class Drivetrain { * PID values for X, Y, and Rotation (THETA) */ - public static double X_kP = 1.8; // TODO + public static double X_kP = 1.0; // TODO public static double X_kI = 0.0; public static double X_kD = 0.0; @@ -157,7 +157,7 @@ public static final class Drivetrain { public static double Y_kI = 0.0; public static double Y_kD = 0.0; - public static double THETA_kP = 2.3; // TODO + public static double THETA_kP = 5.3; // TODO public static double THETA_kI = 0.0; public static double THETA_kD = 0.0; @@ -184,9 +184,10 @@ public static final class Shooter { public static final int INDEXER_CURRENT_LIMIT = 80; public static final double INDEXING_SPEED = 0.25; - public static final double SHOOTING_SPEED = 0.8; + public static final double SHOOTING_SPEED = 1.0; - public static final double REVVED_RPS = 500.0 / 60.0; + public static final double AUTON_REVVED_RPS = 370.0 / 60.0; + public static final double REVVED_RPS = 1000.0 / 60.0; public static final double REVVED_AMP_RPS = 300.0 / 60.0; public static enum Goal { @@ -211,8 +212,8 @@ public static final class Pivot { public static final double PIVOT_AMP_kD = 4; public static final double PIVOT_AMP_kG = 0.4; - public static final double PIVOT_kG = 0.65; - public static final double PIVOT_kP = 61; // 13 + public static final double PIVOT_kG = 0.6; + public static final double PIVOT_kP = 57; // 13 public static final double PIVOT_kI = 0; //2.7 public static final double PIVOT_kD = 6.5; //0; public static final double PIVOT_kS = 0; // 0.69673; @@ -281,7 +282,7 @@ public static final class Intake { public static final double ROLLER_OUTAKE_SPEED = -0.8; public static final double DEPLOY_kP = 0.07; - public static final double INTAKE_DEPLOY = 23.5; // rotations + public static final double INTAKE_DEPLOY = 23; // rotations public static final double INTAKE_STALLING_CURRENT = 70; diff --git a/src/main/java/frc/robot/auton/Autons.java b/src/main/java/frc/robot/auton/Autons.java index 43f43d5..7883493 100644 --- a/src/main/java/frc/robot/auton/Autons.java +++ b/src/main/java/frc/robot/auton/Autons.java @@ -21,12 +21,13 @@ public class Autons public static final SequentialCommandGroup fourNotePath = new SequentialCommandGroup( CommandGroups.getFullZeroCommand(), CommandGroups.getFullShootSpeaker(), - new SwervePositionController(Trajectories.startToNote1_four, () -> Rotation2d.fromDegrees(133.13), false).alongWith(CommandGroups.getFullIntakeCommand()), + new SwervePositionController(Trajectories.startToNote1_four, () -> Rotation2d.fromDegrees(133), false).alongWith(CommandGroups.getFullIntakeCommand()), new SwervePositionController(Trajectories.note1ToShoot1_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()), new SwervePositionController(Trajectories.shoot1ToNote2_four, () -> Rotation2d.fromDegrees(180), false).alongWith(CommandGroups.getFullIntakeCommand()), new SwervePositionController(Trajectories.note2ToShoot2_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()), - new SwervePositionController(Trajectories.shoot2ToNote3_four, () -> Rotation2d.fromDegrees(-133.83), false).alongWith(CommandGroups.getFullIntakeCommand()), - new SwervePositionController(Trajectories.note3ToShoot3_four, () -> Rotation2d.fromDegrees(-136.16), true).alongWith(CommandGroups.getFullShootSpeaker()) + new SwervePositionController(Trajectories.shoot2ToNote3_four, () -> Rotation2d.fromDegrees(-133), false).alongWith(CommandGroups.getFullIntakeCommand()), + CommandGroups.getFullShootSpeaker() + // new SwervePositionController(Trajectories.note3ToShoot3_four, () -> Rotation2d.fromDegrees(180), true).alongWith(CommandGroups.getFullShootSpeaker()) ); public static final SequentialCommandGroup threeNotePath = new SequentialCommandGroup( diff --git a/src/main/java/frc/robot/auton/Trajectories.java b/src/main/java/frc/robot/auton/Trajectories.java index 2addec4..b49c3e4 100644 --- a/src/main/java/frc/robot/auton/Trajectories.java +++ b/src/main/java/frc/robot/auton/Trajectories.java @@ -80,48 +80,48 @@ public class Trajectories { public static Trajectory startToNote1_four = generateTrajectory( List.of(new Pose2d(1.28, 5.41, Rotation2d.fromDegrees(180)), new Pose2d(2.45, 4.15, Rotation2d.fromDegrees(180))), - 3.0, - 1.5, + 4.0, + 2.5, 0.0, 0.0, true); public static Trajectory note1ToShoot1_four = generateTrajectory( List.of(new Pose2d(2.45, 4.15, Rotation2d.fromDegrees(180)), - new Pose2d(2.1, 5.41, Rotation2d.fromDegrees(180))), + new Pose2d(1.4, 5.3, Rotation2d.fromDegrees(180))), 5.0, - 2.5, + 4.0, 0, 0, false); public static Trajectory shoot1ToNote2_four = generateTrajectory( - List.of(new Pose2d(2.1, 5.41, Rotation2d.fromDegrees(180)), + List.of(new Pose2d(1.4, 5.3, Rotation2d.fromDegrees(180)), new Pose2d(2.6, 5.59, Rotation2d.fromDegrees(180))), 5.0, 4.0, 0, 0, - false); + true); public static Trajectory note2ToShoot2_four = generateTrajectory( List.of(new Pose2d(2.6, 5.59, Rotation2d.fromDegrees(180)), - new Pose2d(2.1, 5.36, Rotation2d.fromDegrees(-157.83))), + new Pose2d(2.0, 6.0, Rotation2d.fromDegrees(180))), 5.0, - 2.5, + 4.0, 0.0, 0.0, false); public static Trajectory shoot2ToNote3_four = generateTrajectory( - List.of(new Pose2d(2.1, 5.36, Rotation2d.fromDegrees(-157.83)), - new Pose2d(2.6, 7.06, Rotation2d.fromDegrees(-133.83))), + List.of(new Pose2d(2.0, 6.0, Rotation2d.fromDegrees(180)), + new Pose2d(2.45, 7.06, Rotation2d.fromDegrees(180))), 5.0, 4.0, 0.0, 0.0, true); public static Trajectory note3ToShoot3_four = generateTrajectory( - List.of(new Pose2d(2.6, 7.06, Rotation2d.fromDegrees(-133.83)), + List.of(new Pose2d(2.45, 7.06, Rotation2d.fromDegrees(180)), new Pose2d(2.1, 6.43, Rotation2d.fromDegrees(-136.16))), 5.0, - 2.5, + 4.0, 0.0, 0.0, false); diff --git a/src/main/java/frc/robot/commands/pivot/PivotToAngle.java b/src/main/java/frc/robot/commands/pivot/PivotToAngle.java index 80baee2..d738dd6 100644 --- a/src/main/java/frc/robot/commands/pivot/PivotToAngle.java +++ b/src/main/java/frc/robot/commands/pivot/PivotToAngle.java @@ -15,7 +15,7 @@ public class PivotToAngle extends Command { public PivotToAngle(RobotMap.Pivot.Goal goal) { setpoint = goal; - debouncer = new Debouncer(0.5, DebounceType.kRising); + debouncer = new Debouncer(0.25, DebounceType.kRising); // 0.5 if not working addRequirements(Pivot.getInstance()); } diff --git a/src/main/java/frc/robot/commands/pivot/QuickPivot.java b/src/main/java/frc/robot/commands/pivot/QuickPivot.java index c09c2cd..1b9dd4f 100644 --- a/src/main/java/frc/robot/commands/pivot/QuickPivot.java +++ b/src/main/java/frc/robot/commands/pivot/QuickPivot.java @@ -10,7 +10,7 @@ public QuickPivot() { } public void execute() { - Pivot.getInstance().setPercentOutput(0.3); + Pivot.getInstance().setPercentOutput(0.45); } public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/shooter/RevShooter.java b/src/main/java/frc/robot/commands/shooter/RevShooter.java index 96f890c..7260679 100644 --- a/src/main/java/frc/robot/commands/shooter/RevShooter.java +++ b/src/main/java/frc/robot/commands/shooter/RevShooter.java @@ -1,5 +1,6 @@ package frc.robot.commands.shooter; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotMap; import frc.robot.subsystems.Shooter; @@ -29,6 +30,8 @@ public boolean isFinished() { case AMP: return Shooter.getInstance().isShooterAmpRevved(); case SPEAKER: + if (DriverStation.isAutonomous()) + return Shooter.getInstance().isAutonShooterSpeakerRevved(); return Shooter.getInstance().isShooterSpeakerRevved(); default: return false; diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 7aa62af..04e2c1d 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -58,16 +58,16 @@ private Pivot() { canCoder = new CANcoder(RobotMap.Pivot.CAN_CODER_ID); speakerAngles = new InterpolatingDoubleTreeMap(); - speakerAngles.put(0.0, 22.5); - speakerAngles.put(1.787, 22.5); - speakerAngles.put(2.043, 35.571); - speakerAngles.put(2.361, 37.5); - speakerAngles.put(2.839, 39.643); - speakerAngles.put(3.228, 42.574); - speakerAngles.put(3.713, 45.914); - speakerAngles.put(4.156, 46.5); - speakerAngles.put(4.507, 47.463); - speakerAngles.put(5.051, 48.990); + speakerAngles.put(0.0, 22.0); + speakerAngles.put(1.787, 22.1); + speakerAngles.put(2.043, 25.5); + speakerAngles.put(2.361, 30.0); + speakerAngles.put(2.839, 39.643 - 1.7 ); + speakerAngles.put(3.228, 42.574 - 2.0); + speakerAngles.put(3.713, 45.914 - 1.0); + speakerAngles.put(4.156, 46.5 - 1.0); + speakerAngles.put(4.507, 47.463 - 1.0); + speakerAngles.put(5.051, 48.990 - 1.0); configCANcoder(); configMotors(); } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 72749e2..2718e83 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -70,6 +70,10 @@ public boolean isShooterSpeakerRevved() { return master.getRotorVelocity().getValue() >= RobotMap.Shooter.REVVED_RPS; } + public boolean isAutonShooterSpeakerRevved() { + return master.getRotorVelocity().getValue() >= RobotMap.Shooter.AUTON_REVVED_RPS; + } + public boolean isShooterAmpRevved() { return master.getRotorVelocity().getValue() >= RobotMap.Shooter.REVVED_AMP_RPS; } From 470065fbc7cc2c3f2bcdf5730a9761730210245a Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Thu, 28 Mar 2024 19:46:05 -0700 Subject: [PATCH 28/31] MBR Day 1 --- src/main/java/frc/robot/OI.java | 37 ++++++++----------- src/main/java/frc/robot/RobotMap.java | 16 ++++---- src/main/java/frc/robot/auton/Autons.java | 4 +- .../frc/robot/commands/CommandGroups.java | 7 ++-- .../robot/commands/pivot/PivotToAngle.java | 2 +- .../frc/robot/commands/pivot/QuickPivot.java | 2 +- .../{ShootNote.java => ShootAmpNote.java} | 4 +- .../commands/shooter/ShootSpeakerNote.java | 26 +++++++++++++ src/main/java/frc/robot/subsystems/Pivot.java | 4 +- .../robot/subsystems/swerve/Drivetrain.java | 25 ++++++++++--- .../robot/subsystems/swerve/SwerveModule.java | 6 ++- src/main/java/frc/robot/util/Limelight.java | 8 ++-- src/main/java/frc/robot/util/Telemetry.java | 9 +++-- 13 files changed, 94 insertions(+), 56 deletions(-) rename src/main/java/frc/robot/commands/shooter/{ShootNote.java => ShootAmpNote.java} (88%) create mode 100644 src/main/java/frc/robot/commands/shooter/ShootSpeakerNote.java diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index b39204e..7e679cf 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -16,7 +16,7 @@ import frc.robot.commands.pivot.PivotToAngle; import frc.robot.commands.pivot.ZeroPivot; import frc.robot.commands.shooter.MoveNoteToShooter; -import frc.robot.commands.shooter.ShootNote; +import frc.robot.commands.shooter.ShootSpeakerNote; import frc.robot.subsystems.Pivot; import frc.robot.subsystems.swerve.Drivetrain; import frc.robot.util.Flip; @@ -46,36 +46,29 @@ public XboxGamepad getOperator() { } private void initBindings() { - if (RobotMap.FIRST_BOT) - { - driver.getLeftBumper().onTrue(CommandGroups.getFullShootAmp()); - driver.getRightBumper().onTrue(CommandGroups.getFullShootSpeaker()); - } - - driver.getUpDPadButton().onTrue(new PivotToAngle(Goal.SPEAKER)); - driver.getDownDPadButton().onTrue(new PivotToAngle(Goal.AMP)); - - driver.getButtonB().whileTrue(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0.1))); - driver.getButtonB().whileFalse(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0))); - // driver.getButtonA().onTrue(new AlignToStage("left")); - + driver.getRightBumper().onTrue(CommandGroups.getFullShootSpeaker()); + driver.getLeftBumper().onTrue(CommandGroups.getFullIntakeCommand()); + driver.getButtonA().onTrue(CommandGroups.getFullRetractClimb()); driver.getButtonY().onTrue(CommandGroups.getFullClimb()); - driver.getButtonA().whileTrue(CommandGroups.getFullRetractClimb()); + driver.getButtonB().onTrue(CommandGroups.getFullShootAmp()); + operator.getButtonY().onTrue(CommandGroups.getFullClimb()); + operator.getButtonA().whileTrue(new MoveToPosition(0)); + operator.getLeftBumper().onTrue(CommandGroups.getFullZeroCommand()); + // operator.getRightBumper().onTrue(CommandGroups.getFullShootAmp()); + + driver.getUpDPadButton().onTrue(new PivotToAngle(Goal.SPEAKER)); + driver.getDownDPadButton().onTrue(new PivotToAngle(Goal.AMP)); driver.getButtonStart().onTrue(new InstantCommand(() -> { Drivetrain.getInstance().toggleRobotCentric(); })); - driver.getButtonX().onTrue(new InstantCommand( () -> Drivetrain.getInstance().setPose(new Pose2d(1.28, 5.41, Rotation2d.fromDegrees(180))))); - - operator.getLeftBumper().onTrue(CommandGroups.getFullZeroCommand()); - operator.getButtonY().whileTrue(new MoveToPosition(RobotMap.Elevator.STAGE_HEIGHT * 0.95)); - - operator.getRightBumper().onTrue(CommandGroups.getFullIntakeCommand()); - //TESTING + // driver.getButtonB().whileTrue(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0.1))); + // driver.getButtonB().whileFalse(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0))); + // driver.getButtonA().onTrue(new AlignToStage("left")); // operator.getUpDPadButton().onTrue(CommandGroups.PRE_ALIGN_CLIMB); // operator.getDownDPadButton().onTrue(CommandGroups.POST_ALIGN_CLIMB); // operator.getRightDPadButton().onTrue(CommandGroups.FULL_SHOOT_TRAP); diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index e52dc1d..fc349c6 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -96,11 +96,11 @@ public static final class SwerveModule { // Translation FF Values public static final double TRANSLATION_kS = 0.13561; // TODO public static final double TRANSLATION_kV = 2.1051; // TODO - public static final double TRANSLATION_kA = 0.2737; // TODO + public static final double TRANSLATION_kA = 1.5737; // TODO // pid - public static final double TRANSLATION_kP = 0.27581; // TODO - public static final double TRANSLATION_kI = 0.00; // TODO + public static final double TRANSLATION_kP = 1.7; // TODO + public static final double TRANSLATION_kI = 0.5; // TODO public static final double TRANSLATION_kD = 0.00; // TODO public static final double MAX_SPEED = 5.0; @@ -115,10 +115,10 @@ public static final class Drivetrain { public static final double MIN_OUTPUT = 0.05; // PID for omega (turning) control - public static final double OMEGA_kP = 3.5; // TODO + public static final double OMEGA_kP = 10; // TODO public static final double OMEGA_kI = 0.0; public static final double OMEGA_kD = 0.1; - public static final double MAX_ERROR_SPEAKER = 1; + public static final double MAX_ERROR_SPEAKER = Math.toRadians(5); public static final double VX_AMP_kP = 0.5; public static final double VY_AMP_kP = 0.5; @@ -207,9 +207,9 @@ public static final class Pivot { public static final double ZERO_SPEED = -0.35; - public static final double PIVOT_AMP_kP = 37; // 11 + public static final double PIVOT_AMP_kP = 25; // 11 public static final double PIVOT_AMP_kS = 0; - public static final double PIVOT_AMP_kD = 4; + public static final double PIVOT_AMP_kD = 3.1; public static final double PIVOT_AMP_kG = 0.4; public static final double PIVOT_kG = 0.6; @@ -221,7 +221,7 @@ public static final class Pivot { public static final double CLIMB_ANGLE = 100; public static final double AMP_ANGLE = 80; - public static final double QUICK_ANGLE = 45; + public static final double QUICK_ANGLE = 30; public static final double PIVOT_GEAR_RATIO = 37.5; diff --git a/src/main/java/frc/robot/auton/Autons.java b/src/main/java/frc/robot/auton/Autons.java index 7883493..2174790 100644 --- a/src/main/java/frc/robot/auton/Autons.java +++ b/src/main/java/frc/robot/auton/Autons.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.RobotMap; import frc.robot.commands.CommandGroups; import frc.robot.commands.elevator.MoveToPosition; @@ -46,8 +47,7 @@ public class Autons public static final SequentialCommandGroup oneNote = new SequentialCommandGroup( CommandGroups.getFullZeroCommand(), CommandGroups.getFullShootSpeaker(), - new MoveToPosition(RobotMap.Elevator.STAGE_HEIGHT), - new MoveToPosition(0), + new WaitCommand(10), new SwervePositionController(Trajectories.note1_one, () -> Rotation2d.fromDegrees(180), false) ); diff --git a/src/main/java/frc/robot/commands/CommandGroups.java b/src/main/java/frc/robot/commands/CommandGroups.java index 9780d37..04602e9 100644 --- a/src/main/java/frc/robot/commands/CommandGroups.java +++ b/src/main/java/frc/robot/commands/CommandGroups.java @@ -17,7 +17,8 @@ import frc.robot.commands.pivot.ZeroPivot; import frc.robot.commands.shooter.MoveNoteToShooter; import frc.robot.commands.shooter.RevShooter; -import frc.robot.commands.shooter.ShootNote; +import frc.robot.commands.shooter.ShootAmpNote; +import frc.robot.commands.shooter.ShootSpeakerNote; import frc.robot.subsystems.Elevator; public class CommandGroups { @@ -37,14 +38,14 @@ public static Command getFullIntakeCommand() { public static Command getFullShootSpeaker() { return new RevShooter(RobotMap.Shooter.Goal.SPEAKER) .alongWith(new PivotToAngle(RobotMap.Pivot.Goal.SPEAKER)) - .andThen(new ShootNote()) + .andThen(new ShootSpeakerNote()) .andThen(new ZeroPivot()); } public static Command getFullShootAmp() { return new PivotToAngle(RobotMap.Pivot.Goal.AMP) .andThen(new RevShooter(RobotMap.Shooter.Goal.AMP)) - .andThen(new ShootNote()).andThen(new ZeroPivot()); + .andThen(new ShootAmpNote()).andThen(new ZeroPivot()); } public static Command getFullClimb() { diff --git a/src/main/java/frc/robot/commands/pivot/PivotToAngle.java b/src/main/java/frc/robot/commands/pivot/PivotToAngle.java index d738dd6..80134a1 100644 --- a/src/main/java/frc/robot/commands/pivot/PivotToAngle.java +++ b/src/main/java/frc/robot/commands/pivot/PivotToAngle.java @@ -15,7 +15,7 @@ public class PivotToAngle extends Command { public PivotToAngle(RobotMap.Pivot.Goal goal) { setpoint = goal; - debouncer = new Debouncer(0.25, DebounceType.kRising); // 0.5 if not working + debouncer = new Debouncer(0.35, DebounceType.kRising); // 0.5 if not working addRequirements(Pivot.getInstance()); } diff --git a/src/main/java/frc/robot/commands/pivot/QuickPivot.java b/src/main/java/frc/robot/commands/pivot/QuickPivot.java index 1b9dd4f..c09c2cd 100644 --- a/src/main/java/frc/robot/commands/pivot/QuickPivot.java +++ b/src/main/java/frc/robot/commands/pivot/QuickPivot.java @@ -10,7 +10,7 @@ public QuickPivot() { } public void execute() { - Pivot.getInstance().setPercentOutput(0.45); + Pivot.getInstance().setPercentOutput(0.3); } public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/shooter/ShootNote.java b/src/main/java/frc/robot/commands/shooter/ShootAmpNote.java similarity index 88% rename from src/main/java/frc/robot/commands/shooter/ShootNote.java rename to src/main/java/frc/robot/commands/shooter/ShootAmpNote.java index f4784d5..d7a4868 100644 --- a/src/main/java/frc/robot/commands/shooter/ShootNote.java +++ b/src/main/java/frc/robot/commands/shooter/ShootAmpNote.java @@ -4,8 +4,8 @@ import frc.robot.RobotMap; import frc.robot.subsystems.Shooter; -public class ShootNote extends Command { - public ShootNote() { +public class ShootAmpNote extends Command { + public ShootAmpNote() { addRequirements(Shooter.getInstance()); } diff --git a/src/main/java/frc/robot/commands/shooter/ShootSpeakerNote.java b/src/main/java/frc/robot/commands/shooter/ShootSpeakerNote.java new file mode 100644 index 0000000..7f7c51e --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/ShootSpeakerNote.java @@ -0,0 +1,26 @@ +package frc.robot.commands.shooter; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotMap; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.swerve.Drivetrain; + +public class ShootSpeakerNote extends Command { + public ShootSpeakerNote() { + addRequirements(Shooter.getInstance()); + } + + public void execute() { + if (Drivetrain.getInstance().alignedToSpeaker()) + Shooter.getInstance().setIndexer(RobotMap.Shooter.SHOOTING_SPEED); + } + + public boolean isFinished() { + return !Shooter.getInstance().shooterIndexerOccupied(); + } + + public void end(boolean interrupted) { + Shooter.getInstance().setShooter(0); + Shooter.getInstance().setIndexer(0); + } +} diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 04e2c1d..46b3722 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -58,8 +58,8 @@ private Pivot() { canCoder = new CANcoder(RobotMap.Pivot.CAN_CODER_ID); speakerAngles = new InterpolatingDoubleTreeMap(); - speakerAngles.put(0.0, 22.0); - speakerAngles.put(1.787, 22.1); + speakerAngles.put(0.0, 22.0 - .75); + speakerAngles.put(1.787, 22.1 + 1.25); speakerAngles.put(2.043, 25.5); speakerAngles.put(2.361, 30.0); speakerAngles.put(2.839, 39.643 - 1.7 ); diff --git a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java index 61b8599..79a9632 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/Drivetrain.java @@ -30,6 +30,7 @@ import edu.wpi.first.units.Velocity; import edu.wpi.first.units.Voltage; import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -68,7 +69,7 @@ public class Drivetrain extends SubsystemBase { public static PIDController omegaAmpController = new PIDController(RobotMap.Drivetrain.OMEGA_AMP_KP, 0, 0); // Standard deviations of pose estimate (x, y, heading) - private static Matrix stateStdDevs = VecBuilder.fill(0.2, 0.2, 0.01); // increase to trust encoder (state) + private static Matrix stateStdDevs = VecBuilder.fill(0.2, 0.2, 0.1); // increase to trust encoder (state) // measurements less private static Matrix visionStdDevs = VecBuilder.fill(0.5, 0.5, 0.5); // increase to trust vsion // measurements less @@ -266,14 +267,20 @@ public void setPreviousHeading(double prev) { } public double alignToSpeaker() { - Rotation2d refAngleFieldRel = Flip.apply(RobotMap.Field.SPEAKER) - .minus(getPoseEstimatorPose2d().getTranslation()).getAngle(); + double degRefSpeaker = getRefAngleSpeaker(); // Telemetry.putNumber("swerve", "Desired Omega", refAngleFieldRel.getRadians()); // Telemetry.putNumber("swerve", "Current Omega", getPoseEstimatorPose2d().getRotation().getRadians()); // return 0; return omegaSpeakerController.calculate(getPoseEstimatorPose2d().getRotation().getRadians(), - refAngleFieldRel.getRadians()); + Math.toRadians(degRefSpeaker)); + } + + public double getRefAngleSpeaker() { + Rotation2d refAngleFieldRel = Flip.apply(RobotMap.Field.SPEAKER) + .minus(getPoseEstimatorPose2d().getTranslation()).getAngle(); + + return refAngleFieldRel.getDegrees(); } public double getDistanceToSpeaker() { @@ -294,6 +301,8 @@ public double[] alignToAmp() { } public boolean alignedToSpeaker() { + if (DriverStation.isAutonomous()) + return true; return omegaSpeakerController.atSetpoint(); } @@ -408,6 +417,10 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return _sysId.dynamic(direction); } + public boolean isPoseValid() { + return Limelight.isPoseValid(Limelight.getBotPose2d()); + } + public boolean isPoseNear() { return Limelight.isPoseNear(getPoseEstimatorPose2d(), Limelight.getBotPose2d()); } @@ -421,8 +434,8 @@ public void periodic() { double x = Limelight.getDistanceToTag(); double stdX = .085 * x; - double stdTheta = .2 + .085 * x; - if (Limelight.isPoseValid() && Limelight.isPoseNear(getPoseEstimatorPose2d(), visionBot)) { + double stdTheta = .05 + .085 * x; + if (Limelight.isPoseValid(visionBot)) { poseEstimator.addVisionMeasurement(visionBot, Limelight.getTimestamp(), VecBuilder.fill(stdX, stdX, stdTheta)); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index b00faac..e3cda45 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -17,6 +17,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import frc.robot.RobotMap; // import frc.robot.util.Telemetry; +import frc.robot.util.Telemetry; public class SwerveModule { //motors on the swerve modules @@ -86,6 +87,7 @@ private void configTranslation() { transConfig.Slot0.kI = RobotMap.SwerveModule.TRANSLATION_kI; transConfig.Slot0.kD = RobotMap.SwerveModule.TRANSLATION_kD; + translation.getVelocity().setUpdateFrequency(250); translation.getPosition().setUpdateFrequency(250); translation.getConfigurator().apply(transConfig); } @@ -139,8 +141,8 @@ public void setAngleAndDrive(SwerveModuleState state) { driveVelocity.FeedForward = feedforward.calculate(state.speedMetersPerSecond); translation.setControl(driveVelocity); - // Telemetry.putModule(ID, "Desired Velocity", state.speedMetersPerSecond); - // Telemetry.putModule(ID, "Current Velocity", getSpeed()); + Telemetry.putModule(ID, "Desired Velocity", state.speedMetersPerSecond); + Telemetry.putModule(ID, "Current Velocity", getSpeed()); // Telemetry.putModule(ID, "Desired Angle", state.angle.getDegrees()); // Telemetry.putModule(ID, "Current Angle", getAngle()); diff --git a/src/main/java/frc/robot/util/Limelight.java b/src/main/java/frc/robot/util/Limelight.java index 4de619f..7a11acc 100644 --- a/src/main/java/frc/robot/util/Limelight.java +++ b/src/main/java/frc/robot/util/Limelight.java @@ -24,8 +24,9 @@ public static double getBestTargetArea() { return getValue("ta").getDouble(0.0); } - public static boolean isPoseValid() { - return getDistanceToTag() <= RobotMap.Camera.MAX_ERROR_VISION_POSE; + public static boolean isPoseValid(Pose2d visionPose) { + return getDistanceToTag() <= RobotMap.Camera.MAX_ERROR_VISION_POSE && + !MathUtil.compareDouble(visionPose.getTranslation().getNorm(), 0.0); } // @@ -102,8 +103,7 @@ else if (MathUtil.compareDouble(getApriltagId(), 16)) } public static boolean isPoseNear(Pose2d pose, Pose2d visionPose) { - return getDistanceBetweenPose(pose, visionPose) <= 1.0 - && !MathUtil.compareDouble(visionPose.getTranslation().getNorm(), 0.0); + return getDistanceBetweenPose(pose, visionPose) <= 1.0; } diff --git a/src/main/java/frc/robot/util/Telemetry.java b/src/main/java/frc/robot/util/Telemetry.java index 7c4bd06..484ff02 100644 --- a/src/main/java/frc/robot/util/Telemetry.java +++ b/src/main/java/frc/robot/util/Telemetry.java @@ -108,7 +108,7 @@ public void odometry() { isPoseNear.setBoolean(drive.isPoseNear()); NetworkTableEntry isPoseValid = _odometry.getEntry("Pose Validity"); - isPoseValid.setBoolean(Limelight.isPoseValid()); + isPoseValid.setBoolean(drive.isPoseValid()); NetworkTableEntry distanceToTag = _odometry.getEntry("Tag Distance"); distanceToTag.setDouble(Limelight.getDistanceToTag()); @@ -118,6 +118,9 @@ public void odometry() { NetworkTableEntry llPose = _odometry.getEntry("llPose"); llPose.setDoubleArray(new double[]{visionPose.getX(), visionPose.getY(), visionPose.getRotation().getDegrees()}); + + NetworkTableEntry refSpeakerDeg = _odometry.getEntry("Speaker Align Ref in Deg"); + refSpeakerDeg.setDouble(drive.getRefAngleSpeaker()); } public void debug() { @@ -244,7 +247,7 @@ public void pivot() { pivotSensorPosition.setDouble(pivot.getPosition()); NetworkTableEntry pivotRef = _pivot.getEntry("Pivot Speaker Setpoint"); - pivotRef.setDouble(pivot.getPivotSetpoint(Drivetrain.getInstance().getDistanceToSpeaker())); + pivotRef.setDouble(pivot.getPivotSetpoint(drive.getDistanceToSpeaker())); NetworkTableEntry pivotSensorVelocity = _pivot.getEntry("Pivot Sensor Velocity"); pivotSensorVelocity.setDouble(pivot.getVelocity()); @@ -282,7 +285,7 @@ public void vision() { tY.setDouble(Limelight.getTy()); NetworkTableEntry distance = _limelight.getEntry("Distance To Speaker"); - distance.setDouble(Drivetrain.getInstance().getDistanceToSpeaker()); + distance.setDouble(drive.getDistanceToSpeaker()); } From 1e1f588779bcfa7d47d9029fb853e1786c0d22fe Mon Sep 17 00:00:00 2001 From: 26brennar <26brennar@students.harker.org> Date: Mon, 8 Apr 2024 15:10:29 -0700 Subject: [PATCH 29/31] MBR Day 3 Changes --- .../pathplanner/paths/FourNotePath.path | 8 +- .../pathplanner/paths/New New Path.path | 58 ------- .../paths/{New Path.path => OneNotePath.path} | 24 ++- src/main/java/frc/robot/OI.java | 7 +- src/main/java/frc/robot/RobotMap.java | 8 +- .../commands/intake/OuttakeStuckNote.java | 8 +- .../robot/commands/pivot/PivotToAngle.java | 4 +- src/main/java/frc/robot/subsystems/Pivot.java | 23 +-- .../java/frc/robot/subsystems/Shooter.java | 1 + .../robot/util/AdvantageScope 3-29-2024.json | 145 ++++++++++++++++++ src/main/java/frc/robot/util/Telemetry.java | 4 +- src/main/java/frc/robot/util/limelight.vpr | 4 +- 12 files changed, 199 insertions(+), 95 deletions(-) delete mode 100644 src/main/deploy/pathplanner/paths/New New Path.path rename src/main/deploy/pathplanner/paths/{New Path.path => OneNotePath.path} (69%) create mode 100644 src/main/java/frc/robot/util/AdvantageScope 3-29-2024.json diff --git a/src/main/deploy/pathplanner/paths/FourNotePath.path b/src/main/deploy/pathplanner/paths/FourNotePath.path index c12faf4..124da77 100644 --- a/src/main/deploy/pathplanner/paths/FourNotePath.path +++ b/src/main/deploy/pathplanner/paths/FourNotePath.path @@ -112,12 +112,12 @@ }, { "anchor": { - "x": 1.79, - "y": 5.430145569998894 + "x": 1.7150425771027837, + "y": 4.810667177680737 }, "prevControl": { - "x": 1.7970951627420186, - "y": 6.020001367599977 + "x": 1.7221377398448023, + "y": 5.40052297528182 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/New New Path.path b/src/main/deploy/pathplanner/paths/New New Path.path deleted file mode 100644 index afe4e22..0000000 --- a/src/main/deploy/pathplanner/paths/New New Path.path +++ /dev/null @@ -1,58 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.0, - "y": 6.0 - }, - "prevControl": { - "x": 3.0, - "y": 6.0 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1, - "rotationDegrees": 45.0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 90.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/OneNotePath.path similarity index 69% rename from src/main/deploy/pathplanner/paths/New Path.path rename to src/main/deploy/pathplanner/paths/OneNotePath.path index 86ee0ed..a6881f9 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/OneNotePath.path @@ -16,12 +16,28 @@ }, { "anchor": { - "x": 4.3068121248935265, - "y": 7.3620736565316545 + "x": 1.2252231911871232, + "y": 6.750016462995888 }, "prevControl": { - "x": 3.3068121248935265, - "y": 7.3620736565316545 + "x": 0.9931733443713804, + "y": 6.462274652944366 + }, + "nextControl": { + "x": 1.8529756423768975, + "y": 7.528429502471214 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.613150954696971, + "y": 7.37191005246208 + }, + "prevControl": { + "x": 3.870591444886594, + "y": 7.223398150500005 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 7e679cf..367427e 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -13,6 +13,7 @@ // import frc.robot.commands.drivetrain.AlignToStage; // import frc.robot.commands.elevator.ElevatorManual; import frc.robot.commands.elevator.MoveToPosition; +import frc.robot.commands.intake.OuttakeStuckNote; import frc.robot.commands.pivot.PivotToAngle; import frc.robot.commands.pivot.ZeroPivot; import frc.robot.commands.shooter.MoveNoteToShooter; @@ -48,6 +49,8 @@ public XboxGamepad getOperator() { private void initBindings() { driver.getRightBumper().onTrue(CommandGroups.getFullShootSpeaker()); driver.getLeftBumper().onTrue(CommandGroups.getFullIntakeCommand()); + driver.getDownDPadButton().onTrue(CommandGroups.getFullZeroCommand()); + driver.getUpDPadButton().whileTrue(new OuttakeStuckNote()); driver.getButtonA().onTrue(CommandGroups.getFullRetractClimb()); driver.getButtonY().onTrue(CommandGroups.getFullClimb()); @@ -58,8 +61,8 @@ private void initBindings() { operator.getLeftBumper().onTrue(CommandGroups.getFullZeroCommand()); // operator.getRightBumper().onTrue(CommandGroups.getFullShootAmp()); - driver.getUpDPadButton().onTrue(new PivotToAngle(Goal.SPEAKER)); - driver.getDownDPadButton().onTrue(new PivotToAngle(Goal.AMP)); + driver.getRightDPadButton().onTrue(new PivotToAngle(Goal.SPEAKER)); + driver.getLeftDPadButton().onTrue(new PivotToAngle(Goal.AMP)); driver.getButtonStart().onTrue(new InstantCommand(() -> { Drivetrain.getInstance().toggleRobotCentric(); })); diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index fc349c6..3ed90bb 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -102,8 +102,6 @@ public static final class SwerveModule { public static final double TRANSLATION_kP = 1.7; // TODO public static final double TRANSLATION_kI = 0.5; // TODO public static final double TRANSLATION_kD = 0.00; // TODO - - public static final double MAX_SPEED = 5.0; } public static final class Drivetrain { @@ -188,7 +186,7 @@ public static final class Shooter { public static final double AUTON_REVVED_RPS = 370.0 / 60.0; public static final double REVVED_RPS = 1000.0 / 60.0; - public static final double REVVED_AMP_RPS = 300.0 / 60.0; + public static final double REVVED_AMP_RPS = 60.0 / 60.0; public static enum Goal { AMP, @@ -213,7 +211,7 @@ public static final class Pivot { public static final double PIVOT_AMP_kG = 0.4; public static final double PIVOT_kG = 0.6; - public static final double PIVOT_kP = 57; // 13 + public static final double PIVOT_kP = 62; // 13 public static final double PIVOT_kI = 0; //2.7 public static final double PIVOT_kD = 6.5; //0; public static final double PIVOT_kS = 0; // 0.69673; @@ -282,7 +280,7 @@ public static final class Intake { public static final double ROLLER_OUTAKE_SPEED = -0.8; public static final double DEPLOY_kP = 0.07; - public static final double INTAKE_DEPLOY = 23; // rotations + public static final double INTAKE_DEPLOY = 23.5; // rotations public static final double INTAKE_STALLING_CURRENT = 70; diff --git a/src/main/java/frc/robot/commands/intake/OuttakeStuckNote.java b/src/main/java/frc/robot/commands/intake/OuttakeStuckNote.java index bcc5a3e..c6a44d7 100644 --- a/src/main/java/frc/robot/commands/intake/OuttakeStuckNote.java +++ b/src/main/java/frc/robot/commands/intake/OuttakeStuckNote.java @@ -14,11 +14,8 @@ public OuttakeStuckNote() @Override public void execute() { - if (OI.getInstance().getOperator().getRightTrigger() > 0.5) - { - // Intake.getInstance().setDeployPos(0); - Intake.getInstance().setRollerPower(RobotMap.Intake.ROLLER_OUTAKE_SPEED); - } + Intake.getInstance().setDeployPos(RobotMap.Intake.INTAKE_DEPLOY); + Intake.getInstance().setRollerPower(RobotMap.Intake.ROLLER_OUTAKE_SPEED); } public boolean isFinished() { @@ -27,6 +24,7 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { + Intake.getInstance().setDeployPos(0); Intake.getInstance().setRollerPower(0); } diff --git a/src/main/java/frc/robot/commands/pivot/PivotToAngle.java b/src/main/java/frc/robot/commands/pivot/PivotToAngle.java index 80134a1..48d0a04 100644 --- a/src/main/java/frc/robot/commands/pivot/PivotToAngle.java +++ b/src/main/java/frc/robot/commands/pivot/PivotToAngle.java @@ -46,11 +46,11 @@ public boolean isFinished() { switch(setpoint) { case SPEAKER: error = RobotMap.Pivot.MAX_ERROR_SPEAKER; - break; + return debouncer.calculate(MathUtil.compareSetpoint(Pivot.getInstance().getPosition(), ref, error)); default: error = RobotMap.Pivot.MAX_ERROR_AMP; + return MathUtil.compareSetpoint(Pivot.getInstance().getPosition(), ref, error); } - return debouncer.calculate(MathUtil.compareSetpoint(Pivot.getInstance().getPosition(), ref, error)); } public void end(boolean interrupted) { diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 46b3722..1d1a7a6 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -9,6 +9,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; @@ -58,16 +59,16 @@ private Pivot() { canCoder = new CANcoder(RobotMap.Pivot.CAN_CODER_ID); speakerAngles = new InterpolatingDoubleTreeMap(); - speakerAngles.put(0.0, 22.0 - .75); - speakerAngles.put(1.787, 22.1 + 1.25); - speakerAngles.put(2.043, 25.5); - speakerAngles.put(2.361, 30.0); - speakerAngles.put(2.839, 39.643 - 1.7 ); - speakerAngles.put(3.228, 42.574 - 2.0); - speakerAngles.put(3.713, 45.914 - 1.0); - speakerAngles.put(4.156, 46.5 - 1.0); - speakerAngles.put(4.507, 47.463 - 1.0); - speakerAngles.put(5.051, 48.990 - 1.0); + speakerAngles.put(0.0, 10.0); + speakerAngles.put(1.787, 22.1 - 2.0); + speakerAngles.put(2.043, 25.5 - 2.0); + speakerAngles.put(2.361, 30.0 - 2.0); + speakerAngles.put(2.839, 39.643 - 5.0); + speakerAngles.put(3.228, 42.574 - 5.0); + speakerAngles.put(3.713, 45.914 - 5.0); + speakerAngles.put(4.156, 46.5 - 5.0); + speakerAngles.put(4.507, 47.463 - 2.0); + speakerAngles.put(5.051, 48.990 - 2.0); configCANcoder(); configMotors(); } @@ -155,7 +156,7 @@ public void setPercentOutput(double power) { { master.stopMotor(); } - DutyCycleOut percentOutput = new DutyCycleOut(power); + VoltageOut percentOutput = new VoltageOut(power * RobotMap.MAX_VOLTAGE); master.setControl(percentOutput); } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 2718e83..8ddca37 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotMap; +import frc.robot.subsystems.swerve.Drivetrain; public class Shooter extends SubsystemBase { private static Shooter instance; diff --git a/src/main/java/frc/robot/util/AdvantageScope 3-29-2024.json b/src/main/java/frc/robot/util/AdvantageScope 3-29-2024.json new file mode 100644 index 0000000..493161c --- /dev/null +++ b/src/main/java/frc/robot/util/AdvantageScope 3-29-2024.json @@ -0,0 +1,145 @@ +{ + "version": "3.0.2", + "layout": [ + { + "type": 5, + "fields": [], + "listFields": [ + [ + { + "type": "Robot", + "key": "NT:/SmartDashboard/Field/Robot", + "sourceTypeIndex": 0, + "sourceType": 5 + }, + { + "type": "Ghost", + "key": "NT:/TEAM 1072/Odometry/llPose", + "sourceTypeIndex": 0, + "sourceType": 5 + } + ] + ], + "options": { + "game": "2024 Field", + "unitDistance": "meters", + "unitRotation": "degrees", + "origin": "right", + "size": 0.75, + "allianceBumpers": "auto", + "allianceOrigin": "blue", + "orientation": "blue left, red right" + }, + "configHidden": false, + "title": "Odometry" + }, + { + "type": 1, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/TEAM 1072/Pivot/Pivot Angle", + "color": "#2b66a2", + "show": true + }, + { + "key": "NT:/TEAM 1072/Pivot/Pivot Speaker Setpoint", + "color": "#e5b31b", + "show": true + } + ] + }, + "discrete": { + "fields": [] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [] + } + }, + "title": "pivot" + }, + { + "type": 1, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/SmartDashboard/Field/Robot/2", + "color": "#2b66a2", + "show": true + }, + { + "key": "NT:/TEAM 1072/Odometry/Speaker Align Ref in Deg", + "color": "#e5b31b", + "show": true + } + ] + }, + "discrete": { + "fields": [] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [] + } + }, + "title": "rot" + }, + { + "type": 1, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "NT:/TEAM 1072/Drive/SwerveModuleStates/Measured/0/speed", + "color": "#2b66a2", + "show": true + }, + { + "key": "NT:/TEAM 1072/Drive/SwerveModuleStates/Setpoints Optimized/0/speed", + "color": "#e5b31b", + "show": true + } + ] + }, + "discrete": { + "fields": [] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [] + } + }, + "title": "Line Graph" + } + ] +} diff --git a/src/main/java/frc/robot/util/Telemetry.java b/src/main/java/frc/robot/util/Telemetry.java index 484ff02..99baf31 100644 --- a/src/main/java/frc/robot/util/Telemetry.java +++ b/src/main/java/frc/robot/util/Telemetry.java @@ -238,8 +238,8 @@ public void elevator() { } public void intake() { - // NetworkTableEntry intakeStalling = _intake.getEntry("Intake Stalling"); - // intakeStalling.setBoolean(intake.isStalling()); + NetworkTableEntry intakeStalling = _intake.getEntry("Intake Stalling"); + intakeStalling.setBoolean(intake.isStalling()); } public void pivot() { diff --git a/src/main/java/frc/robot/util/limelight.vpr b/src/main/java/frc/robot/util/limelight.vpr index f39df22..38a5837 100644 --- a/src/main/java/frc/robot/util/limelight.vpr +++ b/src/main/java/frc/robot/util/limelight.vpr @@ -62,12 +62,12 @@ multigroup_min:1 multigroup_rejector:0 pipeline_led_enabled:0 pipeline_led_power:0 -pipeline_res:2 +pipeline_res:3 pipeline_type:3 red_balance:1303 roi_x:0.000000 roi_y:0.000000 -rsf:1.35 +rsf:1.5 rspitch:-30.494 rsroll:0 rss:0 From a5b504e4b7156b51d13401ad36520796dd4b3f99 Mon Sep 17 00:00:00 2001 From: 25ChilingH <25ChilingH@students.harker.org> Date: Sat, 19 Oct 2024 10:55:41 -0700 Subject: [PATCH 30/31] Add ferrying --- .../deploy/pathplanner/paths/New Path.path | 65 +++++++++++++++++++ 1 file changed, 65 insertions(+) create mode 100644 src/main/deploy/pathplanner/paths/New Path.path diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..1316498 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.5482137470971375, + "y": 6.513777708545001 + }, + "prevControl": null, + "nextControl": { + "x": 6.548213747097137, + "y": 6.013777708545001 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.612227993615214, + "y": 6.028951994659942 + }, + "prevControl": { + "x": 4.7296587217802415, + "y": 6.138704806241458 + }, + "nextControl": { + "x": 2.204786894958409, + "y": 5.89071468489254 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.798129930709267, + "y": 1.7608511984137063 + }, + "prevControl": { + "x": 3.548129930709267, + "y": 3.2608511984137065 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 90.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -2.2217127139514274, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file From 01858976fa29e0e10abc070a0018601117db052a Mon Sep 17 00:00:00 2001 From: 25ChilingH <25ChilingH@students.harker.org> Date: Sat, 19 Oct 2024 10:56:29 -0700 Subject: [PATCH 31/31] Add ferrying --- src/main/java/frc/robot/OI.java | 5 ++--- src/main/java/frc/robot/commands/CommandGroups.java | 6 ++++++ 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 367427e..d1f2d0f 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -50,7 +50,7 @@ private void initBindings() { driver.getRightBumper().onTrue(CommandGroups.getFullShootSpeaker()); driver.getLeftBumper().onTrue(CommandGroups.getFullIntakeCommand()); driver.getDownDPadButton().onTrue(CommandGroups.getFullZeroCommand()); - driver.getUpDPadButton().whileTrue(new OuttakeStuckNote()); + driver.getUpDPadButton().onTrue(new InstantCommand( () -> Drivetrain.getInstance().setPose(new Pose2d(1.28, 5.41, Rotation2d.fromDegrees(180))))); driver.getButtonA().onTrue(CommandGroups.getFullRetractClimb()); driver.getButtonY().onTrue(CommandGroups.getFullClimb()); @@ -59,14 +59,13 @@ private void initBindings() { operator.getButtonY().onTrue(CommandGroups.getFullClimb()); operator.getButtonA().whileTrue(new MoveToPosition(0)); operator.getLeftBumper().onTrue(CommandGroups.getFullZeroCommand()); - // operator.getRightBumper().onTrue(CommandGroups.getFullShootAmp()); + driver.getButtonX().onTrue(CommandGroups.getFullShootNoAlign()); driver.getRightDPadButton().onTrue(new PivotToAngle(Goal.SPEAKER)); driver.getLeftDPadButton().onTrue(new PivotToAngle(Goal.AMP)); driver.getButtonStart().onTrue(new InstantCommand(() -> { Drivetrain.getInstance().toggleRobotCentric(); })); - driver.getButtonX().onTrue(new InstantCommand( () -> Drivetrain.getInstance().setPose(new Pose2d(1.28, 5.41, Rotation2d.fromDegrees(180))))); //TESTING // driver.getButtonB().whileTrue(new InstantCommand(() -> Pivot.getInstance().setPercentOutput(0.1))); diff --git a/src/main/java/frc/robot/commands/CommandGroups.java b/src/main/java/frc/robot/commands/CommandGroups.java index 04602e9..b0e010d 100644 --- a/src/main/java/frc/robot/commands/CommandGroups.java +++ b/src/main/java/frc/robot/commands/CommandGroups.java @@ -60,6 +60,12 @@ public static Command getFullRetractClimb() { .andThen(new InstantCommand(() -> Elevator.getInstance().setFollowerNeutralMode( new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake)))); } + + public static Command getFullShootNoAlign() { + return new RevShooter(RobotMap.Shooter.Goal.AMP) + .andThen(new ShootAmpNote()).andThen(new ZeroPivot()); + } + // public static final Command PRE_DRIVEFWD_CLIMB = new PivotToAngle(RobotMap.Pivot.Goal.TRAP1); // wait for drive forward