diff --git a/src/main/java/org/wildstang/year2023/robot/WSInputs.java b/src/main/java/org/wildstang/year2023/robot/WSInputs.java index b5ccb94b..383f81d3 100644 --- a/src/main/java/org/wildstang/year2023/robot/WSInputs.java +++ b/src/main/java/org/wildstang/year2023/robot/WSInputs.java @@ -126,8 +126,8 @@ public enum WSInputs implements Inputs { // --------------------------------- // Manipulator Triggers // --------------------------------- - MANIPULATOR_LEFT_TRIGGER ("Manipulator left trigger", new WsJSJoystickInputConfig(1, JoystickConstants.LEFT_TRIGGER)), - MANIPULATOR_RIGHT_TRIGGER ("Manipulator right trigger", new WsJSJoystickInputConfig(1, JoystickConstants.RIGHT_TRIGGER)), + MANIPULATOR_LEFT_TRIGGER ("Intake ingest", new WsJSJoystickInputConfig(1, JoystickConstants.LEFT_TRIGGER)), + MANIPULATOR_RIGHT_TRIGGER ("Intake expel", new WsJSJoystickInputConfig(1, JoystickConstants.RIGHT_TRIGGER)), // ******************************** // Digital IOs diff --git a/src/main/java/org/wildstang/year2023/robot/WSOutputs.java b/src/main/java/org/wildstang/year2023/robot/WSOutputs.java index ae05c776..23014994 100644 --- a/src/main/java/org/wildstang/year2023/robot/WSOutputs.java +++ b/src/main/java/org/wildstang/year2023/robot/WSOutputs.java @@ -42,7 +42,7 @@ public enum WSOutputs implements Outputs { DRIVE4("Module 4 Drive Motor", new WsSparkMaxConfig(CANConstants.DRIVE4, true)), ANGLE4("Module 4 Angle Motor", new WsSparkMaxConfig(CANConstants.ANGLE4, true)), - + INTAKE_MOTOR("Intake Motor", new WsSparkMaxConfig(CANConstants.INTAKE, true)), // --------------------------------- // Servos // --------------------------------- diff --git a/src/main/java/org/wildstang/year2023/robot/WSSubsystems.java b/src/main/java/org/wildstang/year2023/robot/WSSubsystems.java index 8c9f3ec4..cd21bbd2 100644 --- a/src/main/java/org/wildstang/year2023/robot/WSSubsystems.java +++ b/src/main/java/org/wildstang/year2023/robot/WSSubsystems.java @@ -4,6 +4,7 @@ import org.wildstang.year2023.subsystems.SampleSubsystem; import org.wildstang.year2023.subsystems.swerve.SwerveDrive; import org.wildstang.year2023.subsystems.targeting.AimHelper; +import org.wildstang.year2023.subsystems.intake.intake; /** * All subsystems are enumerated here. @@ -13,6 +14,7 @@ public enum WSSubsystems implements Subsystems { // enumerate subsystems SWERVE_DRIVE("Swerve Drive", SwerveDrive.class), + INTAKE("Intake", intake.class), //AIM_HELPER("Aim Helper", AimHelper.class), //SAMPLE("Sample", SampleSubsystem.class) ; diff --git a/src/main/java/org/wildstang/year2023/subsystems/intake/intake.java b/src/main/java/org/wildstang/year2023/subsystems/intake/intake.java new file mode 100644 index 00000000..e521f4b2 --- /dev/null +++ b/src/main/java/org/wildstang/year2023/subsystems/intake/intake.java @@ -0,0 +1,84 @@ +package org.wildstang.year2023.subsystems.intake; + +import org.wildstang.framework.core.Core; +import org.wildstang.framework.io.inputs.DigitalInput; +import org.wildstang.framework.io.inputs.AnalogInput; +import org.wildstang.framework.io.inputs.Input; +import org.wildstang.framework.subsystems.Subsystem; +import org.wildstang.hardware.roborio.inputs.WsJoystickAxis; +import org.wildstang.year2023.robot.WSInputs; + +import org.wildstang.framework.subsystems.Subsystem; + +import org.wildstang.hardware.roborio.outputs.WsSparkMax; +import org.wildstang.year2023.robot.WSOutputs; + +/** + * Conrols intake motor with two triggers. Speed proportional to amount trigger is pressed. + * @author jwannebo3524 + * @author dimwitt + * @author dimwitt71 + */ +public class intake implements Subsystem { + // inputs + + // outputs + private WsSparkMax intakeMotor; + private AnalogInput ingest, expel; + + + // states + private static final double ingestSpeed = 1; + private static final double expelSpeed = -1; + private static final double holdingSpeed = 0.1; + private static final double deadband = 0.05; + + private double speed; + + private boolean isHolding; + @Override + public void init() { + + intakeMotor = (WsSparkMax) Core.getOutputManager().getOutput(WSOutputs.INTAKE_MOTOR); + ingest = (AnalogInput) Core.getInputManager().getInput(WSInputs.MANIPULATOR_LEFT_TRIGGER); + ingest.addInputListener(this); + expel = (AnalogInput) Core.getInputManager().getInput(WSInputs.MANIPULATOR_RIGHT_TRIGGER); + expel.addInputListener(this); + resetState(); + } + + @Override + public void resetState() { + speed = 0; + isHolding = false; + } + + @Override + public void update() { + intakeMotor.setValue(speed); + } + + @Override + public void inputUpdate(Input source) { + double in = Math.abs(ingest.getValue()); + double out = Math.abs(expel.getValue()); + if (in > deadband && in >= out) { + speed = ingestSpeed * in; + isHolding = true; + } else if (out > deadband && out >= in) { + speed = expelSpeed * out; + isHolding = false; + } else { + speed = (isHolding? 1.0 : 0.0) * holdingSpeed; + } + } + + @Override + public String getName() { + return "Intake"; + } + + @Override + public void selfTest() { + } +} \ No newline at end of file