From ab14d2023504cb8efaf7e3f19cd939a559c89a6f Mon Sep 17 00:00:00 2001 From: KrishWu Date: Sat, 22 Oct 2022 19:39:00 -0700 Subject: [PATCH 1/7] Fixed some turret stuff and yeah. --- .../frc/robot/commands/ShooterCommand.java | 12 ++-- .../java/frc/robot/subsystems/Shooter.java | 56 ++++++++++++------- 2 files changed, 41 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/commands/ShooterCommand.java b/src/main/java/frc/robot/commands/ShooterCommand.java index dd9d987..b8ed939 100644 --- a/src/main/java/frc/robot/commands/ShooterCommand.java +++ b/src/main/java/frc/robot/commands/ShooterCommand.java @@ -7,8 +7,8 @@ import com.revrobotics.ColorMatch; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.filter.Debouncer.DebounceType; +// import edu.wpi.first.math.filter.Debouncer; +// import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; @@ -44,8 +44,8 @@ public static interface Controls { private final boolean isRedAlliance; private boolean isAuto = false; - private boolean missBall = false; - private final double ballMissRPM = 300; + // private boolean missBall = false; + // private final double ballMissRPM = 300; private NetworkTable ntTable; private NetworkTable ntTableClimb; @@ -55,7 +55,7 @@ public static interface Controls { private NetworkTableEntry ntUseCalibrationMap; private NetworkTableEntry hotRPMAddition; - private NetworkTableEntry hotRPMReduction; + // private NetworkTableEntry hotRPMReduction; private NetworkTableEntry ntTeleopBuff; @@ -68,7 +68,7 @@ public static interface Controls { private int targetBallCount = -1; private double doneShootingFrames = 0; - private Debouncer debouncer = new Debouncer(2, DebounceType.kFalling); + // private Debouncer debouncer = new Debouncer(2, DebounceType.kFalling); private double rpmBuff; private final double rpmBuffZeta = 17; diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 24389f0..1750d13 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -7,7 +7,13 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.can.TalonFX; import com.revrobotics.CANSparkMax; +// import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.CANSparkMax.ControlType; + +// import edu.wpi.first.math.controller.PIDController; import com.revrobotics.CANSparkMaxLowLevel.MotorType; +// import com.revrobotics.SparkMaxPIDController.ArbFFUnits; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; @@ -25,7 +31,7 @@ public class Shooter extends SubsystemBase { private final double RPMAcceptableDiff = 100; private final double turretAcceptableDiff = 3; - private final double turretTurnSpeed = 0.35; + // private final double turretTurnSpeed = 0.35; private final TalonFX shooterMotorL; private final TalonFX shooterMotorR; @@ -45,8 +51,13 @@ public class Shooter extends SubsystemBase { private NetworkTable ntTableLimelight; private NetworkTableEntry ntShooterReady; + private SparkMaxPIDController turrPidController; + private int ballShotCount = 0; + private NetworkTableEntry ntIsTestingTurret; + private NetworkTableEntry ntTestTurrestPos; + /** Creates a new Shooter. */ public Shooter() { targetRPM = 0; @@ -60,7 +71,16 @@ public Shooter() { turretMotor = new CANSparkMax(RobotIds.SHOOTER_TURRET_MOTOR, MotorType.kBrushless); turretMotor.getEncoder().setPosition(0.0); + turrPidController = turretMotor.getPIDController(); + turrPidController.setP(0.225); + turrPidController.setI(0.00001); + turrPidController.setD(0.0); + turrPidController.setIZone(0.0); + turrPidController.setFF(0); + turrPidController.setOutputRange(-1.0, 1.0); + + turrPidController.setFeedbackDevice(turretMotor.getEncoder()); shooterMotorL.configFactoryDefault(); shooterMotorR.configFactoryDefault(); @@ -110,6 +130,13 @@ public Shooter() { .add("Num Balls Shot", this::getBallShotCount) ; + + + ntIsTestingTurret = ntTable.getEntry("isTestingTurret"); + ntIsTestingTurret.setBoolean(false); + ntTestTurrestPos = ntTable.getEntry("ntTestTurrestPos"); + ntTestTurrestPos.setDouble(0.0); + } @Override @@ -122,28 +149,15 @@ public void periodic() { shooterMotorL.set(ControlMode.Velocity, targetRPM / Constants.PhysicalConstants.RPM_PER_FALCON_UNIT); shooterMotorR.set(ControlMode.Velocity, targetRPM / Constants.PhysicalConstants.RPM_PER_FALCON_UNIT); - // shooterMotorL.set(ControlMode.PercentOutput, 0.5); - - double turretPosDiff = targetTurretPos - this.getTurretPosDegrees(); - - if(Math.abs(turretPosDiff) < turretAcceptableDiff){ - turretMotor.set(0.0); - }else{ - double speed = Math.abs(turretPosDiff) > turretAcceptableDiff * 3 ? - turretTurnSpeed : - Math.abs(turretPosDiff) > turretAcceptableDiff * 2 ? - turretTurnSpeed / (3 * (3 * turretAcceptableDiff - Math.abs(turretPosDiff)) / (turretAcceptableDiff)): // basically a gradient down from 1 to 1/3 - turretTurnSpeed / 3 - ; - - speed /= 1.333; - speed *= Math.copySign(1, turretPosDiff); - - //changed - turretMotor.set(speed); - // turretMotor.set(0.0); + // shooterMotorL.set(ControlMode.PercentOutput, 0.5); + if (ntIsTestingTurret.getBoolean(false)) { + targetTurretPos = ntTestTurrestPos.getDouble(0.0); } + if (targetTurretPos < -90) targetTurretPos = -90; + if (targetTurretPos > 90) targetTurretPos = 90; + turrPidController.setReference((targetTurretPos - startAngle) * 0.8888 / 45 / (5.33333 * 1.028571428571429) * 360 / 2, ControlType.kPosition); + } public void setHoodPos(double pos) { From ec2953d4b5bb22277bce3c609033ddd21e76d224 Mon Sep 17 00:00:00 2001 From: markzakharyan Date: Fri, 28 Oct 2022 17:32:09 -0700 Subject: [PATCH 2/7] fixed jitter --- src/main/java/frc/robot/commands/ShooterCommand.java | 2 +- src/main/java/frc/robot/subsystems/Shooter.java | 12 +++++++----- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/ShooterCommand.java b/src/main/java/frc/robot/commands/ShooterCommand.java index b8ed939..44d6839 100644 --- a/src/main/java/frc/robot/commands/ShooterCommand.java +++ b/src/main/java/frc/robot/commands/ShooterCommand.java @@ -180,7 +180,7 @@ public void execute() { turrTarget = 0; else if (controls.overrideTurretCenter()) shooter.setTurretPos(0); // override controlled turret pos - else shooter.setTurretDeltaPos(turrTarget); // limelight controlled turret pos; + else if (limelight.hasTarget()) shooter.setTurretDeltaPos(turrTarget); // limelight controlled turret pos; // ntTestHood.setDouble(targetHoodPos); }else{ diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 1750d13..1531a48 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -73,11 +73,11 @@ public Shooter() { turretMotor.getEncoder().setPosition(0.0); turrPidController = turretMotor.getPIDController(); - turrPidController.setP(0.225); - turrPidController.setI(0.00001); - turrPidController.setD(0.0); - turrPidController.setIZone(0.0); - turrPidController.setFF(0); + turrPidController.setP(0.075); + turrPidController.setI(0.00005); + turrPidController.setD(0.1); + turrPidController.setIZone(0.2); + turrPidController.setFF(0.01); turrPidController.setOutputRange(-1.0, 1.0); turrPidController.setFeedbackDevice(turretMotor.getEncoder()); @@ -172,7 +172,9 @@ public double getTurretSpeed() { return turretMotor.getEncoder().getVelocity(); } + // private static final double acceptableBullshit = 5.0; public void setTurretDeltaPos(double delta){ + // if (Math.abs(delta) > acceptableBullshit) setTurretPos(this.getTurretPosDegrees() + delta); } From 8280a7a85455b246fc9f5237d46387660f2abbc3 Mon Sep 17 00:00:00 2001 From: markzakharyan Date: Wed, 2 Nov 2022 18:58:29 -0700 Subject: [PATCH 3/7] fix offset and shooter never ready bug --- src/main/java/frc/robot/subsystems/Shooter.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 1531a48..7d3d078 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -29,7 +29,7 @@ public class Shooter extends SubsystemBase { private final double RPMAcceptableDiff = 100; - private final double turretAcceptableDiff = 3; + private final double turretAcceptableDiff = 3.3; //+-degs // private final double turretTurnSpeed = 0.35; @@ -129,6 +129,7 @@ public Shooter() { .add("Actual Hood", this::getHoodPos) .add("Num Balls Shot", this::getBallShotCount) + .add("turretOffsetPos", () -> (getTurretPos() - ((targetTurretPos - startAngle) * 0.8888 / 45 / (5.33333 * 1.028571428571429) * 360 / 2))) ; @@ -156,7 +157,7 @@ public void periodic() { if (targetTurretPos < -90) targetTurretPos = -90; if (targetTurretPos > 90) targetTurretPos = 90; - turrPidController.setReference((targetTurretPos - startAngle) * 0.8888 / 45 / (5.33333 * 1.028571428571429) * 360 / 2, ControlType.kPosition); + turrPidController.setReference((targetTurretPos - startAngle) * 0.8888 * 0.8888 / 45 / (5.33333 * 1.028571428571429) * 360 / 2, ControlType.kPosition); } @@ -217,6 +218,6 @@ public boolean isShooterReady(){ ntTableLimelight.getEntry("Has Target").getBoolean(false) && Math.abs(getShooterRpm()) > 2000 && Math.abs(getShooterRpm() - targetRPM) < RPMAcceptableDiff && - Math.abs(getTurretPosDegrees() - targetTurretPos) < turretAcceptableDiff; + Math.abs(getTurretPos() - ((targetTurretPos - startAngle) * 0.8888 / 45 / (5.33333 * 1.028571428571429) * 360 / 2)) < turretAcceptableDiff; } } From cc217afab764dfe69e65da8ea7f8d324e115fc7f Mon Sep 17 00:00:00 2001 From: markzakharyan Date: Fri, 4 Nov 2022 19:28:58 -0700 Subject: [PATCH 4/7] idk --- src/main/java/frc/robot/subsystems/Shooter.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 7d3d078..7c7cb48 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -29,7 +29,7 @@ public class Shooter extends SubsystemBase { private final double RPMAcceptableDiff = 100; - private final double turretAcceptableDiff = 3.3; //+-degs + private final double turretAcceptableDiff = 3.0; //+-degs // private final double turretTurnSpeed = 0.35; @@ -76,7 +76,7 @@ public Shooter() { turrPidController.setP(0.075); turrPidController.setI(0.00005); turrPidController.setD(0.1); - turrPidController.setIZone(0.2); + turrPidController.setIZone(0.25); turrPidController.setFF(0.01); turrPidController.setOutputRange(-1.0, 1.0); @@ -155,7 +155,7 @@ public void periodic() { targetTurretPos = ntTestTurrestPos.getDouble(0.0); } - if (targetTurretPos < -90) targetTurretPos = -90; + if (targetTurretPos < -70) targetTurretPos = -70; if (targetTurretPos > 90) targetTurretPos = 90; turrPidController.setReference((targetTurretPos - startAngle) * 0.8888 * 0.8888 / 45 / (5.33333 * 1.028571428571429) * 360 / 2, ControlType.kPosition); From 2513454716ed12b1b0f0600ea176cb7e8f08aa05 Mon Sep 17 00:00:00 2001 From: cookieMosnter <43896387+cookieMosnter@users.noreply.github.com> Date: Fri, 4 Nov 2022 19:44:27 -0700 Subject: [PATCH 5/7] broken --- src/main/java/frc/robot/subsystems/Shooter.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 7c7cb48..3245d59 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -217,7 +217,7 @@ public boolean isShooterReady(){ return ntTableLimelight.getEntry("Has Target").getBoolean(false) && Math.abs(getShooterRpm()) > 2000 && - Math.abs(getShooterRpm() - targetRPM) < RPMAcceptableDiff && - Math.abs(getTurretPos() - ((targetTurretPos - startAngle) * 0.8888 / 45 / (5.33333 * 1.028571428571429) * 360 / 2)) < turretAcceptableDiff; + Math.abs(getShooterRpm() - targetRPM) < RPMAcceptableDiff; + // Math.abs(getTurretPos() - ((targetTurretPos - startAngle) * 0.8888 / 45 / (5.33333 * 1.028571428571429) * 360 / 2)) < turretAcceptableDiff; } } From 4ce373719d97b8107c9113b399af64bcb7b130fb Mon Sep 17 00:00:00 2001 From: cookieMosnter <43896387+cookieMosnter@users.noreply.github.com> Date: Sun, 6 Nov 2022 08:44:43 -0800 Subject: [PATCH 6/7] mid comp --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/commands/ShooterCommand.java | 12 ++++++++++-- .../robot/controls/manualdrive/ManualControls.java | 2 +- src/main/java/frc/robot/subsystems/Shooter.java | 4 ++-- 4 files changed, 14 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b8662e0..14f23d4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -65,7 +65,7 @@ public static final class RobotIds { public static final int CONVEYOR_MOTOR_TOP = 16; public static final int CONVEYOR_MOTOR_BOTTOM = 15; - public static final int CONVEYOR_SENSOR = 0; + public static final int CONVEYOR_SENSOR = 1; public static final int SHOOTER_MOTOR_LEFT = 21; public static final int SHOOTER_MOTOR_RIGHT = 22; diff --git a/src/main/java/frc/robot/commands/ShooterCommand.java b/src/main/java/frc/robot/commands/ShooterCommand.java index 44d6839..ee8893e 100644 --- a/src/main/java/frc/robot/commands/ShooterCommand.java +++ b/src/main/java/frc/robot/commands/ShooterCommand.java @@ -51,6 +51,8 @@ public static interface Controls { private NetworkTable ntTableClimb; private NetworkTableEntry ntTestRPM; private NetworkTableEntry ntTestHood; + + private NetworkTableEntry ntConstantAiming; private NetworkTableEntry ntUseCalibrationMap; @@ -102,6 +104,9 @@ public ShooterCommand(Shooter shooter, Controls controls, Limelight limelight) { ntTeleopBuff = ntTable.getEntry("Teleop RPM Buff"); ntTeleopBuff.setDouble(0); + + ntConstantAiming = ntTable.getEntry("Constant Aiming Override"); + ntConstantAiming.setBoolean(true); isRedAlliance = NetworkTableInstance.getDefault().getTable("FMSInfo").getEntry("IsRedAlliance").getBoolean(false); @@ -136,10 +141,13 @@ public void execute() { // missBall = false; // } - limelight.setForceOff(!(controls.getAimShooter() || controls.getConstantAiming())); + boolean constantAiming = ntConstantAiming.getBoolean(true); + // if (!constantAiming) constantAiming = controls.getConstantAiming(); + + limelight.setForceOff(!(controls.getAimShooter() || constantAiming)); double dist = limelight.getDistance(); - if(controls.getAimShooter() || controls.getConstantAiming()) { + if(controls.getAimShooter() || constantAiming) { if (controls.getAimShooter()) { TeleOpBaseRPMBuff = ntTeleopBuff.getDouble(0.0); diff --git a/src/main/java/frc/robot/controls/manualdrive/ManualControls.java b/src/main/java/frc/robot/controls/manualdrive/ManualControls.java index ff1eb51..c33db75 100644 --- a/src/main/java/frc/robot/controls/manualdrive/ManualControls.java +++ b/src/main/java/frc/robot/controls/manualdrive/ManualControls.java @@ -64,7 +64,7 @@ public double getY() { @Override public double getSpeed() { - return speed.get(); + return speed.get() * (xbox.getRawButton(ControllerIds.XBOX_R_BUMPER) ? 1.25 : 1) * (xbox.getRawButton(ControllerIds.XBOX_L_BUMPER) ? 0.75 : 1); } @Override diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 3245d59..7c7cb48 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -217,7 +217,7 @@ public boolean isShooterReady(){ return ntTableLimelight.getEntry("Has Target").getBoolean(false) && Math.abs(getShooterRpm()) > 2000 && - Math.abs(getShooterRpm() - targetRPM) < RPMAcceptableDiff; - // Math.abs(getTurretPos() - ((targetTurretPos - startAngle) * 0.8888 / 45 / (5.33333 * 1.028571428571429) * 360 / 2)) < turretAcceptableDiff; + Math.abs(getShooterRpm() - targetRPM) < RPMAcceptableDiff && + Math.abs(getTurretPos() - ((targetTurretPos - startAngle) * 0.8888 / 45 / (5.33333 * 1.028571428571429) * 360 / 2)) < turretAcceptableDiff; } } From 87936348f6944aecaba1e76dd41ece8e45063440 Mon Sep 17 00:00:00 2001 From: markzakharyan Date: Sun, 6 Nov 2022 17:09:49 -0800 Subject: [PATCH 7/7] final commit :( --- src/main/java/frc/robot/commands/ShooterCommand.java | 2 +- .../java/frc/robot/controls/manualdrive/ManualControls.java | 4 ++-- src/main/java/frc/robot/subsystems/DriveTrain.java | 5 ++++- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/ShooterCommand.java b/src/main/java/frc/robot/commands/ShooterCommand.java index ee8893e..58decc2 100644 --- a/src/main/java/frc/robot/commands/ShooterCommand.java +++ b/src/main/java/frc/robot/commands/ShooterCommand.java @@ -103,7 +103,7 @@ public ShooterCommand(Shooter shooter, Controls controls, Limelight limelight) { hotRPMAddition.setDouble(35.0); ntTeleopBuff = ntTable.getEntry("Teleop RPM Buff"); - ntTeleopBuff.setDouble(0); + ntTeleopBuff.setDouble(60.0); ntConstantAiming = ntTable.getEntry("Constant Aiming Override"); ntConstantAiming.setBoolean(true); diff --git a/src/main/java/frc/robot/controls/manualdrive/ManualControls.java b/src/main/java/frc/robot/controls/manualdrive/ManualControls.java index c33db75..3723dd0 100644 --- a/src/main/java/frc/robot/controls/manualdrive/ManualControls.java +++ b/src/main/java/frc/robot/controls/manualdrive/ManualControls.java @@ -35,13 +35,13 @@ public ManualControls(Joystick xbox, Joystick controlStation) { this.controlStation = controlStation; this.speed = new PovNumberStepper( - new NumberStepper(0.5, 0.2, PhysicalConstants.MAX_SPEED, 0.1), + new NumberStepper(0.8, 0.2, PhysicalConstants.MAX_SPEED, 0.1), xbox, PovNumberStepper.PovDirection.VERTICAL ); this.turnSpeed = new PovNumberStepper( - new NumberStepper(0.5, 0.2, PhysicalConstants.MAX_TURN_SPEED, 0.05), + new NumberStepper(0.55, 0.2, PhysicalConstants.MAX_TURN_SPEED, 0.05), xbox, PovNumberStepper.PovDirection.HORIZONTAL ); diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 1022284..3bc8e34 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -77,7 +77,7 @@ public class DriveTrain extends SubsystemBase { private double kP, kI, kD, kFF; private NetworkTable ntTable; - private NetworkTableEntry ntPosition, ntspeed, ntP, ntI, ntD, ntFF, ntifTestingVelocity, ntifTestingRotation; + private NetworkTableEntry ntPosition, ntspeed, ntP, ntI, ntD, ntFF, ntifTestingVelocity, ntifTestingRotation, ntLeftSideBuff; private DifferentialDrivePoseEstimator estimator = new DifferentialDrivePoseEstimator(new Rotation2d(), new Pose2d(), new MatBuilder<>(Nat.N5(), Nat.N1()).fill(0.02, 0.02, 0.01, 0.02, 0.02), // State measurement standard deviations. X, Y, theta. @@ -130,6 +130,9 @@ public DriveTrain() { ntifTestingRotation = ntTable.getEntry("If testing Rotation"); ntifTestingRotation.setBoolean(false); + // ntLeftSideBuff = ntTable.getEntry("Left Side Buff"); + // ntLeftSideBuff.setDouble(1.0); + NtValueDisplay.ntDispTab("Drivetrain").add("L Actual Speed", this::getLVelocity).add("R Actual Speed", this::getRVelocity); NtValueDisplay.ntDispTab("Drivetrain") .add("Actual FF", () -> leftMotors[0].getPIDController().getFF())