diff --git a/src/main/java/frc/robot/Manipulation.java b/src/main/java/frc/robot/Manipulation.java new file mode 100644 index 0000000..9abc682 --- /dev/null +++ b/src/main/java/frc/robot/Manipulation.java @@ -0,0 +1,82 @@ +package frc.robot; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import frc.robot.logging.Loggable; +import frc.robot.logging.Logger; + +public class Manipulation implements Loggable { + + private CANSparkMax intakeWheel; + private DoubleSolenoid intakePneumatics; + private CANSparkMax indexLoad; + + private double speed; + private boolean spinning; + + /** + * Constructor + * + * @param pneumaticsForwardChannel The Solenoid id for the forward channel for the intake + * @param pneumaticsReverseChannel The Solenoid id for the reverse channel for the intake + * @param intakeWheelID The CAN id of the spark for the intake + * @param indexLoadID The CAN id of the spark for the index loader + * + */ + Manipulation(int pneumaticsForwardChannel, int pneumaticsReverseChannel, int intakeWheelID, int indexLoadID) { + this.intakeWheel = new CANSparkMax(intakeWheelID, MotorType.kBrushless); + this.indexLoad = new CANSparkMax(indexLoadID, MotorType.kBrushless); + this.intakePneumatics = new DoubleSolenoid(PneumaticsModuleType.REVPH, pneumaticsForwardChannel, pneumaticsReverseChannel); + + intakeWheel.setInverted(true); + } + + /** + * Spins the intake motor + * + * @param spin True if the motor should spin; false if not + * + */ + public void setIntakeSpin(boolean spin) { + this.speed = spin ? 0.3 : 0.0; + intakeWheel.set(speed); + this.spinning = spin; + } + + /** + * Moves the intake system + * + * @param extend True if the pneumatics should extend; false if not + * + */ + public void setIntakeExtend(boolean extend) { + intakePneumatics.set(extend ? Value.kForward : Value.kReverse); + } + /** + * Moves power cells down indexing system + * + * @param load True if it should load; false if not + * + */ + public void setIndexLoad(boolean load) { + indexLoad.set(load ? 0.5 : 0.0); + } + + @Override + public void setupLogging(Logger logger) { + logger.addAttribute("Manipulation/IntakeWheelSpeed"); + logger.addAttribute("Manipulation/IntakeWheelEnabled"); + } + + @Override + public void log(Logger logger) { + logger.log("Manipulation/IntakeWheelSpeed", speed); + logger.log("Manipulation/IntakeWheelEnabled", spinning); + } + +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 46bb8e8..744293f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -29,6 +29,7 @@ public class Robot extends TimedRobot { DriveModule rightModule; LoggableController driver; LoggableController operator; + Manipulation manipulation; LoggablePowerDistribution pdp; LoggableCompressor compressor; @@ -47,6 +48,8 @@ public double deadband(double in) { return joystickDeadband.scale(out); } + boolean manipulationEnabled = true; + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -57,6 +60,14 @@ public void robotInit() { driver = new LoggableController("Driver", 0); operator = new LoggableController("Operator", 1); + + if (manipulationEnabled) { + System.out.println("Initializing manipulation..."); + manipulation = new Manipulation(0, 1, 7, 8); + } else { + System.out.println("Manipulation initialization disabled."); + } + logger = new Logger(); timer = new LoggableTimer(); @@ -113,6 +124,16 @@ public void teleopInit() { @Override public void teleopPeriodic() { // Robot code goes here + if (this.manipulationEnabled) { + if (driver.getRightBumperPressed()) { + manipulation.setIntakeExtend(true); + } else if (driver.getLeftBumperPressed()) { + manipulation.setIntakeExtend(false); + } + manipulation.setIntakeSpin(operator.getYButton()); + manipulation.setIndexLoad(operator.getXButton()); + } + if (this.drivetrainEnabled) { if (tankDriveEnabled) { double leftInput = deadband(-driver.getLeftY()); diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..997e2a4 --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,73 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2022.1.1", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2022.1.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2022.1.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2022.1.1", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2022.1.1", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + } + ] +} \ No newline at end of file