diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fdbbf082..539791ab 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,19 +4,21 @@ package frc.robot; -import edu.wpi.first.wpilibj.XboxController; +//import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.IntakeBaseCommand; import frc.robot.subsystems.intake.IntakeBase; +import frc.robot.subsystems.intake.IntakeIOSim; public class RobotContainer { - public final XboxController controller; + private final CommandXboxController controller;// = new CommandXboxController(0); public final IntakeBase intake; public RobotContainer() { - controller = new XboxController(0); - + controller = new CommandXboxController(0); + boolean swerveBot = false; boolean tankBot = false; @@ -25,14 +27,14 @@ public RobotContainer() { } else if (tankBot) { intake = null; } else { - intake = new IntakeBase(); + intake = new IntakeBase(new IntakeIOSim()); } configureBindings(); } private void configureBindings() { - intake.setDefaultCommand(new IntakeBaseCommand(intake, () -> controller.getRightBumper(), () -> controller.getLeftBumper() )); + intake.setDefaultCommand(new IntakeBaseCommand(intake, () -> controller.rightBumper().getAsBoolean(), () -> controller.leftBumper().getAsBoolean() )); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/IntakeBaseCommand.java b/src/main/java/frc/robot/commands/IntakeBaseCommand.java index 039d9d34..7eee32ba 100644 --- a/src/main/java/frc/robot/commands/IntakeBaseCommand.java +++ b/src/main/java/frc/robot/commands/IntakeBaseCommand.java @@ -7,12 +7,12 @@ public class IntakeBaseCommand extends Command { IntakeBase intake; - BooleanSupplier rightBumper; - BooleanSupplier leftBumper; - public IntakeBaseCommand(IntakeBase intake, BooleanSupplier rightBumper, BooleanSupplier leftBumper) { + BooleanSupplier inEnable; + BooleanSupplier outEnable; + public IntakeBaseCommand(IntakeBase intake, BooleanSupplier inEnable, BooleanSupplier outEnable) { this.intake = intake; - this.rightBumper = rightBumper; - this.leftBumper = leftBumper; + this.inEnable = inEnable; + this.outEnable = inEnable; addRequirements(intake); } @@ -25,22 +25,27 @@ public void initialize() { @Override public void execute() { // Turns off motors if No/All bumpers - if ((rightBumper.getAsBoolean() && leftBumper.getAsBoolean()) || (rightBumper.getAsBoolean() == false && leftBumper.getAsBoolean() == false)) { + if (inEnable.getAsBoolean() == true && outEnable.getAsBoolean() == true) { // Disable the intake motors intake.disable(); - - } else if (rightBumper.getAsBoolean()) { // Motors on (IN) if right bumper pressed - intake.setSpeed(1); + //System.out.println(outEnable.getAsBoolean()); + } else if(inEnable.getAsBoolean() == false && outEnable.getAsBoolean() == false) { + intake.disable(); + //System.out.println(outEnable.getAsBoolean()); + } else if (inEnable.getAsBoolean() == true) { // Motors on (IN) if right bumper pressed + intake.setVoltage(1); + System.out.println(outEnable.getAsBoolean()); // Enable motors, It has to be called regularly for voltage compensation to work properly intake.enable(); - } else if (leftBumper.getAsBoolean()) { // Motors on (Out) if right bumper pressed - intake.setSpeed(-1); - + } else if (outEnable.getAsBoolean()== true) { // Motors on (Out) if right bumper pressed + intake.setVoltage(-1); + System.out.println(outEnable.getAsBoolean()); // Enable motors, It has to be called regularly for voltage compensation to work properly intake.enable(); } else { // Disable the intake motors intake.disable(); } + //System.out.println(outEnable.getAsBoolean()); } } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index adcc6099..fb67daab 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -11,10 +11,6 @@ public default void disable() {} */ public default void enable() {} - /** - * Set the speed of the motor for the intake. - * - * @param speed The speed to set. Value should be between -1.0 and 1.0. - */ - public default void setSpeed(double speed) {} + + public default void setVoltage(double volts) {} } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeBase.java b/src/main/java/frc/robot/subsystems/intake/IntakeBase.java index cb9628c6..d21a8acb 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeBase.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeBase.java @@ -1,33 +1,40 @@ package frc.robot.subsystems.intake; -import edu.wpi.first.wpilibj.motorcontrol.MotorController; -import edu.wpi.first.wpilibj.motorcontrol.Spark; +import org.littletonrobotics.junction.Logger; + +//import frc.robot.subsystems.intake.IntakeIO; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class IntakeBase extends SubsystemBase implements Intake { - private final MotorController IntakeMotor; - + private final IntakeIO IO; + private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); private double voltage; - public IntakeBase() { - IntakeMotor = new Spark(0); + public IntakeBase(IntakeIO IO) { + + this.IO = IO; + voltage = 0; } @Override public void disable() { voltage = 0; - IntakeMotor.setVoltage(voltage); + IO.setVoltage(voltage); } @Override public void enable() { - IntakeMotor.setVoltage(voltage); + IO.setVoltage(voltage); } @Override - //Sets the voltage to 5.0 times speed. the speed value is -1 1 - public void setSpeed(double speed) { - voltage = 5.0 * speed; + public void setVoltage(double volts) { + voltage = volts; + } + @Override + public void periodic() { + IO.updateInputs(inputs); + Logger.processInputs("Intake", inputs); } } 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 00000000..7673a4f8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -0,0 +1,18 @@ +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeIO { + @AutoLog + public static class IntakeIOInputs { + public double velocityRadPerSec = 0.0; + public double appliedVolts = 0.0; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(IntakeIOInputs inputs) {} + + /** Run open loop at the specified voltage. */ + public default void setVoltage(double volts) {} +} + diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java new file mode 100644 index 00000000..05de8edc --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -0,0 +1,36 @@ +package frc.robot.subsystems.intake; + +//import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; + +public class IntakeIOSim implements IntakeIO{ + private FlywheelSim sim = new FlywheelSim(DCMotor.getNEO(1), 1.5, 0.004); + private double appliedVolts = 0.0; + + @Override + public void updateInputs(IntakeIOInputs inputs) { + + + sim.update(0.02); + + //inputs.positionRad = 0.0; + inputs.velocityRadPerSec = sim.getAngularVelocityRadPerSec(); + inputs.appliedVolts = appliedVolts; + //inputs.currentAmps = new double[] {sim.getCurrentDrawAmps()}; + + + + } + + @Override + public void setVoltage(double volts) { + appliedVolts = volts; + sim.setInputVoltage(volts); + } + + + + +} +