diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4013790..c4a3dee 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -2,16 +2,16 @@ // 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; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.Tank.Tank; import frc.robot.subsystems.Transporter.Transporter; - public class RobotContainer { public static final Transporter TRANSPORTER = new Transporter(); + public static final Tank TANK = new Tank(); public RobotContainer() { configureBindings(); diff --git a/src/main/java/frc/robot/subsystems/Tank/Tank.java b/src/main/java/frc/robot/subsystems/Tank/Tank.java new file mode 100644 index 0000000..85ced17 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Tank/Tank.java @@ -0,0 +1,24 @@ +package frc.robot.subsystems.Tank; + + +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Tank extends SubsystemBase { + private final DifferentialDrive differentialDrive = TankConstants.DIFFERENTIAL_DRIVE; + + void stopMotors() { + differentialDrive.stopMotor(); + } + + /** + * Applies arcade drive to the differential drive. + * Arcade drive uses two joy sticks, one drives it forwards and backwards and the other turns it left or right. + * + * @param driveDirection a double that gives the backwards or forwards power (-1 to 1) + * @param turnDirection a double that gives the rotation power (-1 to 1) + */ + void arcadeDrive(double driveDirection, double turnDirection) { + differentialDrive.arcadeDrive(driveDirection, turnDirection); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Tank/TankCommands.java b/src/main/java/frc/robot/subsystems/Tank/TankCommands.java new file mode 100644 index 0000000..bc4b8e2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Tank/TankCommands.java @@ -0,0 +1,28 @@ +package frc.robot.subsystems.Tank; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import frc.robot.RobotContainer; + +import java.util.function.DoubleSupplier; + +public class TankCommands { + /** + * Creates a command that uses arcade drive to drive the tank. + * Arcade drive uses two joy sticks, one drives it forwards and backwards and the other turns it left or right. + * + * @param driveDirection a supplier that gives us the forwards or backwards power (-1 to 1) + * @param turnDirection a supplier that gives us the power of the rotation (-1 to 1) + * @return the command + */ + public static Command getArcadeDriveCommand(DoubleSupplier driveDirection, DoubleSupplier turnDirection) { + return new FunctionalCommand( + () -> { + }, + () -> RobotContainer.TANK.arcadeDrive(driveDirection.getAsDouble(), turnDirection.getAsDouble()), + (interrupted) -> RobotContainer.TANK.stopMotors(), + () -> false, + RobotContainer.TANK + ); + } +} diff --git a/src/main/java/frc/robot/subsystems/Tank/TankConstants.java b/src/main/java/frc/robot/subsystems/Tank/TankConstants.java new file mode 100644 index 0000000..e0e270a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Tank/TankConstants.java @@ -0,0 +1,58 @@ +package frc.robot.subsystems.Tank; + +import com.ctre.phoenix.motorcontrol.FollowerType; +import com.ctre.phoenix.motorcontrol.InvertType; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; + +public class TankConstants { + private static final int + RIGHT_MASTER_MOTOR_ID = 1, + RIGHT_FOLLOWER_MOTOR_ID = 2, + LEFT_MASTER_MOTOR_ID = 3, + LEFT_FOLLOWER_MOTOR_ID = 4; + + static final WPI_TalonSRX + RIGHT_MASTER_MOTOR = new WPI_TalonSRX(RIGHT_MASTER_MOTOR_ID), + RIGHT_FOLLOWER_MOTOR = new WPI_TalonSRX(RIGHT_FOLLOWER_MOTOR_ID), + LEFT_MASTER_MOTOR = new WPI_TalonSRX(LEFT_MASTER_MOTOR_ID), + LEFT_FOLLOWER_MOTOR = new WPI_TalonSRX(LEFT_FOLLOWER_MOTOR_ID); + + private static final InvertType LEFT_MOTORS_INVERTED_VALUE = InvertType.InvertMotorOutput; + private static final InvertType RIGHT_MOTORS_INVERTED_VALUE = InvertType.None; + private static final NeutralMode NEUTRAL_MODE = NeutralMode.Brake; + + static final DifferentialDrive DIFFERENTIAL_DRIVE = new DifferentialDrive(LEFT_MASTER_MOTOR, RIGHT_MASTER_MOTOR); + + static { + configureRightMotors(); + configureLeftMotors(); + } + + private static final void configureRightMotors() { + RIGHT_MASTER_MOTOR.configFactoryDefault(); + RIGHT_FOLLOWER_MOTOR.configFactoryDefault(); + + RIGHT_MASTER_MOTOR.setInverted(RIGHT_MOTORS_INVERTED_VALUE); + RIGHT_MASTER_MOTOR.setNeutralMode(NEUTRAL_MODE); + + RIGHT_FOLLOWER_MOTOR.setInverted(RIGHT_MOTORS_INVERTED_VALUE); + RIGHT_FOLLOWER_MOTOR.setNeutralMode(NEUTRAL_MODE); + + RIGHT_FOLLOWER_MOTOR.follow(RIGHT_MASTER_MOTOR, FollowerType.PercentOutput); + } + + private static final void configureLeftMotors() { + LEFT_MASTER_MOTOR.configFactoryDefault(); + LEFT_FOLLOWER_MOTOR.configFactoryDefault(); + + LEFT_MASTER_MOTOR.setInverted(LEFT_MOTORS_INVERTED_VALUE); + LEFT_MASTER_MOTOR.setNeutralMode(NEUTRAL_MODE); + + LEFT_FOLLOWER_MOTOR.setInverted(LEFT_MOTORS_INVERTED_VALUE); + LEFT_FOLLOWER_MOTOR.setNeutralMode(NEUTRAL_MODE); + + LEFT_FOLLOWER_MOTOR.follow(LEFT_MASTER_MOTOR, FollowerType.PercentOutput); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Transporter/TransporterConstants.java b/src/main/java/frc/robot/subsystems/Transporter/TransporterConstants.java index 85b62b0..81d442c 100644 --- a/src/main/java/frc/robot/subsystems/Transporter/TransporterConstants.java +++ b/src/main/java/frc/robot/subsystems/Transporter/TransporterConstants.java @@ -6,8 +6,8 @@ public class TransporterConstants { private static final int - RIGHT_MOTOR_ID = 1, - LEFT_MOTOR_ID = 2; + RIGHT_MOTOR_ID = 10, + LEFT_MOTOR_ID = 11; static final WPI_TalonSRX RIGHT_MOTOR = new WPI_TalonSRX(RIGHT_MOTOR_ID),