diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 033c72d..fcd825b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -3,6 +3,7 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; +import edu.wpi.first.wpilibj.XboxController.Button; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -12,4 +13,8 @@ *

It is advised to statically import this class (or one of its inner classes) wherever the * constants are needed, to reduce verbosity. */ -public final class Constants {} +public final class Constants { + public static final int SWITCH_BUTTON = Button.kX.value; + public static final int OUT_TAKE = Button.kRightBumper.value; + public static final int IN_TAKE = Button.kLeftBumper.value; +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b68462c..2f42e56 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,10 +7,8 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; - public class Robot extends TimedRobot { private Command m_autonomousCommand; - private RobotContainer m_robotContainer; @Override @@ -21,6 +19,7 @@ public void robotInit() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + } @Override @@ -35,14 +34,17 @@ public void disabledExit() {} @Override public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - + if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } } + @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + + } @Override public void autonomousExit() {} @@ -53,9 +55,11 @@ public void teleopInit() { m_autonomousCommand.cancel(); } } - + @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + + } @Override public void teleopExit() {} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de2c9d0..8010a4d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,16 +5,30 @@ package frc.robot; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.commands.Autonomous; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Flywheel; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.InstantCommand; public class RobotContainer { + public Flywheel flywheel = new Flywheel(); + public XboxController controller = new XboxController(0); + private Drivetrain drivetrain = new Drivetrain(controller); public RobotContainer() { configureBindings(); } - private void configureBindings() {} + private void configureBindings() { + new JoystickButton(controller, Constants.SWITCH_BUTTON).onTrue(new InstantCommand(()->{drivetrain.swap();})); + new JoystickButton(controller, Constants.OUT_TAKE).onTrue(new InstantCommand(()->{flywheel.out_take(1);})); + new JoystickButton(controller, Constants.OUT_TAKE).onFalse(new InstantCommand(() -> {flywheel.out_take(0);})); + new JoystickButton(controller, Constants.IN_TAKE).onTrue(new InstantCommand(()->{flywheel.intake(1);})); + new JoystickButton(controller, Constants.IN_TAKE).onFalse(new InstantCommand(() -> {flywheel.out_take(0);})); + } public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + return new Autonomous(drivetrain); } } diff --git a/src/main/java/frc/robot/commands/Autonomous.java b/src/main/java/frc/robot/commands/Autonomous.java new file mode 100644 index 0000000..d776f26 --- /dev/null +++ b/src/main/java/frc/robot/commands/Autonomous.java @@ -0,0 +1,47 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Drivetrain; +public class Autonomous extends CommandBase { + /** Creates a new Autonomous. */ + private Drivetrain drivetrain; + private final Timer timer = new Timer(); + public Autonomous(Drivetrain dtrain) { + this.drivetrain = dtrain; + addRequirements(drivetrain); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + drivetrain.isAuto = true; + timer.start(); + drivetrain.drive(0.5,0.5); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + timer.stop(); + drivetrain.isAuto = false; + drivetrain.drive(0, 0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (timer.get() > 5) { + return true; + } + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java new file mode 100644 index 0000000..9a27bc3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -0,0 +1,44 @@ +package frc.robot.subsystems; +import org.carlmontrobotics.lib199.MotorControllerFactory; +import org.carlmontrobotics.lib199.MotorConfig; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + + +public class Drivetrain extends SubsystemBase { + public boolean isAuto; + public boolean isTank = false; + private XboxController controller; + CANSparkMax rightMotor = MotorControllerFactory.createSparkMax(1,MotorConfig.NEO); + CANSparkMax leftMotor = MotorControllerFactory.createSparkMax(2,MotorConfig.NEO); + + public Drivetrain(XboxController ctrlr){ + controller = ctrlr; + } + + public void drive(double left,double right) { + rightMotor.set(right); + leftMotor.set(left); + } + + public void swap() { + isTank = !isTank; + } + + @Override + public void periodic(){ + if (!isAuto) { + double speed = -controller.getLeftY(); + if (isTank) { + double turn = controller.getLeftX(); + double left = speed - turn; + double right = speed + turn; + drive(left, right); + } else { + double rightYAxis = -controller.getRightY(); + drive(speed,rightYAxis); + } + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Flywheel.java b/src/main/java/frc/robot/subsystems/Flywheel.java new file mode 100644 index 0000000..2369f3a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Flywheel.java @@ -0,0 +1,21 @@ +package frc.robot.subsystems; + +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; + +import com.revrobotics.CANSparkMax; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Flywheel extends SubsystemBase{ + CANSparkMax flyWheel1 = MotorControllerFactory.createSparkMax(3,MotorConfig.NEO); + CANSparkMax flyWheel2 = MotorControllerFactory.createSparkMax(4, MotorConfig.NEO); + public void intake(double speed) { + flyWheel1.set(-speed); + flyWheel2.set(speed); + } + public void out_take(double speed) { + flyWheel1.set(speed); + flyWheel2.set(-speed); + } +}