From d70f00b2d944a665750095e70fda006df3ee571f Mon Sep 17 00:00:00 2001 From: blazeboy75 <> Date: Wed, 24 Nov 2021 10:37:12 -0700 Subject: [PATCH 1/4] Added Basic Strafing --- .../ftc/teamcode/StarterTeleOp.java | 58 ++++++++++++++----- 1 file changed, 43 insertions(+), 15 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StarterTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StarterTeleOp.java index 6622d6d38bcb..67c9487ac54b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StarterTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StarterTeleOp.java @@ -32,14 +32,15 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorController; import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; -//import com.qualcomm.robotcore.eventloop.opmode.Disabled; +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.DcMotor; -//import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; @@ -80,6 +81,14 @@ public void init() { backL = hardwareMap.get(DcMotor.class, "Back Left"); backR = hardwareMap.get(DcMotor.class, "Back Right"); + /** Sets the motors to run using encoders. */ + frontL.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frontR.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backL.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backR.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + + /** Most robots need the motor on one side to be reversed to drive forward. * Reverse the motor that runs backwards when connected directly to the battery. */ frontL.setDirection(DcMotor.Direction.FORWARD); @@ -106,28 +115,47 @@ public void start() { @Override public void loop() { /** Setup a variable for each drive wheel to save power level for telemetry. */ - double leftPower; - double rightPower; + double leftFPower ; + double rightFPower; + double leftBPower ; + double rightBPower; /**POV Mode uses right stick to go forward, and left stick to turn * will be combined in later release. * This uses basic math to combine motions and is easier to drive straight. */ - double drive = -gamepad1.left_stick_x; - double turn = gamepad1.right_stick_y; - leftPower = Range.clip(drive + turn, -1.0, 1.0) ; - rightPower = Range.clip(drive - turn, -1.0, 1.0) ; - - + double drive = -gamepad1.right_stick_y; + double turn = gamepad1.right_stick_x; + double strafe = gamepad1.left_stick_x; + + if (strafe !=0){ + /** Untested on the robot. May need to be reversed or changed entirely. */ + leftFPower = -strafe; + rightFPower = strafe; + leftBPower = strafe; + rightBPower = -strafe; + } + else if(drive !=0 || turn !=0) { + leftFPower = Range.clip(drive + turn, -1.0, 1.0); + rightFPower = Range.clip(drive - turn, -1.0, 1.0); + leftBPower = Range.clip(drive + turn, -1.0, 1.0); + rightBPower = Range.clip(drive - turn, -1.0, 1.0); + } + else{ + leftFPower = 0; + rightFPower = 0; + leftBPower = 0; + rightBPower = 0; + } /**Send calculated power to wheels. */ - frontL.setPower(leftPower); - backL.setPower(leftPower); - frontR.setPower(rightPower); - backR.setPower(rightPower); + frontL.setPower(leftFPower); + backL.setPower(leftBPower); + frontR.setPower(rightFPower); + backR.setPower(rightBPower); /** Show the elapsed game time and wheel power. */ telemetry.addData("Status", "Run Time: " + runtime.toString()); - telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower); + telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftFPower, rightFPower); } /** Code to run ONCE after the driver hits STOP. */ From 2b8920a0bd07b32635973113fb93724c364ca792 Mon Sep 17 00:00:00 2001 From: blazeboy75 <> Date: Thu, 25 Nov 2021 19:32:00 -0700 Subject: [PATCH 2/4] Motor to run with encoders --- .../ftc/teamcode/StarterTeleOp.java | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StarterTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StarterTeleOp.java index 67c9487ac54b..30db15d6ba9a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StarterTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StarterTeleOp.java @@ -32,17 +32,17 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorController; +//import com.qualcomm.robotcore.hardware.DcMotorController; import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; -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.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.util.ElapsedTime; -import com.qualcomm.robotcore.util.Range; +//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.DcMotor; +//import com.qualcomm.robotcore.hardware.DcMotorSimple; +//import com.qualcomm.robotcore.util.ElapsedTime; +//import com.qualcomm.robotcore.util.Range; /** This file contains an example of an iterative (Non-Linear) "OpMode". * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. @@ -56,8 +56,8 @@ * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name. * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list. */ -@TeleOp(name="Basic: Iterative OpMode", group="Iterative Opmode") -// +@TeleOp(name="Starter TeleOp", group="Iterative Opmode") + // @Disabled public class StarterTeleOp extends OpMode { From 6fc583ad4c9729a6dac37fca08b71b15c06c219d Mon Sep 17 00:00:00 2001 From: blazeboy75 <> Date: Thu, 25 Nov 2021 19:38:17 -0700 Subject: [PATCH 3/4] Milo is very picky --- .../ftc/teamcode/StarterTeleOp.java | 30 ++++++++++++++----- 1 file changed, 22 insertions(+), 8 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StarterTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StarterTeleOp.java index 30db15d6ba9a..042720542962 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StarterTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StarterTeleOp.java @@ -32,17 +32,10 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; -//import com.qualcomm.robotcore.hardware.DcMotorController; import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; -//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.DcMotor; -//import com.qualcomm.robotcore.hardware.DcMotorSimple; -//import com.qualcomm.robotcore.util.ElapsedTime; -//import com.qualcomm.robotcore.util.Range; + /** This file contains an example of an iterative (Non-Linear) "OpMode". * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. @@ -56,8 +49,10 @@ * Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name. * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list. */ + @TeleOp(name="Starter TeleOp", group="Iterative Opmode") + // @Disabled public class StarterTeleOp extends OpMode { @@ -68,6 +63,8 @@ public class StarterTeleOp extends OpMode private DcMotor backL = null; private DcMotor backR = null; + + /** Code to run ONCE when the driver hits INIT. */ @Override public void init() { @@ -100,17 +97,24 @@ public void init() { telemetry.addData("Status", "Initialized"); } + + /** Code to run REPEATEDLY after the driver hits INIT, but before they hit PLAY. */ @Override public void init_loop() { } + + + /** Code to run ONCE when the driver hits PLAY. */ @Override public void start() { runtime.reset(); } + + /** Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP. */ @Override public void loop() { @@ -120,6 +124,8 @@ public void loop() { double leftBPower ; double rightBPower; + + /**POV Mode uses right stick to go forward, and left stick to turn * will be combined in later release. * This uses basic math to combine motions and is easier to drive straight. */ @@ -127,6 +133,8 @@ public void loop() { double turn = gamepad1.right_stick_x; double strafe = gamepad1.left_stick_x; + + if (strafe !=0){ /** Untested on the robot. May need to be reversed or changed entirely. */ leftFPower = -strafe; @@ -134,12 +142,16 @@ public void loop() { leftBPower = strafe; rightBPower = -strafe; } + + else if(drive !=0 || turn !=0) { leftFPower = Range.clip(drive + turn, -1.0, 1.0); rightFPower = Range.clip(drive - turn, -1.0, 1.0); leftBPower = Range.clip(drive + turn, -1.0, 1.0); rightBPower = Range.clip(drive - turn, -1.0, 1.0); } + + else{ leftFPower = 0; rightFPower = 0; @@ -147,12 +159,14 @@ else if(drive !=0 || turn !=0) { rightBPower = 0; } + /**Send calculated power to wheels. */ frontL.setPower(leftFPower); backL.setPower(leftBPower); frontR.setPower(rightFPower); backR.setPower(rightBPower); + /** Show the elapsed game time and wheel power. */ telemetry.addData("Status", "Run Time: " + runtime.toString()); telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftFPower, rightFPower); From 520b0a67122e6bb5e1684644ddc30eaadc517454 Mon Sep 17 00:00:00 2001 From: blazeboy75 <> Date: Thu, 25 Nov 2021 19:43:44 -0700 Subject: [PATCH 4/4] Did I mention youre picky? --- .../firstinspires/ftc/teamcode/StarterTeleOp.java | 15 +-------------- 1 file changed, 1 insertion(+), 14 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StarterTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StarterTeleOp.java index 042720542962..c6434ad48eea 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StarterTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StarterTeleOp.java @@ -28,7 +28,6 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; @@ -36,7 +35,6 @@ import com.qualcomm.robotcore.util.Range; - /** This file contains an example of an iterative (Non-Linear) "OpMode". * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. * The names of OpModes appear on the menu of the FTC Driver Station. @@ -64,7 +62,6 @@ public class StarterTeleOp extends OpMode private DcMotor backR = null; - /** Code to run ONCE when the driver hits INIT. */ @Override public void init() { @@ -78,6 +75,7 @@ public void init() { backL = hardwareMap.get(DcMotor.class, "Back Left"); backR = hardwareMap.get(DcMotor.class, "Back Right"); + /** Sets the motors to run using encoders. */ frontL.setMode(DcMotor.RunMode.RUN_USING_ENCODER); frontR.setMode(DcMotor.RunMode.RUN_USING_ENCODER); @@ -85,7 +83,6 @@ public void init() { backR.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - /** Most robots need the motor on one side to be reversed to drive forward. * Reverse the motor that runs backwards when connected directly to the battery. */ frontL.setDirection(DcMotor.Direction.FORWARD); @@ -98,15 +95,12 @@ public void init() { } - /** Code to run REPEATEDLY after the driver hits INIT, but before they hit PLAY. */ @Override public void init_loop() { } - - /** Code to run ONCE when the driver hits PLAY. */ @Override public void start() { @@ -114,7 +108,6 @@ public void start() { } - /** Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP. */ @Override public void loop() { @@ -125,7 +118,6 @@ public void loop() { double rightBPower; - /**POV Mode uses right stick to go forward, and left stick to turn * will be combined in later release. * This uses basic math to combine motions and is easier to drive straight. */ @@ -134,7 +126,6 @@ public void loop() { double strafe = gamepad1.left_stick_x; - if (strafe !=0){ /** Untested on the robot. May need to be reversed or changed entirely. */ leftFPower = -strafe; @@ -143,7 +134,6 @@ public void loop() { rightBPower = -strafe; } - else if(drive !=0 || turn !=0) { leftFPower = Range.clip(drive + turn, -1.0, 1.0); rightFPower = Range.clip(drive - turn, -1.0, 1.0); @@ -151,7 +141,6 @@ else if(drive !=0 || turn !=0) { rightBPower = Range.clip(drive - turn, -1.0, 1.0); } - else{ leftFPower = 0; rightFPower = 0; @@ -159,14 +148,12 @@ else if(drive !=0 || turn !=0) { rightBPower = 0; } - /**Send calculated power to wheels. */ frontL.setPower(leftFPower); backL.setPower(leftBPower); frontR.setPower(rightFPower); backR.setPower(rightBPower); - /** Show the elapsed game time and wheel power. */ telemetry.addData("Status", "Run Time: " + runtime.toString()); telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftFPower, rightFPower);