diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java index 78c56d3..8918736 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java @@ -6,6 +6,7 @@ import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference. import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; @@ -78,9 +79,13 @@ public class Robot { // Collector public Servo collectorDispenser; + public CRServo turretServo1; + // Depositor public Servo depositorDispenser; + public CRServo turretServo2; + public Robot(CyberarmEngine engine) { this.engine = engine; @@ -204,13 +209,14 @@ public class Robot { collectorArmBobbin = engine.hardwareMap.dcMotor.get("collectorArmBobbin"); collectorDispenser = engine.hardwareMap.servo.get("collectorDispenser"); collectorArmBobbin.setDirection(DcMotorSimple.Direction.FORWARD); + turretServo1 = engine.hardwareMap.crservo.get("turretServo1"); } private void initDepositor(){ depositorDispenser = engine.hardwareMap.servo.get("depositorDispenser"); depositorArmBobbin = engine.hardwareMap.dcMotor.get("depositorArmBobbin"); depositorArmBobbin.setDirection(DcMotorSimple.Direction.REVERSE); - + turretServo2 = engine.hardwareMap.crservo.get("turretServo2"); } private void initCarousel() { diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java index 87d616d..ce579be 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java @@ -31,21 +31,43 @@ public class TeleOpState extends CyberarmState { robot.driveWarehouseRight.setPower(-engine.gamepad1.right_stick_y * maxDriveSpeed); robot.driveGoalRight.setPower(-engine.gamepad1.right_stick_y * maxDriveSpeed); - + // dispenser powered if (engine.gamepad1.right_bumper){ robot.collectorDispenser.setPosition(.5); - } else { - robot.collectorDispenser.setPosition(0); } + // if not pressed dispenser off + else { + robot.collectorDispenser.setPosition(0); + } + //if one of triggers pressed arm extends or unextends robot.collectorArmBobbin.setPower(engine.gamepad1.right_trigger * maxCollectorArmSpeed); if (engine.gamepad1.right_trigger <= 0){ robot.collectorArmBobbin.setPower(-engine.gamepad1.left_trigger * maxCollectorArmSpeed); } + // if either of these buttons move the servo + // turretServo1 = orange + if (engine.gamepad1.dpad_right || engine.gamepad1.dpad_left) { + + if (engine.gamepad1.dpad_right) { + robot.turretServo1.setPower(1); + } + + if (engine.gamepad1.dpad_left) { + robot.turretServo1.setPower(-1); + } + } + // if neither of these buttons power off + else { + robot.turretServo1.setPower(0); + } + // GamePad 2 + + // if triggers are pressed then arm extends or unextends robot.depositorArmBobbin.setPower(engine.gamepad2.right_trigger * maxDepositorArmSpeed); if (engine.gamepad2.right_trigger <= 0) { @@ -60,6 +82,23 @@ public class TeleOpState extends CyberarmState { } + // if either of these buttons move the servo + // turretServo2 = white + if (engine.gamepad2.dpad_right || engine.gamepad2.dpad_left) { + + if (engine.gamepad2.dpad_right) { + robot.turretServo2.setPower(1); + } + + if (engine.gamepad2.dpad_left) { + robot.turretServo2.setPower(-1); + } + } + // if neither of these buttons power off + else { + robot.turretServo2.setPower(0); + } + }