diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 50f185a..2d41b3a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,42 +7,48 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.commands.CollectingCommands; +import frc.robot.commands.GeneralCommands; +import frc.robot.constants.OperatorConstants; import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.IntakeCommands; +import frc.robot.subsystems.intake.IntakeConstants; import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShooterCommands; +import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.tank.Tank; -import frc.robot.subsystems.tank.TankCommands; import frc.robot.subsystems.transporter.Transporter; -import frc.robot.commands.*; +import frc.robot.subsystems.transporter.TransporterCommands; +import frc.robot.subsystems.transporter.TransporterConstants; public class RobotContainer { public static final Intake INTAKE = new Intake(); - public static Shooter SHOOTER = new Shooter(); + public static Shooter SHOOTER = new Shooter(); + // public static SparkShooter SPARK_SHOOTER = new SparkShooter(); public static Transporter TRANSPORTER = new Transporter(); public static final Tank TANK = new Tank(); - private static final CommandXboxController DRIVER_CONTROLLER = new CommandXboxController(0); public RobotContainer() { configureBindings(); - TANK.setDefaultCommand( - TankCommands.getArcadeDriveCommand( - () -> -DRIVER_CONTROLLER.getLeftY(), - DRIVER_CONTROLLER::getRightX - ) - ); - - /*DRIVER_CONTROLLER.rightTrigger().whileTrue( - IntakeAndTransportTennisBallCommand.CollectTennisBall() - ); - - DRIVER_CONTROLLER.rightBumper().whileTrue( - IntakeAndTransportTennisBallCommand.EjectTennisBall() - );*/ } - + private void configureBindings() { + bindDefaultCommands(); + bindControllerCommands(); + } + + private void bindDefaultCommands() { + TANK.setDefaultCommand(GeneralCommands.getTankDefaultCommand()); + INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST)); + SHOOTER.setDefaultCommand(ShooterCommands.getSetTargetStateCommand(ShooterConstants.ShooterState.REST)); +// SPARK_SHOOTER.setDefaultCommand(SparkShooterCommands.getSetTargetStateCommand(SparkShooterConstants.SparkShooterState.REST)); + TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST)); } - + + private void bindControllerCommands() { + OperatorConstants.COLLECT.whileTrue(CollectingCommands.CollectTennisBall()); + } + public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } diff --git a/src/main/java/frc/robot/commands/CollectWhileShoot.java b/src/main/java/frc/robot/commands/CollectWhileShoot.java new file mode 100644 index 0000000..54bf46b --- /dev/null +++ b/src/main/java/frc/robot/commands/CollectWhileShoot.java @@ -0,0 +1,22 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.subsystems.intake.IntakeCommands; +import frc.robot.subsystems.intake.IntakeConstants; +import frc.robot.subsystems.shooter.ShooterCommands; +import frc.robot.subsystems.shooter.ShooterConstants; +import frc.robot.subsystems.sparkShooter.SparkShooterCommands; +import frc.robot.subsystems.sparkShooter.SparkShooterConstants; +import frc.robot.subsystems.transporter.TransporterCommands; +import frc.robot.subsystems.transporter.TransporterConstants; + +public class CollectWhileShoot { + public static Command CollectingAndShootingCommand(){ + return new ParallelCommandGroup( + ShooterCommands.getSetTargetStateCommand(ShooterConstants.ShooterState.SHOOT), + IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT_STATE), + TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.COLLECT_STATE) + ); + } +} diff --git a/src/main/java/frc/robot/commands/CommandConstants.java b/src/main/java/frc/robot/commands/CommandConstants.java new file mode 100644 index 0000000..95e57f0 --- /dev/null +++ b/src/main/java/frc/robot/commands/CommandConstants.java @@ -0,0 +1,35 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.constants.OperatorConstants; +import frc.robot.subsystems.tank.TankCommands; + +public class CommandConstants { + private static final CommandXboxController DRIVER_CONTROLLER = OperatorConstants.DRIVER_CONTROLLER; + private static final double + MINIMUM_DRIVE_SHIFT_POWER = 0.30, + MINIMUM_ROTATION_SHIFT_POWER = 0.4; + + public static final Command + DRIVE_TANK_COMMAND = TankCommands.getArcadeDriveCommand( + () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftY()), + () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX()) + ); + + public static double calculateDriveStickAxisValue(double axisValue) { + return axisValue / calculateShiftModeValue(MINIMUM_DRIVE_SHIFT_POWER); + } + + public static double calculateRotationStickAxisValue(double axisValue) { + return axisValue / calculateShiftModeValue(MINIMUM_ROTATION_SHIFT_POWER); + } + + public static double calculateShiftModeValue(double minimumPower) { + final double squaredShiftModeValue = Math.sqrt(DRIVER_CONTROLLER.getRightTriggerAxis()); + final double minimumShiftValueCoefficient = 1 - (1 / minimumPower); + + return 1 - squaredShiftModeValue * minimumShiftValueCoefficient; + } +} + diff --git a/src/main/java/frc/robot/commands/GeneralCommands.java b/src/main/java/frc/robot/commands/GeneralCommands.java new file mode 100644 index 0000000..98afceb --- /dev/null +++ b/src/main/java/frc/robot/commands/GeneralCommands.java @@ -0,0 +1,15 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.OperatorConstants; +import frc.robot.subsystems.tank.TankCommands; + +public class GeneralCommands { + + public static Command getTankDefaultCommand() { + return TankCommands.getArcadeDriveCommand( + () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), + () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getRightX()) + ); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ShootingCommands.java b/src/main/java/frc/robot/commands/ShootingCommands.java index fdde0cd..96470a9 100644 --- a/src/main/java/frc/robot/commands/ShootingCommands.java +++ b/src/main/java/frc/robot/commands/ShootingCommands.java @@ -4,6 +4,8 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.robot.subsystems.shooter.ShooterCommands; import frc.robot.subsystems.shooter.ShooterConstants; +import frc.robot.subsystems.sparkShooter.SparkShooterCommands; +import frc.robot.subsystems.sparkShooter.SparkShooterConstants; import frc.robot.subsystems.transporter.TransporterCommands; import frc.robot.subsystems.transporter.TransporterConstants; diff --git a/src/main/java/frc/robot/constants/OperatorConstants.java b/src/main/java/frc/robot/constants/OperatorConstants.java new file mode 100644 index 0000000..358e3c9 --- /dev/null +++ b/src/main/java/frc/robot/constants/OperatorConstants.java @@ -0,0 +1,13 @@ +package frc.robot.constants; + +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +public class OperatorConstants { + private static final int DRIVER_CONTROLLER_PORT = 0; + public static final CommandXboxController DRIVER_CONTROLLER = new CommandXboxController(DRIVER_CONTROLLER_PORT); + public static final Trigger + SHOOT = DRIVER_CONTROLLER.rightBumper().and(DRIVER_CONTROLLER.leftTrigger().negate()), + COLLECT = DRIVER_CONTROLLER.leftTrigger().and(DRIVER_CONTROLLER.rightBumper().negate()), + SHOOT_WHILE_COLLECT = DRIVER_CONTROLLER.leftTrigger().and(DRIVER_CONTROLLER.rightBumper().negate()); +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 08e612e..f6df248 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -42,9 +42,9 @@ private static void configureFollowerMotor() { } public enum IntakeState { - COLLECT_STATE(-12.0), - EJECT_STATE(12.0), - REST(0.0); + COLLECT_STATE(3), + EJECT_STATE(-3), + REST(0); public final double targetVoltage; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 35740d9..8872efd 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -4,7 +4,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Shooter extends SubsystemBase { - private final WPI_TalonSRX motor = ShooterConstants.MASTER_MOTOR; + private final WPI_TalonSRX motor = ShooterConstants.MOTOR; public Shooter() { } @@ -18,7 +18,7 @@ void setTargetVoltage(double targetVoltage) { } void stop(){ - ShooterConstants.MASTER_MOTOR.stopMotor(); + ShooterConstants.MOTOR.stopMotor(); ShooterConstants.FOLLOWER_MOTOR.stopMotor(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index e9c151b..2def824 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -1,54 +1,32 @@ package frc.robot.subsystems.shooter; -import com.ctre.phoenix.motorcontrol.FollowerType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; public class ShooterConstants { - private static final int - MASTER_MOTOR_ID = 9, - FOLLOWER_MOTOR_ID = 10; - static final WPI_TalonSRX - MASTER_MOTOR = new WPI_TalonSRX(MASTER_MOTOR_ID), - FOLLOWER_MOTOR = new WPI_TalonSRX(FOLLOWER_MOTOR_ID); - - private static final boolean - SHOULD_MASTER_MOTOR_INVERT = true, - SHOULD_FOLLOWER_MOTOR_INVERT = false; + private static final int MASTER_MOTOR_ID = 8; + static final WPI_TalonSRX MOTOR = new WPI_TalonSRX(MASTER_MOTOR_ID); + private static final boolean SHOULD_MOTOR_INVERT = true; private static final NeutralMode NEUTRAL_MODE = NeutralMode.Brake; private static final double VOLTAGE_LIMIT = 12; static { - configureMasterMotor(); - configureFollowerMotor(); - } - - private static void configureMasterMotor() { - MASTER_MOTOR.configFactoryDefault(); - MASTER_MOTOR.enableVoltageCompensation(true); - MASTER_MOTOR.configVoltageCompSaturation(VOLTAGE_LIMIT); - MASTER_MOTOR.setNeutralMode(NEUTRAL_MODE); - MASTER_MOTOR.setInverted(SHOULD_MASTER_MOTOR_INVERT); - } - - private static void configureFollowerMotor() { - FOLLOWER_MOTOR.configFactoryDefault(); - FOLLOWER_MOTOR.enableVoltageCompensation(true); - FOLLOWER_MOTOR.configVoltageCompSaturation(VOLTAGE_LIMIT); - FOLLOWER_MOTOR.setNeutralMode(NEUTRAL_MODE); - FOLLOWER_MOTOR.setInverted(SHOULD_FOLLOWER_MOTOR_INVERT); - FOLLOWER_MOTOR.follow(MASTER_MOTOR, FollowerType.PercentOutput); + MOTOR.configFactoryDefault(); + MOTOR.enableVoltageCompensation(true); + MOTOR.configVoltageCompSaturation(VOLTAGE_LIMIT); + MOTOR.setNeutralMode(NEUTRAL_MODE); + MOTOR.setInverted(SHOULD_MOTOR_INVERT); } - public enum ShooterState{ + public enum ShooterState { REST(0), SHOOT(5), EJECT(2); public final double targetVoltage; - ShooterState(double targetVoltage){ + ShooterState(double targetVoltage) { this.targetVoltage = targetVoltage; } } diff --git a/src/main/java/frc/robot/subsystems/sparkShooter/SparkShooter.java b/src/main/java/frc/robot/subsystems/sparkShooter/SparkShooter.java new file mode 100644 index 0000000..897da53 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/sparkShooter/SparkShooter.java @@ -0,0 +1,27 @@ +package frc.robot.subsystems.sparkShooter; + + +import com.revrobotics.spark.SparkMax; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class SparkShooter extends SubsystemBase { + private final SparkMax motor = SparkShooterConstants.MOTOR; + + public SparkShooter() { + } + + void stop() { + motor.stopMotor(); + } + + void setTargetState(SparkShooterConstants.SparkShooterState targetState) { + setTargetVoltage(targetState.targetVoltage); + } + + private void setTargetVoltage(double targetVoltage) { + motor.setVoltage(targetVoltage); + } + + +} + diff --git a/src/main/java/frc/robot/subsystems/sparkShooter/SparkShooterCommands.java b/src/main/java/frc/robot/subsystems/sparkShooter/SparkShooterCommands.java new file mode 100644 index 0000000..125fa90 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/sparkShooter/SparkShooterCommands.java @@ -0,0 +1,14 @@ +package frc.robot.subsystems.sparkShooter; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import frc.robot.RobotContainer; + +public class SparkShooterCommands { + public static Command getSetTargetStateCommand(SparkShooterConstants.SparkShooterState targetState) { + return new StartEndCommand( + () -> RobotContainer.SPARK_SHOOTER.setTargetState(targetState), + RobotContainer.SPARK_SHOOTER::stop + ); + } +} diff --git a/src/main/java/frc/robot/subsystems/sparkShooter/SparkShooterConstants.java b/src/main/java/frc/robot/subsystems/sparkShooter/SparkShooterConstants.java new file mode 100644 index 0000000..fc57284 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/sparkShooter/SparkShooterConstants.java @@ -0,0 +1,39 @@ +package frc.robot.subsystems.sparkShooter; + +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkLowLevel; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkMaxConfig; + +public class SparkShooterConstants { + private static final int MOTOR_ID = 20; + + static final SparkMax MOTOR = new SparkMax(MOTOR_ID, SparkLowLevel.MotorType.kBrushless); + + private static final boolean SHOULD_MOTOR_INVERT = false; + private static final SparkBaseConfig.IdleMode IDLE_MODE = SparkBaseConfig.IdleMode.kBrake; + + private static final double VOLTAGE_LIMIT = 12; + + static { + SparkMaxConfig config = new SparkMaxConfig(); + config.inverted(SHOULD_MOTOR_INVERT); + config.voltageCompensation(VOLTAGE_LIMIT); + config.idleMode(SparkBaseConfig.IdleMode.kBrake); + + MOTOR.configure(config, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kNoPersistParameters); + } + + public enum SparkShooterState { + REST(0), + SHOOT(5), + EJECT(2); + + public final double targetVoltage; + + SparkShooterState(double targetVoltage) { + this.targetVoltage = targetVoltage; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/transporter/Transporter.java b/src/main/java/frc/robot/subsystems/transporter/Transporter.java index 5f61303..f482da3 100644 --- a/src/main/java/frc/robot/subsystems/transporter/Transporter.java +++ b/src/main/java/frc/robot/subsystems/transporter/Transporter.java @@ -4,7 +4,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Transporter extends SubsystemBase { - private final WPI_TalonSRX motor = TransporterConstants.MASTER_MOTOR; + private final WPI_TalonSRX motor = TransporterConstants.MOTOR; public Transporter() { } @@ -18,7 +18,7 @@ void setTargetVoltage(double targetVoltage) { } void stop(){ - TransporterConstants.MASTER_MOTOR.stopMotor(); + TransporterConstants.MOTOR.stopMotor(); TransporterConstants.FOLLOWER_MOTOR.stopMotor(); } } \ 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 23f498f..e46956e 100644 --- a/src/main/java/frc/robot/subsystems/transporter/TransporterConstants.java +++ b/src/main/java/frc/robot/subsystems/transporter/TransporterConstants.java @@ -1,55 +1,34 @@ package frc.robot.subsystems.transporter; -import com.ctre.phoenix.motorcontrol.FollowerType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import edu.wpi.first.units.measure.Voltage; public class TransporterConstants { - private static final int - MASTER_MOTOR_ID = 7, - FOLLOWER_MOTOR_ID = 8; - static final WPI_TalonSRX - MASTER_MOTOR = new WPI_TalonSRX(MASTER_MOTOR_ID), - FOLLOWER_MOTOR = new WPI_TalonSRX(FOLLOWER_MOTOR_ID); - - private static final boolean - SHOULD_MASTER_MOTOR_INVERT = true, - SHOULD_FOLLOWER_MOTOR_INVERT = false; + private static final int MOTOR_ID = 7; + + static final WPI_TalonSRX MOTOR = new WPI_TalonSRX(MOTOR_ID); + + private static final boolean SHOULD_MOTOR_INVERT = true; private static final NeutralMode NEUTRAL_MODE = NeutralMode.Brake; private static final double VOLTAGE_LIMIT = 12; static { - configureMasterMotor(); - configureFollowerMotor(); - } - - private static void configureMasterMotor() { - MASTER_MOTOR.configFactoryDefault(); - MASTER_MOTOR.enableVoltageCompensation(true); - MASTER_MOTOR.configVoltageCompSaturation(VOLTAGE_LIMIT); - MASTER_MOTOR.setNeutralMode(NEUTRAL_MODE); - MASTER_MOTOR.setInverted(SHOULD_MASTER_MOTOR_INVERT); - } - - private static void configureFollowerMotor() { - FOLLOWER_MOTOR.configFactoryDefault(); - FOLLOWER_MOTOR.enableVoltageCompensation(true); - FOLLOWER_MOTOR.configVoltageCompSaturation(VOLTAGE_LIMIT); - FOLLOWER_MOTOR.setNeutralMode(NEUTRAL_MODE); - FOLLOWER_MOTOR.setInverted(SHOULD_FOLLOWER_MOTOR_INVERT); - FOLLOWER_MOTOR.follow(MASTER_MOTOR, FollowerType.PercentOutput); + MOTOR.configFactoryDefault(); + MOTOR.enableVoltageCompensation(true); + MOTOR.configVoltageCompSaturation(VOLTAGE_LIMIT); + MOTOR.setNeutralMode(NEUTRAL_MODE); + MOTOR.setInverted(SHOULD_MOTOR_INVERT); } - public enum TransporterState{ - COLLECT_STATE(5), - EJECT_STAGE(-4), + public enum TransporterState { + COLLECT_STATE(3), + EJECT_STAGE(-3), REST(0); public final double targetVoltage; - TransporterState(double targetVoltage){ + TransporterState(double targetVoltage) { this.targetVoltage = targetVoltage; } } diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..ac62be8 --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,71 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2025.0.3", + "frcYear": "2025", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2025.0.3" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2025.0.3", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2025.0.3", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2025.0.3", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file