diff --git a/build.gradle b/build.gradle index 965618d..57b3aa4 100644 --- a/build.gradle +++ b/build.gradle @@ -65,12 +65,6 @@ configurations.all { exclude group: "edu.wpi.first.wpilibj" } -//task(checkAkitInstall, dependsOn: "classes", type: JavaExec) { -// mainClass = "org.littletonrobotics.junction.CheckInstall" -// classpath = sourceSets.main.runtimeClasspath -//} -//compileJava.finalizedBy checkAkitInstall - // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 4. dependencies { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cf8616c..39be4fb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -62,4 +62,10 @@ public static class Drivetrain{ } + + public static class Intake { + public static int rollerID = 18; + public static int leadPivotID = 17; + public static int followerPivotID = 16; + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 24f9154..0bdf6ca 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,8 +25,11 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.DriveCommands; import frc.robot.commands.FeedForwardCharacterization; +import frc.robot.commands.IntakeCommands; import frc.robot.commands.ShooterCommands; import frc.robot.subsystems.drive.*; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.IntakeIOReal; import frc.robot.subsystems.limelight.Limelight; import frc.robot.subsystems.limelight.LimelightIOReal; import frc.robot.subsystems.shooter.*; @@ -45,6 +48,7 @@ public class RobotContainer { private final Shooter shooter; private final Limelight limelight; + private final Intake intake; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -69,8 +73,10 @@ public RobotContainer() { new Shooter( new FlywheelIOTalonFX(), new FlywheelIOSparkMaxSingle(1, 13, false, 20), - new FlywheelIOSparkMaxSingle(10, 14, true, 40)); + new FlywheelIOSparkMaxSingle(1.0/10.0, 14, true, 40)); limelight = new Limelight(new LimelightIOReal()); +// limelight = null; + intake = new Intake(new IntakeIOReal()); break; } @@ -87,6 +93,7 @@ public RobotContainer() { new ModuleIOSim()); shooter = null; limelight = null; + intake = null; break; } @@ -106,6 +113,7 @@ public RobotContainer() { }); shooter = null; limelight = null; + intake = null; break; } } @@ -134,6 +142,7 @@ private void configureButtonBindings() { // shooter.setDefaultCommand(new ShooterCommands.ShooterIdle(shooter)); shooter.setDefaultCommand(new ShooterCommands.ShooterTesting(shooter)); + intake.setDefaultCommand(new IntakeCommands.IntakeTesting(intake)); controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); controller.y().onTrue(new InstantCommand(() -> drive.resetPose())); // controller @@ -146,7 +155,10 @@ private void configureButtonBindings() { // drive) // .ignoringDisable(true)); - controller.a() + controller.a().onTrue(new IntakeCommands.IntakeDown(intake)); + controller.b().onTrue(new IntakeCommands.IntakeUp(intake)); + + controller.rightTrigger(0.7) .whileTrue(DriveCommands.LimelightRotDrive( drive, () -> -controller.getLeftX(), diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 146ec62..fbefba6 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -28,7 +28,7 @@ public class DriveCommands { private static final double DEADBAND = 0.1; - private static PIDController pid = new PIDController(0,0,0); + private static PIDController pid = new PIDController(1,0,0); private DriveCommands() {} @@ -88,7 +88,7 @@ public static Command LimelightRotDrive( double omega = MathUtil.applyDeadband(limelight.getTx() * limelight.getHorizontalFov()/(2*100), 0.01); // Square values linearMagnitude = linearMagnitude * linearMagnitude; - omega = Math.copySign(omega * omega, omega); +// omega = Math.copySign(omega * omega, omega); // Calcaulate new linear velocity Translation2d linearVelocity = diff --git a/src/main/java/frc/robot/commands/IntakeCommands.java b/src/main/java/frc/robot/commands/IntakeCommands.java new file mode 100644 index 0000000..362147e --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeCommands.java @@ -0,0 +1,58 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.intake.Intake; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; + +public class IntakeCommands { + public static class IntakeTesting extends CommandBase{ + LoggedDashboardNumber intakeAngle = new LoggedDashboardNumber("IntakeAngle", 0); + LoggedDashboardNumber rollerVolts = new LoggedDashboardNumber("RollerVolts", 0); + Intake intake; + public IntakeTesting(Intake intake){ + this.intake = intake; + addRequirements(intake); + } + + @Override + public void execute() { + intake.setPivotAngle(Rotation2d.fromRotations(intakeAngle.get())); + intake.rollerVoltage(rollerVolts.get()); + } + + } + + public static class IntakeUp extends CommandBase{ + Intake intake; + public IntakeUp(Intake intake){ + this.intake = intake; + addRequirements(intake); + } + + @Override + public void initialize() { + intake.setPivotAngle(Rotation2d.fromRotations(5)); + intake.rollerVoltage(0); + } + } + + public static class IntakeDown extends CommandBase{ + Intake intake; + public IntakeDown(Intake intake){ + this.intake = intake; + addRequirements(intake); + } + + @Override + public void initialize() { + intake.setPivotAngle(Rotation2d.fromRotations(24)); + intake.rollerVoltage(8); + } + } + + + + + +} diff --git a/src/main/java/frc/robot/commands/ShooterCommands.java b/src/main/java/frc/robot/commands/ShooterCommands.java index 7af40ef..13bafeb 100644 --- a/src/main/java/frc/robot/commands/ShooterCommands.java +++ b/src/main/java/frc/robot/commands/ShooterCommands.java @@ -99,6 +99,7 @@ public ShooterTesting(Shooter shooter) { @Override public void execute() { +// shooter.setMainVelocity(mainRPM.get()); shooter.setMainVelocity(mainRPM.get()); shooter.setSecondaryVelocity(secondaryRPM.get()); shooter.setLoadingVoltage(loadingVoltage.get()); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java new file mode 100644 index 0000000..1b47207 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -0,0 +1,64 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.util.LoggedTunableNumber; +import org.littletonrobotics.junction.Logger; + + +public class Intake extends SubsystemBase { + private static final boolean TUNING_MODE = true; + private final IntakeIO io; + + private final IntakeInputsAutoLogged inputs = new IntakeInputsAutoLogged(); + + private final LoggedTunableNumber kPLead = new LoggedTunableNumber("Intake/kPLead", 0.058); + private final LoggedTunableNumber kILead = new LoggedTunableNumber("Intake/kILead", 0); + private final LoggedTunableNumber kDLead = new LoggedTunableNumber("Intake/kDLead", 0.7); + + private final LoggedTunableNumber kPFollow = new LoggedTunableNumber("Intake/kPFollow", 0.045); + private final LoggedTunableNumber kIFollow = new LoggedTunableNumber("Intake/kIFollow", 0); + private final LoggedTunableNumber kDFollow = new LoggedTunableNumber("Intake/kDFollow", 3); + +// private final ArmFeedforward ff = new ArmFeedforward() + + private Rotation2d setpoint = new Rotation2d(); + + public Intake(IntakeIO io) { + this.io = io; + io.setPivotBreakMode(true); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.getInstance().processInputs("Intake", inputs); + + io.configureLeadPID(0.05, 0, .7); + io.configureSecondPID(0.045, 0, 4); + + + if (TUNING_MODE && (kPLead.hasChanged() || kILead.hasChanged() || kDLead.hasChanged())) { + io.configureLeadPID(kPLead.get(), kILead.get(), kDLead.get()); + } + if (TUNING_MODE && (kPFollow.hasChanged() || kIFollow.hasChanged() || kDFollow.hasChanged())) { + } + + Logger.getInstance().recordOutput("Intake/setpoint", setpoint.getRotations()); + } + + + public void rollerVoltage(double volts){ + io.setRollerVoltage(volts); + } + + public void setPivotAngle(Rotation2d angle){ + System.out.println("Setting Angle Subsystem"); + setpoint = angle; + io.setPivotPosition(angle); + } + + + +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java new file mode 100644 index 0000000..47fbcbf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -0,0 +1,29 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeIO { + @AutoLog + class IntakeInputs { + public double pivotVoltage = 0.0; + public double[] pivotCurrent = new double[] {}; + public double[] pivotPosition = new double[]{}; + public double pivotVelocity = 0.0; + public double rollerVoltage = 0.0; + public double rollerCurrent = 0.0; + } + + public default void updateInputs(IntakeInputs inputs) {} + + public default void setPivotVoltage(double volts) {} + + public default void setRollerVoltage(double volts) {} + + public default void setPivotBreakMode(boolean breakMode) {} + + public default void setPivotPosition(Rotation2d position){} + + public default void configureLeadPID(double kP, double kI, double kD){} + public default void configureSecondPID(double kP, double kI, double kD){} +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java new file mode 100644 index 0000000..c10efba --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java @@ -0,0 +1,114 @@ +package frc.robot.subsystems.intake; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel; +import com.revrobotics.SparkMaxPIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.Constants; + +public class IntakeIOReal implements IntakeIO { + private final TalonFX roller = new TalonFX(Constants.Intake.rollerID); + private final CANSparkMax leadSpark = + new CANSparkMax(Constants.Intake.leadPivotID, CANSparkMaxLowLevel.MotorType.kBrushless); + private final CANSparkMax followSpark = + new CANSparkMax(Constants.Intake.followerPivotID, CANSparkMaxLowLevel.MotorType.kBrushless); + private final SparkMaxPIDController leadSparkPID = leadSpark.getPIDController(); + private final SparkMaxPIDController followSparkPID = followSpark.getPIDController(); + + private final StatusSignal rollerDutyCycle = roller.getDutyCycle(); + private final StatusSignal rollerSupplyVoltage = roller.getSupplyVoltage(); + private final StatusSignal rollerCurrent = roller.getStatorCurrent(); + + public IntakeIOReal() { + // roller. + + leadSpark.restoreFactoryDefaults(); + followSpark.restoreFactoryDefaults(); + + leadSpark.getEncoder().setPosition(0); + followSpark.getEncoder().setPosition(0); + leadSpark.setInverted(false); + followSpark.setInverted(true); + + leadSpark.enableVoltageCompensation(12.0); + leadSpark.setSmartCurrentLimit(30); + followSpark.enableVoltageCompensation(12.0); + followSpark.setSmartCurrentLimit(30); + + leadSpark.burnFlash(); + followSpark.burnFlash(); + + var config = new TalonFXConfiguration(); + config.CurrentLimits.StatorCurrentLimit = 30.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + roller.getConfigurator().apply(config); + roller.setInverted(true); + + rollerDutyCycle.setUpdateFrequency(50.0); + rollerSupplyVoltage.setUpdateFrequency(50.0); + rollerCurrent.setUpdateFrequency(50.0); + } + + @Override + public void updateInputs(IntakeInputs inputs) { + inputs.pivotCurrent = + new double[] {leadSpark.getOutputCurrent(), followSpark.getOutputCurrent()}; + inputs.pivotVoltage = leadSpark.getBusVoltage() * leadSpark.getAppliedOutput(); + inputs.pivotPosition = new double[]{leadSpark.getEncoder().getPosition(), followSpark.getEncoder().getPosition()}; + inputs.pivotVelocity = leadSpark.getEncoder().getVelocity(); + + rollerDutyCycle.refresh(); + rollerSupplyVoltage.refresh(); + rollerCurrent.refresh(); + + inputs.rollerVoltage = rollerDutyCycle.getValue() * rollerSupplyVoltage.getValue(); + inputs.rollerCurrent = rollerCurrent.getValue(); + } + + @Override + public void setPivotVoltage(double volts) { + leadSpark.setVoltage(volts); + } + + @Override + public void setRollerVoltage(double volts) { + roller.setControl(new VoltageOut(volts)); + // roller.setVoltage(volts); + } + + @Override + public void setPivotBreakMode(boolean breakMode) { + CANSparkMax.IdleMode idleMode = CANSparkMax.IdleMode.fromId(breakMode ? 1 : 0); + leadSpark.setIdleMode(idleMode); + followSpark.setIdleMode(idleMode); + } + + @Override + public void configureLeadPID(double kP, double kI, double kD) { + leadSparkPID.setP(kP, 0); + leadSparkPID.setI(kI, 0); + leadSparkPID.setD(kD, 0); + leadSparkPID.setFF(0, 0); + } + + @Override + public void configureSecondPID(double kP, double kI, double kD){ + followSparkPID.setP(kP, 0); + followSparkPID.setI(kI, 0); + followSparkPID.setD(kD, 0); + followSparkPID.setFF(0, 0); + } + + @Override + public void setPivotPosition(Rotation2d position) { + System.out.println("Set Pivot Position IO"); + leadSparkPID.setReference(position.getRotations(), CANSparkMax.ControlType.kPosition); + followSparkPID.setReference(position.getRotations(), CANSparkMax.ControlType.kPosition);; + } +} diff --git a/src/main/java/frc/robot/subsystems/limelight/Limelight.java b/src/main/java/frc/robot/subsystems/limelight/Limelight.java index 42fd410..00d7d3a 100644 --- a/src/main/java/frc/robot/subsystems/limelight/Limelight.java +++ b/src/main/java/frc/robot/subsystems/limelight/Limelight.java @@ -4,6 +4,8 @@ import frc.robot.LimelightHelpers; import org.littletonrobotics.junction.Logger; +import java.util.ArrayList; + public class Limelight extends SubsystemBase { private final LimelightIO io; private final LimelightIOInputsAutoLogged inputs = new LimelightIOInputsAutoLogged(); @@ -23,9 +25,15 @@ public Limelight(LimelightIO io) { public void periodic() { io.updateInputs(inputs); Logger.getInstance().processInputs("Limelight", inputs); - if(getTv()) target = bestTargetMinDist(); + + + if(getTv()) target = bestTargetCentral(); Logger.getInstance().recordOutput("Limelight/targetDistance", getDistance()); + Logger.getInstance().recordOutput("Limelight/targetX", getTx()); + Logger.getInstance().recordOutput("Limelight/targetY", getTy()); + Logger.getInstance().recordOutput("Limelight/validTargetsLength", io.getValidTargets().size()); + Logger.getInstance().recordOutput("Limelight/allTargetsLength", io.getAllTargets().length); } public double getTa() { @@ -36,6 +44,9 @@ public double getTx() { if(getTv()) return target[3]; return 0; } + public double getTx(double[] target){ + return target[3]; + } public double getTy() { if(getTv()) return target[4]; @@ -43,7 +54,7 @@ public double getTy() { } public boolean getTv() { - return inputs.validTarget; + return inputs.validTarget && io.getValidTargets().size() > 0; } public double getDistance(){ @@ -56,16 +67,32 @@ public double getDistance(double[] target){ } private double[] bestTargetMinDist(){ - if (!getTv()) return null; - double[] minTarget = io.validTargets[0]; + if (!getTv()) return null ; + ArrayList targets = io.getValidTargets(); + double[] minTarget = targets.get(0); double min = getDistance(minTarget); - for (int i = 1; i < io.validTargets.length; i++) { - if(getDistance(io.validTargets[i]) < min){ - min = getDistance(io.validTargets[i]); - minTarget = io.validTargets[i]; + for (int i = 1; i < targets.size(); i++) { + if(getDistance(targets.get(i)) < min){ + min = getDistance(targets.get(i)); + minTarget = targets.get(i); + } + } + return minTarget; + } + + private double[] bestTargetCentral(){ + if (!getTv()) return null; + ArrayList targets = io.getValidTargets(); + double[] minTarget = targets.get(0); + double min = getTx(targets.get(0)); + for (int i = 1; i < targets.size(); i++) { + if(getTx(targets.get(i)) < min){ + min = getTx(targets.get(i)); + minTarget = targets.get(i); } } return minTarget; + } public boolean aimed(){ diff --git a/src/main/java/frc/robot/subsystems/limelight/LimelightIO.java b/src/main/java/frc/robot/subsystems/limelight/LimelightIO.java index 98556bc..2271119 100644 --- a/src/main/java/frc/robot/subsystems/limelight/LimelightIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/LimelightIO.java @@ -4,9 +4,11 @@ import org.littletonrobotics.junction.LogTable; import org.littletonrobotics.junction.inputs.LoggableInputs; +import java.util.ArrayList; + public interface LimelightIO { public double[][] allTargets = {}; - public double[][] validTargets = {}; + public ArrayList validTargets = new ArrayList<>(); @AutoLog public static class LimelightIOInputs{ @@ -25,4 +27,8 @@ public static class LimelightIOInputs{ } public default void updateInputs(LimelightIOInputs inputs) {} + + public double[][] getAllTargets(); + public ArrayList getValidTargets(); + } diff --git a/src/main/java/frc/robot/subsystems/limelight/LimelightIOReal.java b/src/main/java/frc/robot/subsystems/limelight/LimelightIOReal.java index 2d50b78..f35c30b 100644 --- a/src/main/java/frc/robot/subsystems/limelight/LimelightIOReal.java +++ b/src/main/java/frc/robot/subsystems/limelight/LimelightIOReal.java @@ -13,15 +13,15 @@ public class LimelightIOReal implements LimelightIO{ LimelightHelpers.LimelightResults limelightResults; public LimelightIOReal(){ - LimelightHelpers.setPipelineIndex("",9); + LimelightHelpers.setPipelineIndex("limelight-rear",9); LimelightHelpers.setLEDMode_PipelineControl(""); } @Override public void updateInputs(LimelightIOInputs inputs) { validTargets.clear(); - limelightResults = LimelightHelpers.getLatestResults(""); - inputs.fullDump = limelightResults.toString(); + limelightResults = LimelightHelpers.getLatestResults("limelight-rear"); + inputs.fullDump = "bruh"; inputs.validTarget = limelightResults.targetingResults.valid; inputs.captureLatencyMs = limelightResults.targetingResults.latency_capture; inputs.pipelineLatencyMs = limelightResults.targetingResults.latency_pipeline; @@ -36,10 +36,19 @@ public void updateInputs(LimelightIOInputs inputs) { LimelightHelpers.LimelightTarget_Detector target = limelightResults.targetingResults.targets_Detector[i]; double[] temp = new double[]{target.classID, target.confidence, target.ta, target.tx, target.ty}; allTargets[i] = temp; - if(target.classID == (DriverStation.getAlliance() == DriverStation.Alliance.Red?1.0:0)){ + if(target.classID == (DriverStation.getAlliance() == DriverStation.Alliance.Blue?1.0:0)){ validTargets1.add(temp); } } validTargets = validTargets1; } + + public double[][] getAllTargets(){ + return allTargets; + } + + @Override + public ArrayList getValidTargets() { + return validTargets; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOTalonFX.java index 451b9d8..a3f02f9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOTalonFX.java @@ -25,7 +25,7 @@ import edu.wpi.first.math.util.Units; public class FlywheelIOTalonFX implements FlywheelIO { - private static final double GEAR_RATIO = 22 / 16; + private static final double GEAR_RATIO = 16.0/22.0; private final TalonFX leader = new TalonFX(11); private final TalonFX follower = new TalonFX(12); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index f9a288f..6b18ec7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -18,9 +18,9 @@ public class Shooter extends SubsystemBase { private final LoggedTunableNumber kPMain = new LoggedTunableNumber("Shooter/kPMain", 0); private final LoggedTunableNumber kIMain = new LoggedTunableNumber("Shooter/kIMain", 0); - private final LoggedTunableNumber kDMain = new LoggedTunableNumber("Shooter/kDMain", 0); + private final LoggedTunableNumber kDMain = new LoggedTunableNumber("Shooter/kDMain", 0.001); - private final SimpleMotorFeedforward mainff = new SimpleMotorFeedforward(0.05470, 0.01792); + private final SimpleMotorFeedforward mainff = new SimpleMotorFeedforward(-0.02528, 0.01320); private final LoggedTunableNumber kPSecondary = new LoggedTunableNumber("Shooter/kPSecondary", 0); private final LoggedTunableNumber kISecondary = new LoggedTunableNumber("Shooter/kISecondary", 0);