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 b0f2922..69d9648 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java @@ -3,6 +3,7 @@ package org.timecrafters.testing.states; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; import org.cyberarm.engine.V2.CyberarmEngine; @@ -14,6 +15,8 @@ public class PrototypeBot1 { public CRServo collectorLeft, collectorRight; + public Servo collectorWrist; + public PrototypeBot1(CyberarmEngine engine) { this.engine = engine; @@ -32,6 +35,7 @@ public class PrototypeBot1 { // servo configuration collectorLeft = engine.hardwareMap.crservo.get("collector left"); collectorRight = engine.hardwareMap.crservo.get("collector right"); + collectorWrist = engine.hardwareMap.servo.get("collector wrist"); //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 4243c61..ce6ccbe 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java @@ -4,6 +4,8 @@ import com.qualcomm.robotcore.hardware.DcMotor; import org.cyberarm.engine.V2.CyberarmState; +import java.lang.annotation.Target; + public class PrototypeTeleOPState extends CyberarmState { private final PrototypeBot1 robot; @@ -13,6 +15,7 @@ public class PrototypeTeleOPState extends CyberarmState { private boolean bprev; private double drivePower = 1; private boolean UpDPad; + private int armTargetPosition = 0, armCollectPosition = 125, armDeliverPosition = 400; public PrototypeTeleOPState(PrototypeBot1 robot) { this.robot = robot; @@ -20,16 +23,18 @@ public class PrototypeTeleOPState extends CyberarmState { @Override public void telemetry() { - engine.telemetry.addData("Arm Position", robot.armMotor.getCurrentPosition()); + engine.telemetry.addData("Arm Power", robot.armMotor.getPower()); + engine.telemetry.addData("Arm Target Position", robot.armMotor.getTargetPosition()); + engine.telemetry.addData("Arm Current Position", robot.armMotor.getCurrentPosition()); } @Override public void init() { robot.armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - robot.armMotor.setTargetPosition(600); + robot.armMotor.setTargetPosition(armTargetPosition); robot.armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); - + robot.collectorWrist.setPosition(1); } @@ -72,7 +77,7 @@ public class PrototypeTeleOPState extends CyberarmState { double frontRightPower = (y - x - rx) / denominator; double backRightPower = (y + x - rx) / denominator; - // As I programed this and ran it, I realized everything was backwards + // As I programmed this and ran it, I realized everything was backwards // in direction so to fix that i either went in the robot object state and reversed // directions on drive motors or put a negative in behind the joystick power to reverse it. // I put negatives in to reverse it because it was the easiest at the moment. @@ -81,36 +86,83 @@ public class PrototypeTeleOPState extends CyberarmState { robot.backLeftDrive.setPower(backLeftPower); robot.frontRightDrive.setPower(-frontRightPower); robot.backRightDrive.setPower(backRightPower); - robot.armMotor.setPower(engine.gamepad1.left_stick_y * 0.5); - if (engine.gamepad1.y) { + + + robot.armMotor.setPower(engine.gamepad2.left_stick_y * 0.5); + + + if (engine.gamepad2.y) { robot.collectorLeft.setPower(0); robot.collectorRight.setPower(0); } - if (engine.gamepad1.b) { + if (engine.gamepad2.b) { robot.collectorLeft.setPower(1); robot.collectorRight.setPower(-1); } - if (engine.gamepad1.x) { + if (engine.gamepad2.x) { robot.collectorLeft.setPower(-1); robot.collectorRight.setPower(1); } - if (engine.gamepad1.dpad_up) { - - robot.armMotor.setPower(1); - } else { - robot.armMotor.setPower(0); + if (engine.gamepad2.dpad_left) { + robot.collectorWrist.setPosition(0.2); } + + if (engine.gamepad2.dpad_right) { + + robot.collectorWrist.setPosition(1); + + } + + if (engine.gamepad2.dpad_up) { + + robot.armMotor.setPower(1); + } else if (engine.gamepad2.dpad_down) { + robot.armMotor.setPower(0); + + } +// int armTargetPosition = robot.armMotor.getCurrentPosition(); + + if (engine.gamepad2.left_trigger > 0.1) { + armTargetPosition += 1; + if (armTargetPosition > 600) { + armTargetPosition = 600; + } + } + + if (engine.gamepad2.right_trigger > 0.1) { + armTargetPosition -= 1; + if (armTargetPosition <= 0) { + armTargetPosition = 0; + + } + + } + + if (engine.gamepad2.left_bumper) { + + armTargetPosition = armDeliverPosition; + + } + + if (engine.gamepad2.right_bumper) { + + armTargetPosition = armCollectPosition; + + } + + robot.armMotor.setTargetPosition(armTargetPosition); + } } \ No newline at end of file