diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e74bdd4..3ac60ea 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,18 +5,30 @@ package frc.robot; import com.dacubeking.AutoBuilder.robot.GuiAuto; +import com.dacubeking.AutoBuilder.robot.annotations.AutoBuilderAccessible; import com.dacubeking.AutoBuilder.robot.robotinterface.AutonomousContainer; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.RamseteController; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.subsystems.arm.Arm; +import frc.robot.subsystems.arm.ArmIO; +import frc.robot.subsystems.arm.ArmIOSparkMax; +import frc.robot.subsystems.climber.Climber; +import frc.robot.subsystems.climber.ClimberIO; +import frc.robot.subsystems.climber.ClimberIOSparkMax; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveIO; import frc.robot.subsystems.drive.DriveIOFalcon500; import frc.robot.subsystems.drive.DriveIOSim; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.IntakeIO; +import frc.robot.subsystems.intake.IntakeIOSparkMax; import org.jetbrains.annotations.Nullable; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; @@ -28,7 +40,10 @@ */ public class RobotContainer { // Subsystems - private final Drive drive; + private Drive drive; + private Intake intake; + private Arm arm; + private Climber climber; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -36,7 +51,14 @@ public class RobotContainer { private final LoggedDashboardChooser autoChooser = new LoggedDashboardChooser<>("Auto Choices"); private final LoggedDashboardChooser sideChooser = new LoggedDashboardChooser<>("Side Chooser"); - private final LoggedDashboardNumber flywheelSpeedInput = new LoggedDashboardNumber("Flywheel Speed", 1500.0); + private final LoggedDashboardNumber shootSpeed = new LoggedDashboardNumber("Shoot Speed", 1500.0); + private final LoggedDashboardNumber shootAngle = new LoggedDashboardNumber("Shoot Angle", 40); + private final LoggedDashboardNumber pickupAngle = new LoggedDashboardNumber("Pickup Angle", -20); + private final LoggedDashboardNumber ampScoringAngle = new LoggedDashboardNumber("Amp Scoring Angle", 40); + private final LoggedDashboardNumber ampScoringSpeed = new LoggedDashboardNumber("Amp Scoring Speed", 1000.0); + private final LoggedDashboardNumber intakeSpeed = new LoggedDashboardNumber("Intake Speed", 1000); + private final LoggedDashboardNumber ejectSpeed = new LoggedDashboardNumber("Eject Speed", -1000); + /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -44,17 +66,50 @@ public class RobotContainer { public RobotContainer() { switch (Constants.currentMode) { // Real robot, instantiate hardware IO implementations - case REAL -> drive = new Drive(new DriveIOFalcon500()); + case REAL -> { + drive = new Drive(new DriveIOFalcon500()); + intake = new Intake(new IntakeIOSparkMax()); + arm = new Arm(new ArmIOSparkMax()); + climber = new Climber(new ClimberIOSparkMax()); + } // drive = new Drive(new DriveIOFalcon500()); // Sim robot, instantiate physics sim IO implementations - case SIM -> drive = new Drive(new DriveIOSim()); + case SIM -> { + drive = new Drive(new DriveIOSim()); + intake = new Intake(new IntakeIOSparkMax()); + arm = new Arm(new ArmIOSparkMax()); + climber = new Climber(new ClimberIOSparkMax()); + } // Replayed robot, disable IO implementations - default -> drive = new Drive(new DriveIO() { - }); + default -> { + drive = new Drive(new DriveIO() { + }); + intake = new Intake(new IntakeIO() { + }); + arm = new Arm(new ArmIO() { + }); + climber = new Climber(new ClimberIO() { + }); + } } + // Configure the button bindings + configureButtonBindings(); + + intake.setDefaultCommand(new RunCommand( + () -> { + intake.setPrimaryIntakeVelocity(0); + intake.setSecondaryIntakeVelocity(0); + }, intake + )); + + arm.setDefaultCommand(new RunCommand( + () -> arm.setArmPosition(arm.getArmPosition()), + arm + )); + // Initialize autonomous container AutonomousContainer.getInstance().setDebugPrints(true); AutonomousContainer.getInstance().initialize( @@ -72,7 +127,7 @@ public RobotContainer() { drive ), drive::resetOdometry, - false, + true, this ); @@ -81,24 +136,87 @@ public RobotContainer() { sideChooser.addOption("Red", "red"); AutonomousContainer.getInstance().getAutonomousNames().forEach(name -> autoChooser.addOption(name, name)); - - // Configure the button bindings - configureButtonBindings(); } + + @AutoBuilderAccessible + Command intakeCommand = new RunCommand( + () -> arm.setArmPosition(pickupAngle.get()), arm) + .until(() -> Math.abs(arm.getArmPosition() - pickupAngle.get()) < 5) + .andThen(() -> { + intake.setPrimaryIntakeVelocity(intakeSpeed.get()); + intake.setSecondaryIntakeVelocity(intakeSpeed.get()); + }, intake, arm) + .until(intake::isBeamBroken); + + @AutoBuilderAccessible + Command outtakeCommand = new RunCommand( + () -> { + intake.setPrimaryIntakeVelocity(ejectSpeed.get()); + intake.setSecondaryIntakeVelocity(ejectSpeed.get()); + }, intake + ); + + @AutoBuilderAccessible + Command shootCommand = new RunCommand( + () -> { + intake.setPrimaryIntakeVelocity(shootSpeed.get()); + arm.setArmPosition(shootAngle.get()); + }, intake, arm) + .until(() -> Math.abs(arm.getArmPosition() - shootAngle.get()) < 5 + && Math.abs(intake.getPrimaryIntakeVelocity() - shootSpeed.get()) < 100) + .andThen(() -> { + intake.setSecondaryIntakeVelocity(3000); + intake.setPrimaryIntakeVelocity(shootSpeed.get()); + arm.setArmPosition(shootAngle.get()); + }, intake, arm); + + @AutoBuilderAccessible + Command scoreAmp = new RunCommand( + () -> arm.setArmPosition(ampScoringAngle.get()), arm) + .until(() -> Math.abs(arm.getArmPosition() - ampScoringAngle.get()) < 5) + .andThen(() -> { + intake.setPrimaryIntakeVelocity(ampScoringSpeed.get()); + intake.setSecondaryIntakeVelocity(ampScoringSpeed.get()); + }, intake, arm); + + @AutoBuilderAccessible + Command climbCommand = new StartEndCommand( + () -> climber.setClimberPower(-0.5), () -> climber.stopClimber(), climber + ); + + @AutoBuilderAccessible + Command stopClimbCommand = new RunCommand( + () -> climber.stopClimber(), climber + ); + + @AutoBuilderAccessible + Command deployClimberCommand = new StartEndCommand( + () -> climber.extendClimber(), () -> climber.setClimberPower(0), climber + ); + + /** * Use this method to define your button->command mappings. Buttons can be created by instantiating a * {@link GenericHID} or one of its subclasses ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), * and then passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - drive.setDefaultCommand( new RunCommand(() -> drive.driveArcade(-controller.getLeftY(), -controller.getRightX()), drive)); controller.leftBumper() .whileTrue(new RunCommand( () -> drive.driveArcade(-controller.getLeftY() / 2.0, -controller.getRightX() / 2.0), drive)); + + controller.rightTrigger().whileTrue(intakeCommand); + controller.leftTrigger().whileTrue(outtakeCommand); + + controller.a().whileTrue(scoreAmp); + controller.b().whileTrue(shootCommand); + + controller.povUp().whileTrue(deployClimberCommand); + controller.povDown().whileTrue(climbCommand); } /** diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java new file mode 100644 index 0000000..32ca831 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems.arm; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class Arm extends SubsystemBase { + + ArmIO io; + ArmInputsAutoLogged inputs = new ArmInputsAutoLogged(); + + public Arm(ArmIO io) { + this.io = io; + } + + private double targetPosition = 0.0; + private final ArmFeedforward feedforward = new ArmFeedforward(0.0, 0.0, 0.0); + + /** + * @param position The position to set the arm to in degrees + */ + public void setArmPosition(double position) { + targetPosition = position; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.getInstance().processInputs("Arm", inputs); + + double armffvoltage = feedforward.calculate(inputs.armPositionRad, inputs.armVelocityRadPerSec); + Logger.getInstance().recordOutput("ArmFFVoltage", armffvoltage); + + io.setArmPosition(targetPosition, armffvoltage); + } + + public double getArmPosition() { + return inputs.armPositionRad; + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java new file mode 100644 index 0000000..b1d9dc0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -0,0 +1,25 @@ +package frc.robot.subsystems.arm; + +import org.littletonrobotics.junction.AutoLog; + +public interface ArmIO { + @AutoLog + public static class ArmInputs { + public double armPositionRad = 0.0; + public double armVelocityRadPerSec = 0.0; + public double armCurrent = 0.0; + public double armTemperature = 0.0; + public double armVoltage = 0.0; + } + + /** + * Updates the set of loggable inputs. + */ + public default void updateInputs(ArmInputs inputs) { + + } + + public default void setArmPosition(double position, double ffvoltage) { + + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java new file mode 100644 index 0000000..b0c6ea0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java @@ -0,0 +1,43 @@ +package frc.robot.subsystems.arm; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.SparkMaxPIDController.ArbFFUnits; + +public class ArmIOSparkMax implements ArmIO { + + CANSparkMax arm; + CANSparkMax armFollower; + + public ArmIOSparkMax() { + arm = new CANSparkMax(30, MotorType.kBrushless); + arm.getEncoder().setPositionConversionFactor((1.0 / 100.0) * (16.0 / 62.0)); + arm.getEncoder().setVelocityConversionFactor((1.0 / 100.0) * (16.0 / 62.0) * (1.0 / 60.0) * 2.0 * Math.PI); + arm.enableVoltageCompensation(11.0); + arm.setSmartCurrentLimit(40); + + arm.getPIDController().setP(0.01); + arm.getPIDController().setI(0.0); + arm.getPIDController().setD(0.0); + arm.getPIDController().setFF(0.0); + + arm.setSmartCurrentLimit(40); + + armFollower = new CANSparkMax(31, MotorType.kBrushless); + armFollower.follow(arm, true); + + } + + public void updateInputs(ArmInputs inputs) { + inputs.armPositionRad = arm.getEncoder().getPosition(); + inputs.armVelocityRadPerSec = arm.getEncoder().getVelocity(); + inputs.armCurrent = arm.getOutputCurrent(); + inputs.armTemperature = arm.getMotorTemperature(); + inputs.armVoltage = arm.getBusVoltage(); + } + + public void setArmPosition(double position, double ffvoltage) { + arm.getPIDController().setReference(position, CANSparkMax.ControlType.kPosition, 0, + ffvoltage, ArbFFUnits.kVoltage); + } +} diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java new file mode 100644 index 0000000..761e797 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -0,0 +1,79 @@ +package frc.robot.subsystems.climber; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; + +public class Climber extends SubsystemBase { + + ClimberIO io; + ClimberInputsAutoLogged inputs = new ClimberInputsAutoLogged(); + + LoggedDashboardNumber climberStartPosition = new LoggedDashboardNumber("Climb Start Position", 32); + + public Climber(ClimberIO io) { + this.io = io; + } + + enum ClimberState { + IDLE, + CLIMBING, + TargetPosition, + + } + + private ClimberState state = ClimberState.IDLE; + private double targetPower = 0.0; + private double targetPosition = 0.0; + + public void setClimberPower(double voltage) { + state = ClimberState.CLIMBING; + targetPower = voltage; + } + + public void stopClimber() { + state = ClimberState.TargetPosition; + targetPower = 0.0; + targetPosition = getClimberPosition(); + } + + public void extendClimber() { + state = ClimberState.TargetPosition; + targetPower = 0.0; + targetPosition = 32; + } + + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.getInstance().processInputs("Climber", inputs); + + Logger.getInstance().recordOutput("ClimberState", state.toString()); + Logger.getInstance().recordOutput("ClimberTargetPower", targetPower); + Logger.getInstance().recordOutput("ClimberTargetPosition", targetPosition); + + switch (state) { + case IDLE -> { + io.setPrimaryClimberPower(0.0); + io.setSecondaryClimberPower(0.0); + } + case CLIMBING -> { + io.setPrimaryClimberPower(targetPower); + io.setSecondaryClimberPosition(inputs.climberPosition, targetPower); // PID the follower to the primary + } + case TargetPosition -> { + io.setPrimaryClimberPosition(targetPosition, 0.0); + io.setSecondaryClimberPosition(targetPosition, 0.0); + } + } + } + + public double getClimberVelocity() { + return (inputs.climberVelocity + inputs.climberFollowerVelocity) / 2d; + } + + public double getClimberPosition() { + return (inputs.climberPosition + inputs.climberFollowerPosition) / 2d; + } +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java new file mode 100644 index 0000000..a19b75e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -0,0 +1,45 @@ +package frc.robot.subsystems.climber; + +import org.littletonrobotics.junction.AutoLog; + +public interface ClimberIO { + @AutoLog + public class ClimberInputs { + public double climberPosition = 0.0; + public double climberVelocity = 0.0; + public double climberCurrent = 0.0; + public double climberTemperature = 0.0; + public double climberVoltage = 0.0; + + public double climberFollowerPosition = 0.0; + public double climberFollowerVelocity = 0.0; + public double climberFollowerCurrent = 0.0; + public double climberFollowerTemperature = 0.0; + public double climberFollowerVoltage = 0.0; + } + + /** + * Updates the set of loggable inputs. + */ + public default void updateInputs(ClimberInputs inputs) { + + } + + public default void setPrimaryClimberPower(double voltage) { + + } + + public default void setSecondaryClimberPower(double voltage) { + + } + + public default void setPrimaryClimberPosition(double position, double ffvoltage) { + + } + + public default void setSecondaryClimberPosition(double position, double ffvoltage) { + + } + + +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java new file mode 100644 index 0000000..30da31b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkMax.java @@ -0,0 +1,59 @@ +package frc.robot.subsystems.climber; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +public class ClimberIOSparkMax implements ClimberIO { + + CANSparkMax climberMotor; + CANSparkMax climberFollowerMotor; + + public ClimberIOSparkMax() { + climberMotor = new CANSparkMax(50, MotorType.kBrushless); + climberFollowerMotor = new CANSparkMax(51, MotorType.kBrushless); + + climberMotor.getEncoder().setPositionConversionFactor(1.0 / 42.0); //TODO + climberFollowerMotor.getEncoder().setPositionConversionFactor(1.0 / 42.0); //TODO + + climberMotor.getEncoder().setVelocityConversionFactor(1.0 / 42.0); //TODO + climberFollowerMotor.getEncoder().setVelocityConversionFactor(1.0 / 42.0); //TODO + + climberMotor.getPIDController().setP(0.01); + climberMotor.getPIDController().setI(0.0001); + climberMotor.getPIDController().setD(0.0); + + climberFollowerMotor.getPIDController().setP(0.01); + climberFollowerMotor.getPIDController().setI(0.0001); + climberFollowerMotor.getPIDController().setD(0.0); + } + + public void updateInputs(ClimberInputs inputs) { + inputs.climberPosition = climberMotor.getEncoder().getPosition(); + inputs.climberVelocity = climberMotor.getEncoder().getVelocity(); + inputs.climberCurrent = climberMotor.getOutputCurrent(); + inputs.climberTemperature = climberMotor.getMotorTemperature(); + inputs.climberVoltage = climberMotor.getBusVoltage(); + + inputs.climberFollowerPosition = climberFollowerMotor.getEncoder().getPosition(); + inputs.climberFollowerVelocity = climberFollowerMotor.getEncoder().getVelocity(); + inputs.climberFollowerCurrent = climberFollowerMotor.getOutputCurrent(); + inputs.climberFollowerTemperature = climberFollowerMotor.getMotorTemperature(); + inputs.climberFollowerVoltage = climberFollowerMotor.getBusVoltage(); + } + + public void setPrimaryClimberPower(double voltage) { + climberMotor.set(voltage); + } + + public void setSecondaryClimberPower(double voltage) { + climberFollowerMotor.set(voltage); + } + + public void setPrimaryClimberPosition(double position, double ffvoltage) { + climberMotor.getPIDController().setReference(position, CANSparkMax.ControlType.kPosition, 0, ffvoltage); + } + + public void setSecondaryClimberPosition(double position, double ffvoltage) { + climberFollowerMotor.getPIDController().setReference(position, CANSparkMax.ControlType.kPosition, 0, ffvoltage); + } +} 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..fdeb6a4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -0,0 +1,38 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class Intake extends SubsystemBase { + + IntakeIO io; + IntakeInputsAutoLogged inputs = new IntakeInputsAutoLogged(); + + public Intake(IntakeIO io) { + this.io = io; + } + + public void setPrimaryIntakeVelocity(double velocity) { + io.setPrimaryIntakeVelocity(velocity); + Logger.getInstance().recordOutput("PrimaryIntakeTargetVelocity", velocity); + } + + public void setSecondaryIntakeVelocity(double velocity) { + io.setSecondaryIntakeVelocity(velocity); + Logger.getInstance().recordOutput("SecondaryIntakeTargetVelocity", velocity); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.getInstance().processInputs("Intake", inputs); + } + + public double getPrimaryIntakeVelocity() { + return inputs.primaryIntakeVelocity; + } + + public boolean isBeamBroken() { + return inputs.isBeamBroken; + } +} 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..8c803fe --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -0,0 +1,35 @@ +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeIO { + @AutoLog + public static class IntakeInputs { + public double primaryIntakeVoltage = 0.0; + public double primaryIntakeCurrent = 0.0; + public double primaryIntakeVelocity = 0.0; + public double primaryIntakePosition = 0.0; + public double primaryIntakeTemperature = 0.0; + + + public double secondaryIntakeCurrent = 0.0; + public double secondaryIntakeVoltage = 0.0; + public double secondaryIntakeTemperature = 0.0; + public double secondaryIntakeVelocity = 0.0; + public double secondaryIntakePosition = 0.0; + + public boolean isBeamBroken = false; + } + + /** + * Updates the set of loggable inputs. + */ + public default void updateInputs(IntakeInputs inputs) { + } + + public default void setPrimaryIntakeVelocity(double velocity) { + } + + public default void setSecondaryIntakeVelocity(double velocity) { + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java new file mode 100644 index 0000000..b999aed --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -0,0 +1,67 @@ +package frc.robot.subsystems.intake; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import edu.wpi.first.wpilibj.DigitalInput; + +public class IntakeIOSparkMax implements IntakeIO { + CANSparkMax primaryIntake; + CANSparkMax secondaryIntake; + + DigitalInput beamBreak; + + public IntakeIOSparkMax() { + primaryIntake = new CANSparkMax(20, MotorType.kBrushless); + secondaryIntake = new CANSparkMax(22, MotorType.kBrushless); + beamBreak = new DigitalInput(0); // TODO: Does this exist? + + primaryIntake.getEncoder().setPositionConversionFactor(1.0 / 42.0); //TODO + secondaryIntake.getEncoder().setPositionConversionFactor(1.0 / 42.0); //TODO + + primaryIntake.getEncoder().setVelocityConversionFactor(1.0 / 42.0); //TODO + secondaryIntake.getEncoder().setVelocityConversionFactor(1.0 / 42.0); //TODO + + primaryIntake.enableVoltageCompensation(10.0); + secondaryIntake.enableVoltageCompensation(10.0); + + primaryIntake.setSmartCurrentLimit(40); + secondaryIntake.setSmartCurrentLimit(20); //neo 550 + + primaryIntake.getPIDController().setP(0.01); + primaryIntake.getPIDController().setI(0.0); + primaryIntake.getPIDController().setD(0.0); + primaryIntake.getPIDController().setFF(0.0); + + secondaryIntake.getPIDController().setP(0.01); + secondaryIntake.getPIDController().setI(0.0); + secondaryIntake.getPIDController().setD(0.0); + secondaryIntake.getPIDController().setFF(0.0); + + } + + @Override + public void updateInputs(IntakeInputs inputs) { + inputs.isBeamBroken = beamBreak.get(); + + inputs.primaryIntakeCurrent = primaryIntake.getOutputCurrent(); + inputs.primaryIntakeVoltage = primaryIntake.getBusVoltage(); + inputs.primaryIntakeVelocity = primaryIntake.getEncoder().getVelocity(); + inputs.primaryIntakePosition = primaryIntake.getEncoder().getPosition(); + inputs.primaryIntakeTemperature = primaryIntake.getMotorTemperature(); + + inputs.secondaryIntakeCurrent = secondaryIntake.getOutputCurrent(); + inputs.secondaryIntakeVoltage = secondaryIntake.getBusVoltage(); + inputs.secondaryIntakeVelocity = secondaryIntake.getEncoder().getVelocity(); + inputs.secondaryIntakePosition = secondaryIntake.getEncoder().getPosition(); + inputs.secondaryIntakeTemperature = secondaryIntake.getMotorTemperature(); + } + + public void setPrimaryIntakeVelocity(double velocity) { + primaryIntake.getPIDController().setReference(velocity, ControlType.kVelocity); + } + + public void setSecondaryIntakeVelocity(double velocity) { + secondaryIntake.getPIDController().setReference(velocity, ControlType.kVelocity); + } +}