diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsLinearOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsLinearOpMode.java index 1741a42b0..f609a65d6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsLinearOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotsLinearOpMode.java @@ -33,6 +33,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.util.ElapsedTime; @@ -79,6 +80,9 @@ public class CometBotsLinearOpMode extends LinearOpMode { @Override public void runOpMode() { + // Button A counter to cycle speeds + int cntBtnA = 1; + // Initialize the hardware variables. Note that the strings used here must correspond // to the names assigned during the robot configuration step on the DS or RC devices. rhw = hardwareMap.get(DcMotor.class, "right hand wheel"); @@ -116,22 +120,22 @@ public class CometBotsLinearOpMode extends LinearOpMode { // Combine the joystick requests for each axis-motion to determine each wheel's power. // Set up a variable for each drive wheel to save the power level for telemetry. - double leftFrontPower = axial + lateral + yaw; - double rightFrontPower = axial - lateral - yaw; - double leftBackPower = axial - lateral + yaw; - double rightBackPower = axial + lateral - yaw; + double lftFrntPwr = axial + lateral + yaw; + double rtFrntPwr = axial - lateral - yaw; + double ltBckPwr = axial - lateral + yaw; + double rtBckPwr = axial + lateral - yaw; // Normalize the values so no wheel power exceeds 100% // This ensures that the robot maintains the desired motion. - max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); - max = Math.max(max, Math.abs(leftBackPower)); - max = Math.max(max, Math.abs(rightBackPower)); + max = Math.max(Math.abs(lftFrntPwr), Math.abs(rtFrntPwr)); + max = Math.max(max, Math.abs(ltBckPwr)); + max = Math.max(max, Math.abs(rtBckPwr)); if (max > 1.0) { - leftFrontPower /= max; - rightFrontPower /= max; - leftBackPower /= max; - rightBackPower /= max; + lftFrntPwr /= max; + rtFrntPwr /= max; + ltBckPwr /= max; + rtBckPwr /= max; } // This is test code: @@ -145,22 +149,39 @@ public class CometBotsLinearOpMode extends LinearOpMode { // Once the correct motors move in the correct direction re-comment this code. /* - leftFrontPower = gamepad1.x ? 1.0 : 0.0; // X gamepad - leftBackPower = gamepad1.a ? 1.0 : 0.0; // A gamepad - rightFrontPower = gamepad1.y ? 1.0 : 0.0; // Y gamepad - rightBackPower = gamepad1.b ? 1.0 : 0.0; // B gamepad + lftFrntPwr = gamepad1.x ? 1.0 : 0.0; // X gamepad + ltBckPwr = gamepad1.a ? 1.0 : 0.0; // A gamepad + rtFrntPwr = gamepad1.y ? 1.0 : 0.0; // Y gamepad + rtBckPwr = gamepad1.b ? 1.0 : 0.0; // B gamepad */ + lhw.setPower(lftFrntPwr/cntBtnA); + rhw.setPower(rtFrntPwr/cntBtnA); + llw.setPower(ltBckPwr/cntBtnA); + rlw.setPower(rtBckPwr/cntBtnA); + // Send calculated power to wheels - lhw.setPower(leftFrontPower); - rhw.setPower(rightFrontPower); - llw.setPower(leftBackPower); - rlw.setPower(rightBackPower); + if(gamepad1.left_bumper) { + sleep(175); + cntBtnA--; + if(cntBtnA < 0) { + cntBtnA = 0; + } + } + + if(gamepad1.right_bumper) { + sleep(175); + cntBtnA++; + if(cntBtnA > 4) { + cntBtnA = 4; + } + } // Show the elapsed game time and wheel power. telemetry.addData("Status", "Run Time: " + runtime.toString()); - telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); - telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + telemetry.addData("Front left/Right", "%4.2f, %4.2f", lftFrntPwr, rtFrntPwr); + telemetry.addData("Back left/Right", "%4.2f, %4.2f", ltBckPwr, rtBckPwr); + telemetry.addData("Power Mode", cntBtnA); telemetry.update(); } }} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/MovementLibrary.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/MovementLibrary.java index 6d33663c3..43a070e94 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/MovementLibrary.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/MovementLibrary.java @@ -12,21 +12,21 @@ public class MovementLibrary { public static void setLeftSideForward(DcMotor[] motors) { motors[2].setDirection(DcMotor.Direction.FORWARD); - motors[3].setDirection(DcMotor.Direction.REVERSE); + motors[3].setDirection(DcMotor.Direction.FORWARD); } public static void setRightSideForward(DcMotor[] motors) { - motors[0].setDirection(DcMotor.Direction.REVERSE); + motors[0].setDirection(DcMotor.Direction.FORWARD); motors[1].setDirection(DcMotor.Direction.FORWARD); } public static void setLeftSideBackward(DcMotor[] motors) { motors[2].setDirection(DcMotor.Direction.REVERSE); - motors[3].setDirection(DcMotor.Direction.FORWARD); + motors[3].setDirection(DcMotor.Direction.REVERSE); } public static void setRightSideBackward(DcMotor[] motors) { - motors[0].setDirection(DcMotor.Direction.FORWARD); + motors[0].setDirection(DcMotor.Direction.REVERSE); motors[1].setDirection(DcMotor.Direction.REVERSE); }