diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 44de55f..3ee7dba 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,6 +27,10 @@ public static final class DriveTrainMotors { public static final int BRMOTOR = 2; public static final int BLMOTOR = 3; } + public static final class IntakeMotors { + public static final int LMOTOR = 4; + public static final int RMOTOR = 5; + } } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c7a18fc..00e7752 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,7 +1,5 @@ package frc.robot; -import org.littletonrobotics.conduit.schema.Joystick; -import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -9,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Robot.RobotRunType; +import frc.robot.commands.Drive; import frc.robot.subsystems.drive.Drivetrain; import frc.robot.subsystems.drive.DrivetrainIO; import frc.robot.subsystems.drive.DrivetrainReal; @@ -21,12 +20,12 @@ */ public class RobotContainer { /* Controllers */ - Joystick driver = new Joystick(); + XboxController driver = new XboxController(0); // Initialize AutoChooser Sendable private final SendableChooser autoChooser = new SendableChooser<>(); /* Subsystems */ - private Drivetrain drivetrain; + private Drivetrain driveTrain; /** @@ -37,13 +36,13 @@ public RobotContainer(RobotRunType runtimeType) { autoChooser.setDefaultOption("Wait 1 Second", "wait"); switch (runtimeType) { case kReal: - drivetrain = new Drivetrain(new DrivetrainReal()); + driveTrain = new Drivetrain(new DrivetrainReal()); break; case kSimulation: // drivetrain = new Drivetrain(new DrivetrainSim() {}); break; default: - drivetrain = new Drivetrain(new DrivetrainIO() {}); + driveTrain = new Drivetrain(new DrivetrainIO() {}); } // Configure the button bindings configureButtonBindings(); @@ -55,7 +54,15 @@ public RobotContainer(RobotRunType runtimeType) { * ({@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() {} + + ; + + private void configureButtonBindings() { + + driveTrain.setDefaultCommand(new Drive(driveTrain, driver)); + + + } /** * Gets the user's selected autonomous command. diff --git a/src/main/java/frc/robot/commands/.keep b/src/main/java/frc/robot/commands/.keep deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java new file mode 100644 index 0000000..1c97b37 --- /dev/null +++ b/src/main/java/frc/robot/commands/Drive.java @@ -0,0 +1,32 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.drive.Drivetrain; + +/** + * Drive. + */ + +public class Drive extends Command { + private XboxController controller; + private Drivetrain drive; + + /** + * Drive again. + */ + + public Drive(Drivetrain drive, XboxController controller) { + this.controller = controller; + this.drive = drive; + addRequirements(drive); + } + + @Override + 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); + + } +} 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..cc91626 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/intake.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.intake; + +public class intake { + +} 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..e5a8ca3 --- /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 rightMotor; + public double leftMotor; + } + + /** Updates the set of loggable inputs. */ + + public default void intakePower(double lvolts, double rvolts) {} + +} + diff --git a/src/main/java/frc/robot/subsystems/intake/intakeReal.java b/src/main/java/frc/robot/subsystems/intake/intakeReal.java new file mode 100644 index 0000000..c20914a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/intakeReal.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.wpilibj.motorcontrol.VictorSP; + +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() { + rightIntake.setInverted(true); + } + + + public void intakeVoltage(double lvolts, double rvolts) { + leftIntake.set(lvolts); + rightIntake.set(rvolts); + } + +} +