diff --git a/.gitignore b/.gitignore index 5e48910..75d10cd 100644 --- a/.gitignore +++ b/.gitignore @@ -161,4 +161,4 @@ bin/ # Simulation GUI and other tools window save file simgui*.json -src/main/deploy/config.json \ No newline at end of file +src/main/deploy/config.json diff --git a/readme.md b/readme.md new file mode 100644 index 0000000..b6681c9 --- /dev/null +++ b/readme.md @@ -0,0 +1 @@ +# I don't know what to put here \ No newline at end of file diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..b16ea5c --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,101 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "useGamepad": true + }, + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..55c0b83 --- /dev/null +++ b/simgui.json @@ -0,0 +1,60 @@ +{ + "HALProvider": { + "Other Devices": { + "SPARK MAX [5]": { + "header": { + "open": true + } + }, + "SPARK MAX [6]": { + "header": { + "open": true + } + }, + "Talon FX[9]/Integrated Sensor": { + "header": { + "open": true + } + } + } + }, + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/LiveWindow/BoomBoom": "Subsystem", + "/LiveWindow/Hood": "Subsystem", + "/LiveWindow/Intake": "Subsystem", + "/LiveWindow/LED": "Subsystem", + "/LiveWindow/Serializer": "Subsystem", + "/LiveWindow/Storage": "Subsystem", + "/LiveWindow/SwerveDrive": "Subsystem", + "/LiveWindow/SwerveModule": "Subsystem", + "/LiveWindow/Turret": "Subsystem", + "/LiveWindow/Ungrouped/DigitalInput[27]": "Digital Input", + "/LiveWindow/Ungrouped/DigitalInput[28]": "Digital Input", + "/LiveWindow/Ungrouped/DigitalInput[29]": "Digital Input", + "/LiveWindow/Ungrouped/DigitalInput[3]": "Digital Input", + "/LiveWindow/Ungrouped/PIDController[1]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[2]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[3]": "PIDController", + "/LiveWindow/Ungrouped/Scheduler": "Scheduler", + "/LiveWindow/Ungrouped/Spark[0]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [14]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [23]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [24]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [2]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [3]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [4]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [5]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [6]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [7]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [8]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [9]": "Motor Controller", + "/LiveWindow/Vision": "Subsystem", + "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/JVM Memory": "Command", + "/SmartDashboard/Scheduler": "Scheduler", + "/SmartDashboard/Usable Deploy Space": "Command" + } + } +} diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 554f1cf..61982a9 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -121,6 +121,25 @@ public final class Constants { public static final Pose2d HUB_POSE = new Pose2d(new Translation2d(0, 0), new Rotation2d(0)); } + public static final class SerializerConstants { + public static final double SERIALIZER_BELT_SPEED = 0.1d; + + // CAN IDs + public static final int SERIALIZER_BELT = 16; + public static final int SERIALIZER_BELT_BEAM = 27; // TODO + } + + public static final class IntakeConstants { + // CAN IDs + public static final int INTAKE_MOTOR = 14; + public static final int EXTENDER_MOTOR = 15; + } + public static final class StorageConstants { + public static final int STORAGE_CAN_ID = 17; + public static final int BEAM_SENSOR_SHOOTER = 28; //TODO + public static final int BEAM_SENSOR_INTAKE = 29; //TODO + public static final double STORAGE_SPEED = 0.3; + } public static final class LEDConstants { public static final int LED_SPARK_ID = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d7754ae..aefc3b4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -54,18 +54,19 @@ import edu.wpi.first.wpilibj2.command.NotifierCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; - import frc4388.robot.Constants.*; import frc4388.robot.subsystems.Claws; import frc4388.robot.commands.RunClaw; import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Claws.ClawType; - import frc4388.robot.commands.AimToCenter; import frc4388.robot.commands.Shoot; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Hood; +import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Serializer; +import frc4388.robot.subsystems.Storage; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; import frc4388.robot.subsystems.Vision; @@ -96,12 +97,16 @@ public class RobotContainer { public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); - // private final LED m_robotLED = new LED(m_robotMap.LEDController); + private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); + private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer); + private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter); + private final LED m_robotLED = new LED(m_robotMap.LEDController); private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); private final Hood m_robotHood = new Hood(); private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); private final Vision m_robotVision = new Vision(m_robotTurret, m_robotBoomBoom); - + + /* Controllers */ private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); @@ -137,6 +142,7 @@ public class RobotContainer { // m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, // m_robotSwerveDrive)); + //Swerve Drive m_robotSwerveDrive.setDefaultCommand( new RunCommand(() -> m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), @@ -145,6 +151,20 @@ public class RobotContainer { getDriverController().getRightY(), true), m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); + //Intake with Triggers + m_robotIntake.setDefaultCommand( + new RunCommand(() -> m_robotIntake.runWithTriggers( + getOperatorController().getLeftTriggerAxis(), + getOperatorController().getRightTriggerAxis()), + m_robotIntake).withName("Intake runWithTriggers defaultCommand")); + //Storage Management + m_robotStorage.setDefaultCommand( + new RunCommand(() -> m_robotStorage.manageStorage(), + m_robotStorage).withName("Storage manageStorage defaultCommand")); + //Serializer Management + m_robotSerializer.setDefaultCommand( + new RunCommand(() -> m_robotSerializer.setSerializerStateWithBeam(), + m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); // continually sends updates to the Blinkin LED controller to keep the lights on @@ -162,9 +182,10 @@ public class RobotContainer { * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { + /* Driver Buttons */ // "XboxController.Button.kBack" was undefined yet, 7 works just fine - new JoystickButton(getDriverController(), 7) + new JoystickButton(getDriverController(), XboxController.Button.kBack.value) .whenPressed(m_robotSwerveDrive::resetGyro); new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) @@ -179,7 +200,8 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kA.value) .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); - new JoystickButton(getDriverController(), XboxController.Button.kX.value) + + new JoystickButton(getDriverController(), XboxController.Button.kX.value) //Temp .whenPressed(() -> m_robotMap.leftFront.reset()) .whenPressed(() -> m_robotMap.rightFront.reset()) .whenPressed(() -> m_robotMap.leftBack.reset()) @@ -196,18 +218,30 @@ public class RobotContainer { .whenPressed(new RunClaw(m_claws, ClawType.RIGHT, false)); /* - * new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - * .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) - * .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); - * - * new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - * .whenPressed(new InstantCommand()); - * * // activates "BoomBoom" * new JoystickButton(getOperatorController(), XboxController.Button.kA.value) * .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, * m_robotHood)); */ + + //Extender + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + .whenPressed(() -> m_robotIntake.runExtender(true)); + + new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + .whenPressed(() -> m_robotIntake.runExtender(false)); + + + + //Storage + new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) + .whenPressed(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)) + .whenReleased(() -> m_robotStorage.runStorage(0.0)); + + + new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) + .whenPressed(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)) + .whenReleased(() -> m_robotStorage.runStorage(0.0)); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index f035b56..93e2ede 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -17,16 +17,17 @@ import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; - import edu.wpi.first.wpilibj.CAN; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.motorcontrol.Spark; - import frc4388.robot.Constants.ClimberConstants; import frc4388.robot.Constants.ClawConstants; +import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.Constants.SerializerConstants; +import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.SwerveModule; @@ -222,4 +223,18 @@ public class RobotMap { } - } + + /* Serializer Subsystem */ + public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless); +// public final CANSparkMax serializerShooterBelt = new CANSparkMax(SerializerConstants.SERIALIZER_SHOOTER_BELT, MotorType.kBrushless); + public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM); + + /* Intake Subsytem */ + public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR); + public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless); + + /* Storage Subsystem */ + public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); + public final DigitalInput beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER); + public final DigitalInput beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE); +} diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index c9e963f..b811a37 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -4,6 +4,8 @@ package frc4388.robot.subsystems; +import javax.sql.rowset.WebRowSet; + import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxPIDController; @@ -15,6 +17,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj2.command.SubsystemBase; + import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.Gains; diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java new file mode 100644 index 0000000..0c63afc --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -0,0 +1,80 @@ +// 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 frc4388.robot.subsystems; + +//Imported Limit switch ONLY +import com.revrobotics.SparkMaxLimitSwitch; +import com.revrobotics.SparkMaxLimitSwitch.Type; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; + +import edu.wpi.first.wpilibj.motorcontrol.Spark; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.IdleMode; + +public class Intake extends SubsystemBase { + + private WPI_TalonFX m_intakeMotor; + private CANSparkMax m_extenderMotor; + private Serializer m_serializer; + private SparkMaxLimitSwitch m_inLimit; + private SparkMaxLimitSwitch m_outLimit; + + public boolean toggle; + + /** Creates a new Intake. */ + public Intake(WPI_TalonFX intakeMotor, CANSparkMax extenderMotor, Serializer serializer) { + m_intakeMotor = intakeMotor; + m_extenderMotor = extenderMotor; + m_serializer = serializer; + + m_extenderMotor.restoreFactoryDefaults(); + + m_intakeMotor.setNeutralMode(NeutralMode.Brake); + m_intakeMotor.setInverted(false); + m_extenderMotor.setInverted(true); + + m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_inLimit.enableLimitSwitch(true); + m_outLimit.enableLimitSwitch(true); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + /** + * Runs The Intake With Triggers. + * @param leftTrigger Left Trigger to Run - + * @param rightTrigger Right Trigger to Run + + */ + public void runWithTriggers(double leftTrigger, double rightTrigger) { + m_intakeMotor.set(rightTrigger - leftTrigger); + } + /** + * Runs The Extender + * @param extended Wether the Extender Is Extended + */ + public void runExtender(boolean extended) { //TODO: Do not bring intake in if there is a ball in the extender (check if intake being in brakes the beam brake?) + if (!m_serializer.getBeam() && !extended) return; + double extenderMotorSpeed = extended ? 0.25d : -0.25d; + m_extenderMotor.set(extenderMotorSpeed); + } + + public void runExtender(double input) { + if (!m_serializer.getBeam() && input < 0.) return; + m_extenderMotor.set(input); + } + /** + * Toggles The Extender + */ + public void toggleExtender() { + toggle = !toggle; + runExtender(toggle); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java new file mode 100644 index 0000000..5263fb8 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Serializer.java @@ -0,0 +1,53 @@ +package frc4388.robot.subsystems; + +import edu.wpi.first.wpilibj.motorcontrol.Spark; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants; +import frc4388.robot.Constants.SerializerConstants; +import edu.wpi.first.wpilibj.DigitalInput; +import com.revrobotics.CANSparkMax; + +public class Serializer extends SubsystemBase{ + private CANSparkMax m_serializerBelt; + // private CANSparkMax m_serializerShooterBelt; + private DigitalInput m_serializerBeam; + private boolean serializerState; + + public Serializer(CANSparkMax serializerBelt, /*CANSparkMax serializerShooterBelt,*/ DigitalInput serializerBeam) { + m_serializerBelt = serializerBelt; + m_serializerBeam = serializerBeam; + + m_serializerBelt.set(0); + // m_serializerShooterBelt.set(0); + + } + + public void setSerializer(double input){ + m_serializerBelt.set(input); + } + /** + * Gets The State Of The Beam + * @return The State Of The Beam + */ + public boolean getBeam() { + return m_serializerBeam.get(); + } + + /** + * Sets The Serializer State With The Beam + * @param state Your State Of The Button + * @param beambroken The State of the Beam Senser + */ + public void setSerializerStateWithBeam() { + if (m_serializerBeam.get()) setSerializer(0.0); + else setSerializer(SerializerConstants.SERIALIZER_BELT_SPEED); + } + + /** + * Gets the Serializer State + * @return The Serializer State + */ + public boolean getSerializerState() { + return serializerState; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java new file mode 100644 index 0000000..1e3126a --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -0,0 +1,64 @@ +// 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 frc4388.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import edu.wpi.first.wpilibj.DigitalInput; +import frc4388.robot.Constants.StorageConstants; + +public class Storage extends SubsystemBase { + public CANSparkMax m_storageMotor; + private DigitalInput m_beamShooter; + private DigitalInput m_beamIntake; + + /** Creates a new Storage. */ + public Storage(CANSparkMax storageMotor, DigitalInput beamShooter, DigitalInput beamIntake) { + m_storageMotor = storageMotor; + m_beamShooter = beamShooter; + m_beamIntake = beamIntake; + } + /** + * If The Beam Is Broken, Run Storage + * If Else, Stop Running Storage + */ + public void manageStorage() { + if (getBeamIntake()) runStorage(0.d); + else runStorage(1.d); + } + + /** + * Runs The Storage at a Specifyed Speed + * @param input The Specifyed Speed + */ + public void runStorage(double input) { + m_storageMotor.set(input); + } + /** + * Gets The Beam State On The Shooter + * @return The State Of The Beam on the Shooter + */ + public boolean getBeamShooter(){ + return m_beamShooter.get();//True if open + } + + /** + * Gets The Beam State Of The Intake + * @return The Beam State Of The Intake + */ + public boolean getBeamIntake(){ + return m_beamIntake.get(); //True if open + } + + + @Override + /** + * Every Robot Tick Manage The Storage + */ + public void periodic() { + manageStorage(); + } +}