From 5591ed2205fb38f0286aa799ea20f4b432d0d287 Mon Sep 17 00:00:00 2001 From: KavinSandking <150557351+KavinSandking@users.noreply.github.com> Date: Fri, 26 Dec 2025 06:52:56 -0600 Subject: [PATCH 1/7] Update Differential Drive Documentation --- .../drivetrain-commands/differential.md | 139 +++++------------- 1 file changed, 38 insertions(+), 101 deletions(-) diff --git a/src/nextftc/hardware/drivetrain-commands/differential.md b/src/nextftc/hardware/drivetrain-commands/differential.md index 00b9453..84d1c2b 100644 --- a/src/nextftc/hardware/drivetrain-commands/differential.md +++ b/src/nextftc/hardware/drivetrain-commands/differential.md @@ -6,120 +6,37 @@ > [!TIP] INFO > For a differential drive, there are two types of drive systems. You can use the tank and arcade control schemes with a differential drive. > -> Arcade drive use a y-value input from the controller and a value from the turn stick. We know that when the turn stick is pushed left, the right side should move forward and the left side should move backwards. Therefore, since pushing the turn stick to the left returns a negative value, it should be added to the left speed and subtracted from the right speed. +> Arcade drive uses a y-value input from the controller and a value from the turn stick. We know that when the turn stick is pushed left, the right side should move forward and the left side should move backwards. Therefore, since pushing the turn stick to the left returns a negative value, it should be added to the left speed and subtracted from the right speed. > > Tank drive uses a y-value input from the left and right sticks. The sticks control their respective side of the robot. ## Usage -First, you need to create your motors. Let's create variables for their names: +First, you need to create your motors. Let's create variables for them: :::tabs key:code == Kotlin ```kotlin -val frontLeftName = "front_left" -val frontRightName = "front_right" -val backLeftName = "back_left" -val backRightname = "back_right" -``` - -== Java - -```java -public String frontLeftName = "front_left"; -public String frontRightName = "front_right"; -public String backLeftName = "back_left"; -public String backRightName = "back_right"; -``` - -::: - -Next, we'll create variables for the motors and motor groups for each of the sides. If you only have one motor each side, you can skip creating the motor groups. - -:::tabs key:code -== Kotlin - -```kotlin -lateinit var frontLeftMotor: MotorEx -lateinit var frontRightMotor: MotorEx -lateinit var backLeftMotor: MotorEx -lateinit var backRightMotor: MotorEx - -lateinit var leftMotors: MotorGroup -lateinit var rightMtoors: MotorGroup -``` - -== Java - -```java -public MotorEx frontLeftMotor; -public MotorEx frontRightMotor; -public MotorEx backLeftMotor; -public MotorEx backRightMotor; - -public MotorGroup leftMotors; -public MotorGroup rightMotors; -``` - -::: +private val frontLeftMotor = MotorEx("front_left").breakMode().reversed() +private val frontRightMotor = MotorEx("front_right").breakMode() +private val backLeftMotor = MotorEx("back_left").breakMode().reversed() +private val backRightMotor = MotorEx("back_right").breakMode() -Lastly, we'll create a variable for the command: - -:::tabs key:code -== Kotlin - -```kotlin -lateinit var driverControlled: Command +private val leftMotors = MotorGroup(frontLeftMotor, backLeftMotor); +private val rightMotors = MotorGroup(frontRightMotor, backRightMotor); ``` == Java ```java -public Command driverControlled; -``` - -::: +private MotorEx frontLeftMotor = new MotorEx("front_left").breakMode().reversed(); +private MotorEx frontRightMotor = new MotorEx("front_right").breakMode(); +private MotorEx backLeftMotor = new MotorEx("back_left").breakMode().reversed(); +private MotorEx backRightMotor = new MotorEx("back_right").breakMode(); -Now, in the `onInit()` function, we will initialize all our variables: - -:::tabs key:code -== Kotlin - -```kotlin -frontLeftMotor = MotorEx(frontLeftName) -backLeftMotor = MotorEx(backLeftName) -backRightMotor = MotorEx(backRightName) -frontRightMotor = MotorEx(frontRightName) - -// Change your motor directions to suit your robot. -frontLeftMotor.direction = DcMotorSimple.Direction.REVERSE -backLeftMotor.direction = DcMotorSimple.Direction.REVERSE -frontRightMotor.direction = DcMotorSimple.Direction.FORWARD -backRightMotor.direction = DcMotorSimple.Direction.FORWARD - -// Skip this if you are only using two motors. -leftMotors = MotorGroup(frontLeftMotor, backLeftMotor) -rightMotors = MotorGroup(frontRightMotor, backRightMotor) -``` - -== Java - -```java -frontLeftMotor = new MotorEx(frontLeftName); -backLeftMotor = new MotorEx(backLeftName); -backRightMotor = new MotorEx(backRightName); -frontRightMotor = new MotorEx(frontRightName); - -// Change your motor directions to suit your robot. -frontLeftMotor.setDirection(DcMotorSimple.Direction.REVERSE); -backLeftMotor.setDirection(DcMotorSimple.Direction.REVERSE); -frontRightMotor.setDirection(DcMotorSimple.Direction.FORWARD); -backRightMotor.setDirection(DcMotorSimple.Direction.FORWARD); - -// Skip this if you are only using two motors. -leftMotors = new MotorGroup(frontLeftMotor, backLeftMotor); -rightMotors = new MotorGroup(frontRightMotor, backRightMotor); +private MotorGroup leftMotors = new MotorGroup(frontLeftMotor, backLeftMotor); +private MotorGroup rightMotors = new MotorGroup(frontRightMotor, backRightMotor); ``` ::: @@ -135,14 +52,24 @@ You can run it as a tank drive: == Kotlin ```kotlin -driverControlled = DifferentialTankDriverControlled(leftMotors, rightMotors, gamepadManager.gamepad1) +driverControlled = DifferentialTankDriverControlled( + leftMotors, + rightMotors, + Gamepads.gamepad1.leftStickY, + Gamepads.gamepad1.rightStickY +) driverControlled() ``` == Java ```java -driverControlled = new DifferentialTankDriverControlled(leftMotors, rightMotors, gamepadManager.gamepad1); +driverControlled = new DifferentialTankDriverControlled( + leftMotors, + rightMotors, + Gamepads.gamepad1().leftStickY(), + Gamepads.gamepad1().rightStickY() +); driverControlled.schedule(); ``` @@ -154,14 +81,24 @@ Or as an arcade drive: == Kotlin ```kotlin -driverControlled = DifferentialArcadeDriverControlled(leftMotors, rightMotors, gamepadManager.gamepad1, false, imu) +driverControlled = DifferentialArcadeDriverControlled( + leftMotors, + rightMotors, + Gamepads.gamepad1.leftStickY, + Gamepads.gamepad1.rightStickX +) driverControlled() ``` == Java ```java -driverControlled = new DifferentialArcadeDriverControlled(leftMotors, rightMotors, gamepadManager.gamepad1, false, imu); +driverControlled = new DifferentialArcadeDriverControlled( + leftMotors, + rightMotors, + Gamepads.gamepad1().leftStickY(), + Gamepads.gamepad1().rightStickX() +); driverControlled.schedule(); ``` From a1b22e59bed502a7ebee597f026c5a4eef6c357b Mon Sep 17 00:00:00 2001 From: KavinSandking <150557351+KavinSandking@users.noreply.github.com> Date: Fri, 26 Dec 2025 07:01:57 -0600 Subject: [PATCH 2/7] Fix goal setting in Java portion of example usage of Slides --- src/control/examples/slides.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/control/examples/slides.md b/src/control/examples/slides.md index d40fb90..0a008fb 100644 --- a/src/control/examples/slides.md +++ b/src/control/examples/slides.md @@ -88,17 +88,17 @@ public class SlideExample extends OpMode { .elevatorFF(0.04) .build(); - controller.goal = new KineticState(0.0); + controller.setGoal(new KineticState(0)); } @Override public void loop() { if (gamepad1.a) { - controller.goal = new KineticState(1000.0); + controller.setGoal(new KineticState(1000.0)); } else if (gamepad1.b) { - controller.goal = new KineticState(0.0); + controller.setGoal(new KineticState(0.0)); } else if (gamepad1.x) { - controller.goal = new KineticState(500.0); + controller.setGoal(new KineticState(500.0)); } slideMotor.setPower(controller.calculate( From 61320923f39b53235a9ca5aa199ad3b1decadafd Mon Sep 17 00:00:00 2001 From: KavinSandking <150557351+KavinSandking@users.noreply.github.com> Date: Fri, 26 Dec 2025 07:08:31 -0600 Subject: [PATCH 3/7] Fix goal setting method in Flywheels for the java portions --- src/control/examples/flywheels.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/control/examples/flywheels.md b/src/control/examples/flywheels.md index fdd79e0..8760c6d 100644 --- a/src/control/examples/flywheels.md +++ b/src/control/examples/flywheels.md @@ -81,17 +81,17 @@ public class FlywheelExample extends OpMode { .velPid(0.001, 0.0, 0.0) .build(); - controller.goal = new KineticState(0.0); + controller.setGoal(new KineticState(0.0)); } @Override public void loop() { if (gamepad1.aWasPressed()) { - controller.goal = new KineticState(0.0, 2000.0); + controller.setGoal(new KineticState(2000.0)); } else if (gamepad1.bWasPressed()) { - controller.goal = new KineticState(0.0, 0.0); + controller.setGoal(new KineticState(0.0)); } else if (gamepad1.xWasPressed()) { - controller.goal = new KineticState(0.0, 1000.0); + controller.setGoal(new KineticState(1000.0)); } flywheelMotor.setPower(controller.calculate( From 76b58a18f35d2608c24c5e1e7794c5e36ef1ade2 Mon Sep 17 00:00:00 2001 From: KavinSandking <150557351+KavinSandking@users.noreply.github.com> Date: Sun, 4 Jan 2026 12:07:39 -0600 Subject: [PATCH 4/7] fixed java parts of setting motor power in flywheels section --- src/control/examples/flywheels.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/control/examples/flywheels.md b/src/control/examples/flywheels.md index 8760c6d..613c47b 100644 --- a/src/control/examples/flywheels.md +++ b/src/control/examples/flywheels.md @@ -94,10 +94,10 @@ public class FlywheelExample extends OpMode { controller.setGoal(new KineticState(1000.0)); } - flywheelMotor.setPower(controller.calculate( - flywheelMotor.getCurrentPosition(), - flywheelMotor.getVelocity() - )); + flywheelMotor.setPower(controller.calculate(new KineticState( + flywheelMotor.getCurrentPosition(), + flywheelMotor.getVelocity())) + ); } } ``` From 18a3e58c7b0b0741f9f2b2baee006ba95d6b44fa Mon Sep 17 00:00:00 2001 From: KavinSandking <150557351+KavinSandking@users.noreply.github.com> Date: Sun, 4 Jan 2026 12:10:34 -0600 Subject: [PATCH 5/7] fixed java parts for setting power in slides --- src/control/examples/slides.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/control/examples/slides.md b/src/control/examples/slides.md index 0a008fb..46b0c1e 100644 --- a/src/control/examples/slides.md +++ b/src/control/examples/slides.md @@ -101,10 +101,10 @@ public class SlideExample extends OpMode { controller.setGoal(new KineticState(500.0)); } - slideMotor.setPower(controller.calculate( - slideMotor.getCurrentPosition(), - slideMotor.getVelocity() - )); + slideMotor.setPower(controller.calculate(new KineticState( + slideMotor.getCurrentPosition(), + slideMotor.getVelocity())) + ); } } ``` From bbfae770e1a877f30b5609eaabd244ef2e8c3511 Mon Sep 17 00:00:00 2001 From: KavinSandking <150557351+KavinSandking@users.noreply.github.com> Date: Sun, 4 Jan 2026 12:42:26 -0600 Subject: [PATCH 6/7] fix kotlin syntax for initialization and setting motor power in slides --- src/control/examples/slides.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/control/examples/slides.md b/src/control/examples/slides.md index 46b0c1e..154e9d7 100644 --- a/src/control/examples/slides.md +++ b/src/control/examples/slides.md @@ -45,7 +45,7 @@ We can easily do that by simply changing the `goal` of our `ControlSystem`: ```kotlin class SlideExample() : OpMode() { - val slideMotor by lazy { hardwareMap.get(DcMotorEx.class, "slides") } + val slideMotor by lazy { hardwareMap.get(DcMotorEx::class.java, "slides") } val controller = controlSystem { posPid(0.1, 0.0, 0.0) elevatorFF(0.04) @@ -64,10 +64,10 @@ class SlideExample() : OpMode() { controller.goal = KineticState(500.0) } - slideMotor.power = controller.calculate( - slideMotor.currentPosition, + slideMotor.power = controller.calculate(KineticState( + slideMotor.currentPosition.toDouble(), slideMotor.velocity - ) + )) } } ``` From 3d61522a352905d9cf42c1081af8e29f6994fcf8 Mon Sep 17 00:00:00 2001 From: KavinSandking <150557351+KavinSandking@users.noreply.github.com> Date: Sun, 4 Jan 2026 12:45:13 -0600 Subject: [PATCH 7/7] fix kotlin syntax for motor initialization and setting motor power --- src/control/examples/flywheels.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/control/examples/flywheels.md b/src/control/examples/flywheels.md index 613c47b..040759f 100644 --- a/src/control/examples/flywheels.md +++ b/src/control/examples/flywheels.md @@ -40,7 +40,7 @@ We can easily do that by simply changing the `goal` of our `ControlSystem`: ```kotlin class FlywheelExample() : OpMode() { - val flywheelMotor by lazy { hardwareMap.get(DcMotorEx.class, "flywheel") } + val flywheelMotor by lazy { hardwareMap.get(DcMotorEx::class.java, "flywheel") } val controller = controlSystem { velPid(0.001, 0.0, 0.0) } @@ -58,10 +58,10 @@ class FlywheelExample() : OpMode() { controller.goal = KineticState(0.0, 1000.0) } - flywheelMotor.power = controller.calculate( - flywheelMotor.currentPosition, - flywheelMotor.velocity - ) + flywheelMotor.power = controller.calculate(KineticState( + flywheelMotor.currentPosition.toDouble(), + flywheelMotor.velocity + )) } } ```