diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FullRobotControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FullRobotControl.java index 6cbc362..69e5784 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FullRobotControl.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FullRobotControl.java @@ -23,7 +23,12 @@ public class FullRobotControl extends OpMode { DcMotor armMotor; Servo gripper; Servo wrist; + DcMotor arm; public ElapsedTime runtime = new ElapsedTime(); + static final double TICKS_TO_DEGREES = 0.07462686567; + static final double COUNTS_PER_MOTOR_REV = 537.6; + static final double DRIVE_GEAR_REDUCTION = 1.0; + static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.7 * Math.PI); /** @@ -64,6 +69,7 @@ public class FullRobotControl extends OpMode { gripper = hardwareMap.servo.get("gripper"); gripper.setPosition(1); wrist = hardwareMap.servo.get("wrist"); + arm = hardwareMap.dcMotor.get("arm raise"); wrist.setPosition(1); } @@ -165,6 +171,43 @@ public class FullRobotControl extends OpMode { gripper.setPosition(1); } + int armState = 0; + if(gamepad2.x) + { + armState = 1; + raisearm(0); + } + if(gamepad2.y) + { + armState = 2; + raisearm(30); + } + if(gamepad2.y) + { + armState = 2; + raisearm(200); + } + if(gamepad2.right_bumper) + { + armState =+ 1; + } + if(gamepad2.left_bumper) + { + armState =- 1; + } + if(armState == 1) + { + raisearm(0); + } + if(armState == 2) + { + raisearm(30); + } + if(armState == 1) + { + raisearm(200); + } + @@ -182,4 +225,34 @@ public class FullRobotControl extends OpMode { telemetry.update(); } -} \ No newline at end of file + public void raisearm(int degrees) + { + armEncoder(.2, degrees*TICKS_TO_DEGREES); + + } + public void armEncoder(double speed, + double Inches) { + int newarmTarget; + + + // Determine new target position, and pass to motor controller + newarmTarget = arm.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH); + arm.setTargetPosition(newarmTarget); + + // Turn On RUN_TO_POSITION + arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + // reset the timeout time and start motion. + runtime.reset(); + arm.setPower(Math.abs(speed)); + + + arm.setPower(0); + + + // Turn off RUN_TO_POSITION + arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + } + } +