[jvanetten]

@TeleOp(name="Atlas: Teleop POV", group="WRW")

public class AtlasTeleop extends LinearOpMode {


HardwareAtlas robot = new HardwareAtlas();

double clawOffset = 0;
final double CLAW_SPEED = 0.02 ;


public void runOpMode() {
double left;
double right;
double max;


robot.init(hardwareMap);


telemetry.addData("Say", "Hello Atlas Driver");
telemetry.update();


waitForStart();


while (opModeIsActive()) {


left = -gamepad1.left_stick_y + gamepad1.right_stick_x;
right = -gamepad1.left_stick_y - gamepad1.right_stick_x;


max = Math.max(Math.abs(left), Math.abs(right));
if (max > 1.0)
{
left /= max;
right /= max;
}

robot.leftMotor.setPower(left);
robot.rightMotor.setPower(right);


if (gamepad1.right_bumper)
clawOffset += CLAW_SPEED;
else if (gamepad1.left_bumper)
clawOffset -= CLAW_SPEED;


clawOffset = Range.clip(clawOffset, -0.5, 0.5);
robot.leftClaw.setPosition(robot.MID_SERVO + clawOffset);
robot.rightClaw.setPosition(robot.MID_SERVO - clawOffset);


if (gamepad1.y)
robot.armMotor.setPower(robot.ARM_UP_POWER);
else if (gamepad1.a)
robot.armMotor.setPower(robot.ARM_DOWN_POWER);
else
robot.armMotor.setPower(0.0);


telemetry.addData("claw", "Offset = %.2f", clawOffset);
telemetry.addData("left", "%.2f", left);
telemetry.addData("right", "%.2f", right);
telemetry.update();


}
}
}