diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java index 18aa94e..7b23d68 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java @@ -29,8 +29,6 @@ package org.firstinspires.ftc.teamcode; -import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER; -import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION; import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR; import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION; import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR; @@ -39,12 +37,7 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR; import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION; import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR; import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION; -import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER; -import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION; -import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER; -import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION; -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; @@ -81,12 +74,15 @@ import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list */ -@TeleOp(name="Basic: Omni Linear OpMode", group="Linear OpMode") +@TeleOp(name = "Basic: Omni Linear OpMode", group = "Linear OpMode") public class BasicOmniOpMode_Linear extends LinearOpMode { // Declare OpMode members for each of the 4 motors. private final ElapsedTime runtime = new ElapsedTime(); - + private Encoder leftFront; + private Encoder rightFront; + private Encoder leftRear; + private Encoder rightRear; @Override public void runOpMode() { @@ -97,18 +93,12 @@ public class BasicOmniOpMode_Linear extends LinearOpMode { DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR); DcMotor rightBackDrive = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR); + leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, FRONT_LEFT_MOTOR)); + leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_LEFT_MOTOR)); + rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_RIGHT_MOTOR)); + rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, FRONT_RIGHT_MOTOR)); - // TODO: replace these with your encoder ports - Encoder leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER)); - Encoder rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER)); - Encoder strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER)); - - // TODO: reverse any encoders necessary - leftEncoder.setDirection(LEFT_ENCODER_DIRECTION); - rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION); - strafeEncoder.setDirection(BACK_ENCODER_DIRECTION); - // ######################################################################################## // !!! IMPORTANT Drive Information. Test your motor directions. !!!!! // ######################################################################################## @@ -124,11 +114,20 @@ public class BasicOmniOpMode_Linear extends LinearOpMode { rightFrontDrive.setDirection(FRONT_RIGHT_MOTOR_DIRECTION); rightBackDrive.setDirection(BACK_RIGHT_MOTOR_DIRECTION); + leftFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + leftBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + leftFrontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + leftBackDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightFrontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightBackDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + + // Wait for the game to start (driver presses START) telemetry.addData("Status", "Initialized"); - telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition()); - telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition()); - telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition()); telemetry.update(); waitForStart(); @@ -139,16 +138,16 @@ public class BasicOmniOpMode_Linear extends LinearOpMode { double max; // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. - double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value - double lateral = gamepad1.left_stick_x; - double yaw = gamepad1.right_stick_x; + double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value + double lateral = gamepad1.left_stick_x; + double yaw = gamepad1.right_stick_x; // 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 leftFrontPower = axial + lateral + yaw; double rightFrontPower = axial - lateral - yaw; - double leftBackPower = axial - lateral + yaw; - double rightBackPower = axial + lateral - yaw; + double leftBackPower = axial - lateral + yaw; + double rightBackPower = axial + lateral - yaw; // Normalize the values so no wheel power exceeds 100% // This ensures that the robot maintains the desired motion. @@ -157,10 +156,10 @@ public class BasicOmniOpMode_Linear extends LinearOpMode { max = Math.max(max, Math.abs(rightBackPower)); if (max > 1.0) { - leftFrontPower /= max; + leftFrontPower /= max; rightFrontPower /= max; - leftBackPower /= max; - rightBackPower /= max; + leftBackPower /= max; + rightBackPower /= max; } // This is test code: @@ -187,12 +186,14 @@ public class BasicOmniOpMode_Linear extends LinearOpMode { rightBackDrive.setPower(rightBackPower); // Show the elapsed game time and wheel power. - telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Status", "Run Time: " + runtime); telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); - telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition()); - telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition()); - telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition()); + telemetry.addData("Encoder Front Left", leftFrontDrive.getDirection() + " : " + leftFrontDrive.getCurrentPosition()); + telemetry.addData("Encoder Front Right", rightFrontDrive.getCurrentPosition()); + telemetry.addData("Encoder Back Left", leftBackDrive.getCurrentPosition()); + telemetry.addData("Encoder Back Right", rightBackDrive.getCurrentPosition()); telemetry.update(); } - }} + } +}