diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java new file mode 100644 index 0000000..cf22479 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java @@ -0,0 +1,97 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; + +@TeleOp(name = "Dev Teleop Remix", group = "Debug") +@Disabled +public class DevTeleOpRemix extends OpMode { + + public ClawSubsystem claw; + public ArmSubsystem arm; + public WristSubsystem wrist; + public LiftSubsystem lift; + public MotorsSubsystem motors; + + public Gamepad currentGamepad1; + public Gamepad previousGamepad1; + public Gamepad currentGamepad2; + public Gamepad previousGamepad2; + + public double power = .6; + + @Override + public void init() { + + claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED); + arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK); + wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR); + lift = new LiftSubsystem(hardwareMap); + motors = new MotorsSubsystem(hardwareMap, telemetry, power); + + claw.init(); + arm.init(); + wrist.init(); + lift.init(); + motors.init(); + + currentGamepad1 = new Gamepad(); + previousGamepad1 = new Gamepad(); + currentGamepad2 = new Gamepad(); + previousGamepad2 = new Gamepad(); + } + + public void theDrop(ArmSubsystem arm, WristSubsystem wrist) { + if (currentGamepad1.a && !previousGamepad1.a) { + wrist.floorWrist(); + arm.engageArm(); + } + } + + public void thePickup(ClawSubsystem claw) { + if (currentGamepad1.x && !previousGamepad1.x) { + claw.switchState(); + } + } + + public void theLift(ArmSubsystem arm, WristSubsystem wrist) { + if (currentGamepad1.b && !previousGamepad1.b) { + arm.parkArm(); + wrist.bucketWrist(); + } + } + + public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) { + if (currentGamepad1.y && !previousGamepad1.y) { + lift.toLowBucket(); + wrist.bucketWrist(); + } + } + + @Override + public void loop() { + + previousGamepad1.copy(currentGamepad1); + currentGamepad1.copy(gamepad1); + + previousGamepad2.copy(currentGamepad2); + currentGamepad2.copy(gamepad2); + + theDrop(arm, wrist); + thePickup(claw); + theLift(arm, wrist); + theLowBucketScore(lift, wrist, arm); + + motors.calculateTrajectory(gamepad1); + + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java new file mode 100644 index 0000000..21002ce --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java @@ -0,0 +1,107 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; + +@TeleOp(name = "Dev Teleop Remix Deux", group = "Debug") +@Disabled +public class DevTeleOpRemixDeux extends OpMode { + + private Follower follower; + + public ClawSubsystem claw; + public ArmSubsystem arm; + public WristSubsystem wrist; + public LiftSubsystem lift; + public MotorsSubsystem motors; + + public Gamepad currentGamepad1; + public Gamepad previousGamepad1; + public Gamepad currentGamepad2; + public Gamepad previousGamepad2; + + public double power = .6; + + @Override + public void init() { + + follower = new Follower(hardwareMap); + + claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED); + arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK); + wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR); + motors = new MotorsSubsystem(hardwareMap, telemetry); + lift = new LiftSubsystem(hardwareMap); + + claw.init(); + arm.init(); + wrist.init(); + lift.init(); + motors.init(); + + currentGamepad1 = new Gamepad(); + previousGamepad1 = new Gamepad(); + currentGamepad2 = new Gamepad(); + previousGamepad2 = new Gamepad(); + + follower.setMaxPower(this.power); + follower.startTeleopDrive(); + + } + + public void theDrop(ArmSubsystem arm, WristSubsystem wrist) { + if (currentGamepad1.a && !previousGamepad1.a) { + wrist.floorWrist(); + arm.engageArm(); + } + } + + public void thePickup(ClawSubsystem claw) { + if (currentGamepad1.x && !previousGamepad1.x) { + claw.switchState(); + } + } + + public void theLift(ArmSubsystem arm, WristSubsystem wrist) { + if (currentGamepad1.b && !previousGamepad1.b) { + arm.parkArm(); + wrist.bucketWrist(); + } + } + + public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) { + if (currentGamepad1.y && !previousGamepad1.y) { + lift.toLowBucket(); + wrist.bucketWrist(); + } + } + + @Override + public void loop() { + + previousGamepad1.copy(currentGamepad1); + currentGamepad1.copy(gamepad1); + + previousGamepad2.copy(currentGamepad2); + currentGamepad2.copy(gamepad2); + + theDrop(arm, wrist); + thePickup(claw); + theLift(arm, wrist); + theLowBucketScore(lift, wrist, arm); + + follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x); + follower.update(); + + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java index 66e656d..048f264 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ArmTest.java @@ -27,7 +27,7 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.cometbots.tests; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClawTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClawTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java index e7bfa4c..278df77 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClawTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/ClawTest.java @@ -27,7 +27,7 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.cometbots.tests; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftRawTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftRawTest.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftRawTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftRawTest.java index f51e1a5..4065a95 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftRawTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftRawTest.java @@ -27,7 +27,7 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.cometbots.tests; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftTest.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftTest.java index 30b4dc3..88267d3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/LiftTest.java @@ -27,12 +27,10 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.cometbots.tests; 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; @@ -76,7 +74,7 @@ public class LiftTest extends LinearOpMode { currentGamepad1.copy(gamepad1); if (currentGamepad1.square && !previousGamepad1.square) { - lift.toZero(); + lift.toFloor(); } if (currentGamepad1.triangle && !previousGamepad1.triangle) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/WristTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/WristTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java index 296ae94..3ead2d6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/WristTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/WristTest.java @@ -27,7 +27,7 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.cometbots.tests; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java index aa62e1c..cf66dce 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java @@ -13,7 +13,8 @@ public class RobotConstants { public static double wristFloor = 0.625; public static double wristBucket = 0.215; - public static int liftZeroPos = 0; + public static int liftToFloorPos = 0; + public static int liftToPoolPos = 500; public static int liftToLowBucketPos = 2250; public static int liftToHighBucketPos = 3700; public static double liftPower = .45; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java index 257c3d8..6fd3fc2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Auto.java @@ -33,6 +33,8 @@ public class Auto { public Follower follower; public Telemetry telemetry; + public int caseState = 1; + public Auto(HardwareMap hardwareMap, Telemetry telemetry, Follower follower) { claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED); arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK); @@ -61,23 +63,52 @@ public class Auto { } public void update() { - //follower.update(); - if (clawTimer.getElapsedTimeSeconds() > 2) { - claw.openClaw(); - } - if (armTimer.getElapsedTimeSeconds() > 4) { - arm.engageArm(); - } - if (clawTimer.getElapsedTimeSeconds() > 6) { - claw.closeClaw(); - } - if (armTimer.getElapsedTimeSeconds() > 8) { - arm.bucketArm(); - wrist.bucketWrist(); - } - if (clawTimer.getElapsedTimeSeconds() > 10) { - claw.openClaw(); + + this.telemetry.addData("Current State", caseState); + this.telemetry.addData("Claw Timer", clawTimer.getElapsedTimeSeconds()); + this.telemetry.addData("Arm Timer", armTimer.getElapsedTimeSeconds()); + this.telemetry.addData("Wrist Timer", wristTimer.getElapsedTimeSeconds()); + this.telemetry.update(); + + switch(caseState) { + case 1: + claw.openClaw(); + caseState = 2; + break; + case 2: + if (clawTimer.getElapsedTimeSeconds() > 2) { + arm.engageArm(); + caseState = 3; + } + break; + case 3: + if (armTimer.getElapsedTimeSeconds() > 4) { + wrist.floorWrist(); + caseState = 4; + } + break; + case 4: + if (clawTimer.getElapsedTimeSeconds() > 6) { + claw.closeClaw(); + caseState = 5; + } + break; + case 5: + if (armTimer.getElapsedTimeSeconds() > 8) { + arm.bucketArm(); + wrist.bucketWrist(); + caseState = 6; + } + break; + case 6: + if (clawTimer.getElapsedTimeSeconds() > 10) { + claw.openClaw(); + caseState = 7; + } + break; + case 7: + this.init(); + break; } } - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java index 33bd585..ac895b5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java @@ -3,7 +3,7 @@ package org.firstinspires.ftc.teamcode.subsystem; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos; -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftZeroPos; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToPoolPos; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; @@ -14,16 +14,17 @@ import org.firstinspires.ftc.teamcode.util.action.RunAction; public class LiftSubsystem { public DcMotor lift; - public RunAction toZero, toLowBucket, toHighBucket; + public RunAction toFloor, toLowBucket, toHighBucket; + public enum LiftState { - FLOOR, LOW_BUCKET, HIGH_BUCKET + FLOOR, LOW_BUCKET, HIGH_BUCKET, POOL } + private LiftState liftState; public LiftSubsystem(HardwareMap hardwareMap) { lift = hardwareMap.get(DcMotor.class, "lift-motor"); - - toZero = new RunAction(this::toZero); + toFloor = new RunAction(this::toFloor); toLowBucket = new RunAction(this::toLowBucket); toHighBucket = new RunAction(this::toHighBucket); } @@ -33,29 +34,36 @@ public class LiftSubsystem { lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); } - public void toZero() { - lift.setTargetPosition(liftZeroPos); - lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - public void switchState() { - if(this.liftState == LiftState.FLOOR) { + if (this.liftState == LiftState.FLOOR) { + this.toPool(); + } else if (this.liftState == LiftState.POOL) { this.toLowBucket(); } else if (this.liftState == LiftState.LOW_BUCKET) { this.toHighBucket(); } else if (this.liftState == LiftState.HIGH_BUCKET) { - this.toZero(); + this.toFloor(); } } + public void toFloor() { + this.setTarget(liftToPoolPos); + this.setState(LiftState.FLOOR); + } + + public void toPool() { + this.setTarget(liftToPoolPos); + this.setState(LiftState.POOL); + } + public void toLowBucket() { - lift.setTargetPosition(liftToLowBucketPos); - lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.setTarget(liftToLowBucketPos); + this.setState(LiftState.LOW_BUCKET); } public void toHighBucket() { - lift.setTargetPosition(liftToHighBucketPos); - lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.setTarget(liftToHighBucketPos); + this.setState(LiftState.HIGH_BUCKET); } public void init() { @@ -73,7 +81,7 @@ public class LiftSubsystem { } public void start() { - setTarget(10); + this.toFloor(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java new file mode 100644 index 0000000..30fb764 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java @@ -0,0 +1,117 @@ +package org.firstinspires.ftc.teamcode.subsystem; + +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; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION; +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 com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +public class MotorsSubsystem { + + public HardwareMap hardwareMap; + + public Telemetry telemetry; + + public DcMotor frontLeftMotor; + public DcMotor backLeftMotor; + public DcMotor frontRightMotor; + public DcMotor backRightMotor; + + public double power; + + public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { + this.hardwareMap = hardwareMap; + this.telemetry = telemetry; + this.power = 1.0; + } + + public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry, double power) { + this.hardwareMap = hardwareMap; + this.telemetry = telemetry; + this.power = power; + } + + public void init() { + frontLeftMotor = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR); + backLeftMotor = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR); + frontRightMotor = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR); + backRightMotor = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR); + + frontLeftMotor.setDirection(FRONT_LEFT_MOTOR_DIRECTION); + backLeftMotor.setDirection(BACK_LEFT_MOTOR_DIRECTION); + frontRightMotor.setDirection(FRONT_RIGHT_MOTOR_DIRECTION); + backRightMotor.setDirection(BACK_RIGHT_MOTOR_DIRECTION); + + frontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + frontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + + public void setFrontLeftMotorPower(double power) { + frontLeftMotor.setPower(power); + } + + public void setBackLeftMotorPower(double power) { + backLeftMotor.setPower(power); + } + + public void setFrontRightMotorPower(double power) { + frontRightMotor.setPower(power); + } + + public void setBackRightMotorPower(double power) { + backRightMotor.setPower(power); + } + + public void calculateTrajectory(Gamepad gamepad1) { + 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; + + // 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 leftBackPower = axial - lateral + yaw; + + double rightFrontPower = 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. + max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); + max = Math.max(max, Math.abs(leftBackPower)); + max = Math.max(max, Math.abs(rightBackPower)); + + if (max > 1.0) { + leftFrontPower /= max; + rightFrontPower /= max; + leftBackPower /= max; + rightBackPower /= max; + } + + // Send calculated power to wheels + this.setFrontLeftMotorPower(leftFrontPower * this.power); + this.setFrontRightMotorPower(rightFrontPower * this.power); + this.setBackLeftMotorPower(leftBackPower * this.power); + this.setBackRightMotorPower(rightBackPower * this.power); + + // Show the elapsed game time and wheel power. + this.telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); + this.telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + this.telemetry.update(); + } + +}