diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java index 1397ad7..4bc1988 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java @@ -9,7 +9,7 @@ import org.cyberarm.engine.V2.CyberarmEngine; public class PrototypeBot1 { - public Servo RackRiserLeft, RackRiserRight; + public Servo RackRiserLeft, RackRiserRight, FrontRiserLeft, FrontRiserRight; private final CyberarmEngine engine; public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, armMotor; @@ -39,6 +39,8 @@ public class PrototypeBot1 { collectorWrist = engine.hardwareMap.servo.get("collector wrist"); RackRiserLeft = engine.hardwareMap.servo.get("RackRiserLeft"); RackRiserRight = engine.hardwareMap.servo.get("RackRiserRight"); + FrontRiserLeft = engine.hardwareMap.servo.get("FrontRiserLeft"); + FrontRiserRight = engine.hardwareMap.servo.get("FrontRiserRight"); //motors direction and encoders frontLeftDrive.setDirection(DcMotorSimple.Direction.FORWARD); diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java index 3a02556..2f51168 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java @@ -38,6 +38,8 @@ public class PrototypeTeleOPState extends CyberarmState { robot.armMotor.setTargetPosition(armTargetPosition); robot.armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); robot.collectorWrist.setPosition(1); + robot.RackRiserLeft.setPosition(0.5); + robot.RackRiserRight.setPosition(0.5); } @@ -174,24 +176,46 @@ public class PrototypeTeleOPState extends CyberarmState { if (engine.gamepad1.x) { - robot.RackRiserLeft.setPosition(0.5); - robot.RackRiserRight.setPosition(1-.5); -// .5's are originally .13's. + robot.RackRiserLeft.setPosition(1); + robot.RackRiserRight.setPosition(0); + } if (engine.gamepad2.right_stick_y > 0.1) { - robot.RackRiserRight.setPosition(+ 1); - robot.RackRiserLeft.setPosition(+ 1); + robot.RackRiserRight.setPosition(1); + robot.RackRiserLeft.setPosition(-1); } if (engine.gamepad2.right_stick_y < -0.1) { - robot.RackRiserLeft.setPosition(- 1); - robot.RackRiserRight.setPosition(- 1); + robot.RackRiserRight.setPosition(-1); + robot.RackRiserLeft.setPosition(1); } + if (engine.gamepad2.left_stick_y > 0.1) { + + robot.FrontRiserLeft.setPosition(-1); + robot.FrontRiserRight.setPosition(1); + + } + + if (engine.gamepad2.left_stick_y < -0.1) { + + robot.FrontRiserLeft.setPosition(1); + robot.FrontRiserRight.setPosition(-1); + + } + + if (engine.gamepad2.right_bumper) { + + robot.RackRiserRight.setPosition(0.5); + robot.RackRiserLeft.setPosition(0.5); + + } + + } } \ No newline at end of file