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 dd9d987..58decc2 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,18 +44,20 @@ 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; private NetworkTableEntry ntTestRPM; private NetworkTableEntry ntTestHood; + + private NetworkTableEntry ntConstantAiming; private NetworkTableEntry ntUseCalibrationMap; private NetworkTableEntry hotRPMAddition; - private NetworkTableEntry hotRPMReduction; + // private NetworkTableEntry hotRPMReduction; private NetworkTableEntry ntTeleopBuff; @@ -68,7 +70,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; @@ -101,7 +103,10 @@ 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); 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); @@ -180,7 +188,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/controls/manualdrive/ManualControls.java b/src/main/java/frc/robot/controls/manualdrive/ManualControls.java index ff1eb51..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 ); @@ -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/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()) diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 24389f0..7c7cb48 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; @@ -23,9 +29,9 @@ public class Shooter extends SubsystemBase { private final double RPMAcceptableDiff = 100; - private final double turretAcceptableDiff = 3; + private final double turretAcceptableDiff = 3.0; //+-degs - 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.075); + turrPidController.setI(0.00005); + turrPidController.setD(0.1); + turrPidController.setIZone(0.25); + turrPidController.setFF(0.01); + turrPidController.setOutputRange(-1.0, 1.0); + + turrPidController.setFeedbackDevice(turretMotor.getEncoder()); shooterMotorL.configFactoryDefault(); shooterMotorR.configFactoryDefault(); @@ -109,7 +129,15 @@ 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))) ; + + + ntIsTestingTurret = ntTable.getEntry("isTestingTurret"); + ntIsTestingTurret.setBoolean(false); + ntTestTurrestPos = ntTable.getEntry("ntTestTurrestPos"); + ntTestTurrestPos.setDouble(0.0); + } @Override @@ -122,28 +150,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 < -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); + } public void setHoodPos(double pos) { @@ -158,7 +173,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); } @@ -201,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; } }