diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3ee7dba..e89b1ed 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,14 +22,14 @@ public final class Constants { */ public static final class Motors { public static final class DriveTrainMotors { - public static final int FRMOTOR = 0; - public static final int FLMOTOR = 1; - public static final int BRMOTOR = 2; - public static final int BLMOTOR = 3; + public static final int FRMOTOR = 5; + public static final int FLMOTOR = 6; + public static final int BRMOTOR = 4; + public static final int BLMOTOR = 7; } public static final class IntakeMotors { - public static final int LMOTOR = 4; - public static final int RMOTOR = 5; + public static final int LMOTOR = 1; + public static final int RMOTOR = 2; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 00e7752..49732c5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,13 +1,16 @@ package frc.robot; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Robot.RobotRunType; import frc.robot.commands.Drive; +import frc.robot.commands.IntakeCommand; +import frc.robot.commands.OutTakeCommand; +import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.drive.Drivetrain; import frc.robot.subsystems.drive.DrivetrainIO; import frc.robot.subsystems.drive.DrivetrainReal; @@ -20,12 +23,13 @@ */ public class RobotContainer { /* Controllers */ - XboxController driver = new XboxController(0); + CommandXboxController driver = new CommandXboxController(0); // Initialize AutoChooser Sendable private final SendableChooser autoChooser = new SendableChooser<>(); /* Subsystems */ private Drivetrain driveTrain; + private Intake intake; /** @@ -45,6 +49,7 @@ public RobotContainer(RobotRunType runtimeType) { driveTrain = new Drivetrain(new DrivetrainIO() {}); } // Configure the button bindings + driveTrain.setDefaultCommand(new Drive(driveTrain, driver)); configureButtonBindings(); } @@ -58,8 +63,9 @@ public RobotContainer(RobotRunType runtimeType) { ; private void configureButtonBindings() { + driver.rightTrigger().whileTrue(new OutTakeCommand(intake, driver)); + driver.leftTrigger().whileTrue(new IntakeCommand(intake, driver)); - driveTrain.setDefaultCommand(new Drive(driveTrain, driver)); } diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index 1c97b37..dc80940 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -1,7 +1,7 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drivetrain; /** @@ -9,14 +9,14 @@ */ public class Drive extends Command { - private XboxController controller; + private CommandXboxController controller; private Drivetrain drive; /** * Drive again. */ - public Drive(Drivetrain drive, XboxController controller) { + public Drive(Drivetrain drive, CommandXboxController controller) { this.controller = controller; this.drive = drive; addRequirements(drive); @@ -26,7 +26,7 @@ public Drive(Drivetrain drive, XboxController controller) { public void execute() { double leftY = (Math.abs(controller.getLeftY()) < .05) ? 0 : controller.getLeftY(); double rightY = (Math.abs(controller.getRightY()) < .05) ? 0 : controller.getRightY(); - this.drive.setPower(leftY, rightY); + drive.setPower(leftY, rightY); } } diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java new file mode 100644 index 0000000..c3e58dd --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeCommand.java @@ -0,0 +1,23 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.subsystems.Intake.Intake; + +public class IntakeCommand extends Command { + private CommandXboxController controller; + private Intake intake; + + public IntakeCommand(Intake intake, CommandXboxController controller) { + this.controller = controller; + this.intake = intake; + addRequirements(intake); + } + + @Override + public void execute() { + intake.setIntakeVoltage(1); + controller.leftTrigger().whileTrue(new InstantCommand(() -> intake.setIntakeVoltage(-1))); + } +} diff --git a/src/main/java/frc/robot/commands/OutTakeCommand.java b/src/main/java/frc/robot/commands/OutTakeCommand.java new file mode 100644 index 0000000..d91251b --- /dev/null +++ b/src/main/java/frc/robot/commands/OutTakeCommand.java @@ -0,0 +1,24 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.subsystems.Intake.Intake; + +public class OutTakeCommand extends Command { + private CommandXboxController controller; + private Intake intake; + + public OutTakeCommand(Intake intake, CommandXboxController controller) { + this.controller = controller; + this.intake = intake; + addRequirements(intake); + } + + @Override + public void execute() { + intake.setIntakeVoltage(-1); + controller.rightTrigger().whileTrue(new InstantCommand(() -> intake.setIntakeVoltage(1))); + controller.leftTrigger().whileTrue(new InstantCommand(() -> intake.setIntakeVoltage(-1))); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/DrivetrainReal.java b/src/main/java/frc/robot/subsystems/drive/DrivetrainReal.java index 2acfeed..6a76367 100644 --- a/src/main/java/frc/robot/subsystems/drive/DrivetrainReal.java +++ b/src/main/java/frc/robot/subsystems/drive/DrivetrainReal.java @@ -22,7 +22,6 @@ public DrivetrainReal() { FRMotor.addFollower(BRMotor); FRMotor.setInverted(true); - BRMotor.setInverted(true); } @@ -35,8 +34,8 @@ public void updateInputs(DrivetrainIOInputs inputs) { * Drive Voltage */ public void setDriveVoltage(double lvolts, double rvolts) { - FRMotor.setVoltage(lvolts); - FRMotor.setVoltage(rvolts); + FLMotor.set(lvolts); + FRMotor.set(rvolts); } } diff --git a/src/main/java/frc/robot/subsystems/intake/intake.java b/src/main/java/frc/robot/subsystems/intake/intake.java index cc91626..ca8b9b8 100644 --- a/src/main/java/frc/robot/subsystems/intake/intake.java +++ b/src/main/java/frc/robot/subsystems/intake/intake.java @@ -1,5 +1,22 @@ -package frc.robot.subsystems.intake; +package frc.robot.subsystems.Intake; -public class intake { +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Intake extends SubsystemBase { + private IntakeIO io; + private IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); + + public Intake(IntakeIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + } + + public void setIntakeVoltage(int power) { + io.setIntakeVoltage(power); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/intakeIO.java b/src/main/java/frc/robot/subsystems/intake/intakeIO.java index e5a8ca3..deb44b3 100644 --- a/src/main/java/frc/robot/subsystems/intake/intakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/intakeIO.java @@ -1,18 +1,19 @@ -package frc.robot.subsystems.intake; +package frc.robot.subsystems.Intake; import org.littletonrobotics.junction.AutoLog; -public interface intakeIO { +public interface IntakeIO { @AutoLog - public static class intakeIOInputs { + public static class IntakeIOInputs { public double rightMotor; public double leftMotor; } /** Updates the set of loggable inputs. */ + public default void updateInputs(IntakeIOInputs inputs) {} - public default void intakePower(double lvolts, double rvolts) {} + public default void setIntakeVoltage(double power) {} } diff --git a/src/main/java/frc/robot/subsystems/intake/intakeReal.java b/src/main/java/frc/robot/subsystems/intake/intakeReal.java index c20914a..f291889 100644 --- a/src/main/java/frc/robot/subsystems/intake/intakeReal.java +++ b/src/main/java/frc/robot/subsystems/intake/intakeReal.java @@ -1,20 +1,20 @@ -package frc.robot.subsystems.intake; +package frc.robot.subsystems.Intake; import edu.wpi.first.wpilibj.motorcontrol.VictorSP; -public class intakeReal implements intakeIO { +public class intakeReal implements IntakeIO { VictorSP leftIntake = new VictorSP(frc.robot.Constants.Motors.IntakeMotors.LMOTOR); VictorSP rightIntake = new VictorSP(frc.robot.Constants.Motors.IntakeMotors.RMOTOR); - public intakeReal() { + + public void setIntakeVoltage(double volts) { rightIntake.setInverted(true); + leftIntake.set(volts); + rightIntake.set(volts); } - - public void intakeVoltage(double lvolts, double rvolts) { - leftIntake.set(lvolts); - rightIntake.set(rvolts); - } + @Override + public void updateInputs(IntakeIOInputs inputs) {}; }