diff --git a/build.gradle b/build.gradle index fadc9a4..4dc2bb0 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.1.1" + id "edu.wpi.first.GradleRIO" version "2023.2.1" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 95281a4..3cf6198 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,11 +7,15 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.lib.util.COTSFalconSwerveConstants; import frc.lib.util.SwerveModuleConstants; public final class Constants { public static final double stickDeadband = 0.1; + public static final XboxController driverControls = new XboxController(0); + public static final XboxController operatorControls = new XboxController(1); public static final class Swerve { public static final int pigeonID = 14; @@ -26,6 +30,7 @@ public static final class Swerve { public static final double wheelBase = Units.inchesToMeters(21.25); //TODO: This must be tuned to specific robot public static final double wheelCircumference = chosenModule.wheelCircumference; + /* Swerve Kinematics * No need to ever change this unless you are not doing a traditional rectangular/square 4 module swerve */ public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics( @@ -146,4 +151,19 @@ public static final class AutoConstants { //TODO: The below constants are used i new TrapezoidProfile.Constraints( kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); } + + public static final class IntakeConstants { + public static final int INTAKE_MOTOR = 13; + public static final double INTAKE_SPEED = 0.3; + public static Boolean intakeRunning = false; + public static final JoystickButton CONT_INTAKE_RUN = new JoystickButton(operatorControls, XboxController.Button.kRightBumper.value); + public static final JoystickButton CONT_INTAKE_RUN_REV = new JoystickButton(operatorControls, XboxController.Button.kLeftBumper.value); + public static final int coneSensorID = 0; + public static final int cubeSensorID = 0; + } + + public static final class WristConstants { + public static final int WRIST_MOTOR = 17; + + } } diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 8776e5d..4a48233 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -21,5 +21,6 @@ private Main() {} */ public static void main(String... args) { RobotBase.startRobot(Robot::new); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7104d08..bb099f3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,10 +3,11 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; - +import frc.robot.Constants.IntakeConstants; import frc.robot.autos.*; import frc.robot.commands.*; import frc.robot.subsystems.*; @@ -32,6 +33,7 @@ public class RobotContainer { /* Subsystems */ private final Swerve s_Swerve = new Swerve(); + private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem(); /** The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -58,7 +60,35 @@ public RobotContainer() { */ private void configureButtonBindings() { /* Driver Buttons */ + + + /*Zero Gyro */ zeroGyro.onTrue(new InstantCommand(() -> s_Swerve.zeroGyro())); + + // if(IntakeConstants.intakeRunning == false) { + + // /*Run Intake- FORWARD */ + + // IntakeConstants.CONT_INTAKE_RUN.onTrue(new InstantCommand(() -> m_IntakeSubsystem.run())); + + // /*Run Intake- REVERSE */ + + // IntakeConstants.CONT_INTAKE_RUN_REV.onTrue(new InstantCommand(() -> m_IntakeSubsystem.runReverse())); + + // } else if (IntakeConstants.intakeRunning == true) { + + // /*Stop Intake- Both ways */ + + // IntakeConstants.CONT_INTAKE_RUN.onTrue(new InstantCommand(() -> m_IntakeSubsystem.stop())); + // IntakeConstants.CONT_INTAKE_RUN.onTrue(new InstantCommand(() -> m_IntakeSubsystem.stop())); + + // } + IntakeConstants.CONT_INTAKE_RUN.onTrue(new InstantCommand(() -> m_IntakeSubsystem.run())); + IntakeConstants.CONT_INTAKE_RUN.onFalse(new InstantCommand(() -> m_IntakeSubsystem.stop())); + + IntakeConstants.CONT_INTAKE_RUN_REV.onTrue(new InstantCommand(() -> m_IntakeSubsystem.runReverse())); + IntakeConstants.CONT_INTAKE_RUN_REV.onFalse(new InstantCommand(() -> m_IntakeSubsystem.stop())); + } /** diff --git a/src/main/java/frc/robot/commands/WristCommand.java b/src/main/java/frc/robot/commands/WristCommand.java index 596e9ec..3feb82d 100644 --- a/src/main/java/frc/robot/commands/WristCommand.java +++ b/src/main/java/frc/robot/commands/WristCommand.java @@ -1,9 +1,15 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj.drive.RobotDriveBase.MotorType; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.WristConstants; -public class WristSubsystem extends SubsystemBase{ - CANSparkMax motor = new CANSparkMax(17, MotorType.kBrushless); +public class WristCommand extends CommandBase{ + CANSparkMax motor = new CANSparkMax(WristConstants.WRIST_MOTOR, MotorType.kBrushless); public void positive () { motor.set(1); } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 4cd0ec8..abc53ce 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -1,6 +1,57 @@ package frc.robot.subsystems; + + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + + +import edu.wpi.first.wpilibj.DigitalInput; + import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.IntakeConstants; -public class IntakeSubsystem extends SubsystemBase{ +public class IntakeSubsystem extends SubsystemBase { + DigitalInput coneSensor = new DigitalInput(IntakeConstants.coneSensorID); + DigitalInput cubeSensor = new DigitalInput(IntakeConstants.cubeSensorID); + private boolean debug = false; + + private CANSparkMax intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_MOTOR, MotorType.kBrushless); + private double motorSpeed = 0; + + // private static final Spark m_intakeRoller = new Spark(INTAKE_ROLLER_CAN_NUM + + public IntakeSubsystem() { + } + + public void setDebug(boolean debug) { + this.debug = debug; + } + + public void run(){ + motorSpeed = IntakeConstants.INTAKE_SPEED; + IntakeConstants.intakeRunning = true; + } + + public void runReverse() { + motorSpeed = -(IntakeConstants.INTAKE_SPEED); + IntakeConstants.intakeRunning = true; + } + + public void stop(){ + motorSpeed = 0; + IntakeConstants.intakeRunning = false; + } + + @Override + public void periodic() { + intakeMotor.set(motorSpeed); + } + + public boolean isConePresent(){ + return coneSensor.get(); + } + public boolean isCubePresent(){ + return cubeSensor.get(); + } }