diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java index 6ae93c0..b6b46d2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java @@ -44,6 +44,8 @@ public class DevTeleop extends OpMode { public DcMotor backLeftMotor; public DcMotor frontRightMotor; public DcMotor backRightMotor; + + private double MAX_POWER = .6; @Override public void init() { claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED); @@ -67,7 +69,8 @@ public class DevTeleop extends OpMode { currentGamepad1 = new Gamepad(); previousGamepad1 = new Gamepad(); - + currentGamepad2 = new Gamepad(); + previousGamepad2 = new Gamepad(); } public void theDrop(ArmSubsystem arm, WristSubsystem wrist) { @@ -98,6 +101,7 @@ public class DevTeleop extends OpMode { public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) { if (currentGamepad2.y && !previousGamepad2.y) { lift.toLowBucket(); + arm.bucketArm(); wrist.bucketWrist(); } } @@ -105,6 +109,7 @@ public class DevTeleop extends OpMode { if (currentGamepad2.a && !previousGamepad2.a) { lift.toHighBucket(); + arm.bucketArm(); wrist.bucketWrist(); } } @@ -112,6 +117,9 @@ public class DevTeleop extends OpMode { public void loop() { previousGamepad1.copy(currentGamepad1); currentGamepad1.copy(gamepad1); + previousGamepad2.copy(currentGamepad2); + currentGamepad2.copy(gamepad2); + theDrop(arm, wrist); thePickup(claw); @@ -148,10 +156,10 @@ public class DevTeleop extends OpMode { // Send calculated power to wheels - frontLeftMotor.setPower(leftFrontPower); - frontRightMotor.setPower(rightFrontPower); - backLeftMotor.setPower(leftBackPower); - backRightMotor.setPower(rightBackPower); + frontLeftMotor.setPower(leftFrontPower * MAX_POWER); + frontRightMotor.setPower(rightFrontPower * MAX_POWER); + backLeftMotor.setPower(leftBackPower * MAX_POWER); + backRightMotor.setPower(rightBackPower * MAX_POWER); // Show the elapsed game time and wheel power. telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);