diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0a6876c..0185305 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,10 +13,12 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - public static final class Shooter { - public static final int right_shooter_motor = 15; - public static final int left_shooter_motor = 16; - public static final double upper_speed = 0.5; - public static final double lower_speed = 0.2; + public static final class Drivetrain + { + public static final int right_front = 13; + public static final int left_front = 11; + public static final int right_back = 14; + public static final int left_back = 12; + public static final int drive_speed = 1; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 92b59e2..84456d7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,13 +4,14 @@ package frc.robot; +import javax.naming.ConfigurationException; + import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Button; -import frc.robot.commands.ExampleCommand; -import frc.robot.commands.UpperShoot; -import frc.robot.subsystems.ExampleSubsystem; -import frc.robot.subsystems.Shooter; +import frc.robot.Constants; +import frc.robot.commands.TankDrive; +import frc.robot.subsystems.Drivetrain; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -21,14 +22,10 @@ * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - // The robot's subsystems and commands are defined here... - private final Shooter shooterSubsystem = new Shooter(); - - private final UpperShoot shooterCommand = new UpperShoot(shooterSubsystem); - /** The container for the robot. Contains subsystems, OI devices, and commands. */ + // rename o drivetrain (f2 shortcut) + private final Drivetrain drivetrain = new Drivetrain(); public RobotContainer() { - // Configure the button bindings configureButtonBindings(); } @@ -37,11 +34,11 @@ public RobotContainer() { * 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() { - XboxController exBox = new XboxController(0); - JoystickButton joySteck = new JoystickButton(exBox, Button.kY.value); - joySteck.whenHeld(shooterCommand); + XboxController pilot = new XboxController(0); + + drivetrain.setDefaultCommand(new TankDrive(drivetrain, pilot::getLeftY, pilot::getRightY)); } /** diff --git a/src/main/java/frc/robot/commands/TankDrive.java b/src/main/java/frc/robot/commands/TankDrive.java new file mode 100644 index 0000000..6c27334 --- /dev/null +++ b/src/main/java/frc/robot/commands/TankDrive.java @@ -0,0 +1,55 @@ +package frc.robot.commands; + +import java.util.function.DoublePredicate; +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.subsystems.Drivetrain; + +public class TankDrive extends CommandBase { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final Drivetrain drivetrain; + private DoubleSupplier left; + private DoubleSupplier right; + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + + // bcz this keyword, u can use left and right in params + public TankDrive(Drivetrain drivetrainl, DoubleSupplier leftl, DoubleSupplier rightl) { + this.drivetrain = drivetrainl; + this.left = leftl; + this.right = rightl; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(drivetrainl); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + drivetrain.setSpeed(0, 0); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + drivetrain.setSpeed(left.getAsDouble(), right.getAsDouble()); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + drivetrain.setSpeed(0, 0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } + } diff --git a/src/main/java/frc/robot/commands/UpperShoot.java b/src/main/java/frc/robot/commands/UpperShoot.java deleted file mode 100644 index b6741b5..0000000 --- a/src/main/java/frc/robot/commands/UpperShoot.java +++ /dev/null @@ -1,40 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.Constants; -import frc.robot.subsystems.Shooter; - -public class UpperShoot extends CommandBase{ - private Shooter shot; - Timer timeIt = new Timer(); - public UpperShoot(Shooter asdf) { - shot = asdf; - } - @Override - public void initialize() { - shot.setSpeed(0); - timeIt.start(); - } - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - shot.setSpeed(Constants.Shooter.upper_speed); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - shot.setSpeed(0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - if (timeIt.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..0d9db84 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -0,0 +1,28 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; + +public class Drivetrain extends SubsystemBase{ + private WPI_TalonFX rightFrontMotor = new WPI_TalonFX(Constants.Drivetrain.right_front); + private WPI_TalonFX leftFrontMotor = new WPI_TalonFX(Constants.Drivetrain.left_front); + private WPI_TalonFX rightBackMotor = new WPI_TalonFX(Constants.Drivetrain.right_back); + private WPI_TalonFX leftBackMotor = new WPI_TalonFX(Constants.Drivetrain.left_back); + + private MotorControllerGroup leftDrive = new MotorControllerGroup(leftFrontMotor, leftBackMotor); + private MotorControllerGroup rightDrive = new MotorControllerGroup(rightFrontMotor, rightBackMotor); + + public Drivetrain () + { + rightDrive.setInverted(true); + leftDrive.setInverted(false); + } + public void setSpeed (double leftSpeed, double rightSpeed) + { + leftDrive.set(leftSpeed); + rightDrive.set(rightSpeed); + } +} diff --git a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java index f5e9470..572ac64 100644 --- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java @@ -4,11 +4,12 @@ package frc.robot.subsystems; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import frc.robot.Constants; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class ExampleSubsystem extends SubsystemBase { /** Creates a new ExampleSubsystem. */ - public ExampleSubsystem() {} @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java deleted file mode 100644 index a8adc86..0000000 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ /dev/null @@ -1,22 +0,0 @@ -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; - -public class Shooter extends SubsystemBase { - private WPI_TalonFX leftMotor = new WPI_TalonFX(Constants.Shooter.left_shooter_motor); - private WPI_TalonFX rightMotor = new WPI_TalonFX(Constants.Shooter.right_shooter_motor); - //class name is same as setter - public Shooter() { - leftMotor.setInverted(true); - rightMotor.setInverted(false); - } - - private MotorControllerGroup shoot = new MotorControllerGroup(rightMotor, leftMotor); - public void setSpeed (double speed) - { - shoot.set(speed); - } -}