From 6eae3fa3052ff1b22a3f731cb89977cc369bd0c1 Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Thu, 26 Oct 2023 20:29:13 -0500 Subject: [PATCH 1/5] Begin useful stuff, need to work on game strategy --- .../Engines/ProtoBotEngineSodi.java | 4 ++-- .../Autonomous/States/ProtoBotStateSodi.java | 24 +++++++++---------- 2 files changed, 13 insertions(+), 15 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java index 91dd568..c147779 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java @@ -7,12 +7,12 @@ import org.timecrafters.CenterStage.Autonomous.States.ProtoBotStateSodi; import dev.cyberarm.engine.V2.CyberarmEngine; -@Autonomous(name = "Autonomous- Sodi", group = "Simple Test") +@Autonomous(name = "Autonomous- Sodi", group = "Arm Sequence") public class ProtoBotEngineSodi extends CyberarmEngine { private ProtoBotSodi robot; @Override public void setup() { - this.robot = new ProtoBotSodi("Autonomous - Sodi"); + this.robot = new ProtoBotSodi("Autonomous- Sodi"); this.robot.setup(); addState(new ProtoBotStateSodi(robot)); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java index b66ec61..ed2851a 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java @@ -10,15 +10,16 @@ import org.timecrafters.CenterStage.Common.ProtoBotSodi; public class ProtoBotStateSodi extends CyberarmState { ProtoBotSodi robot; - private double avgVelocity, avgDrivePower; private long lastTimeChecked; private int testSequence; + private int targetPos; + private int currentPos; + private int totalDist; // + public ProtoBotStateSodi(ProtoBotSodi robot) { this.robot = robot; } public void telemetry() { -// engine.telemetry.addData("Avg Drive Velocity", avgVelocity); -// engine.telemetry.addData("Avg Drive Power", avgDrivePower); engine.telemetry.addData("Front Left Velocity", robot.flDrive.getVelocity()); engine.telemetry.addData("Front Right Velocity", robot.frDrive.getVelocity()); engine.telemetry.addData("Back Left Velocity", robot.blDrive.getVelocity()); @@ -30,16 +31,6 @@ public class ProtoBotStateSodi extends CyberarmState { } -// public double getAvgDrivePower() { -// avgDrivePower = (robot.flDrive.motor.getPower() + robot.frDrive.motor.getPower() + robot.blDrive.motor.getPower() + robot.brDrive.motor.getPower())/4; -// return avgDrivePower; -// } - -// public double getAvgVelocity() { -// avgVelocity = (robot.flDrive.getVelocity() + robot.frDrive.getVelocity() + robot.blDrive.getVelocity() + robot.brDrive.getVelocity())/4; -// return avgVelocity; -// } - @Override public void init() { robot.flDrive.motor.setPower(0); @@ -59,10 +50,14 @@ public class ProtoBotStateSodi extends CyberarmState { testSequence = 0; + + } @Override public void exec() { + + currentPos = robot.liftMotor.motor.getCurrentPosition(); // // if (System.currentTimeMillis() - lastTimeChecked >= 500 && System.currentTimeMillis() - lastTimeChecked < 2500) { // robot.flDrive.motor.setPower(0.5); @@ -99,11 +94,14 @@ public class ProtoBotStateSodi extends CyberarmState { switch (testSequence) { case 1: + robot.liftMotor.motor.setPower(0.4); + robot.liftMotor.motor.setTargetPosition(targetPos); //lift motor go up for some way //wait for about 0.25 case 2: + robot.liftMotor.motor.setTargetPosition(targetPos); //lift motor go down //repeat From 74093473b846c3cf635fc50970ade4f70d35c52e Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Sat, 28 Oct 2023 12:03:12 -0500 Subject: [PATCH 2/5] New drive test config, need to work on game strategy --- .../Autonomous/States/ArmStateSodi.java | 54 ++++++++--- .../Autonomous/States/ProtoBotStateSodi.java | 91 ++++++++----------- .../CenterStage/Common/ProtoBotSodi.java | 48 +++++----- 3 files changed, 106 insertions(+), 87 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ArmStateSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ArmStateSodi.java index 36303a4..bea0a49 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ArmStateSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ArmStateSodi.java @@ -1,5 +1,9 @@ package org.timecrafters.CenterStage.Autonomous.States; +/**BEGAN WITH 43 LINES**/ + +import com.arcrobotics.ftclib.hardware.motors.Motor; + import org.timecrafters.CenterStage.Common.ProtoBotSodi; import org.timecrafters.CenterStage.Common.PrototypeRobot; @@ -10,12 +14,24 @@ public class ArmStateSodi extends CyberarmState { private final boolean stateDisabled; ProtoBotSodi robot; private int testSequence; + private int targetPos; + private int currentPos; + private int totalDist; + public ArmStateSodi(ProtoBotSodi robot, String groupName, String actionName) { this.robot = robot; this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; } + private int getTotalDist() { + int totalDist = targetPos - currentPos; + return totalDist; + } + + + + @Override public void start() { //add variables that need to be reinitialized @@ -23,18 +39,34 @@ public class ArmStateSodi extends CyberarmState { } @Override - public void exec() { - - if (robot.liftMotor.motor.getCurrentPosition() < 0) { - robot.liftMotor.motor.setPower(0); - } - if (robot.liftMotor.motor.getCurrentPosition() >= 0 && robot.liftMotor.motor.getCurrentPosition() <= 49) { - - } - if (robot.liftMotor.motor.getCurrentPosition() >= 50 && robot.liftMotor.motor.getCurrentPosition() <= 250) { - - } + public void init() { + } + + @Override + public void exec() { + +// if (robot.liftMotor.motor.getCurrentPosition() >= 2800 && +// Math.abs(robot.liftMotor.motor.getPower()) != robot.liftMotor.motor.getPower()) { +// robot.liftMotor.motor.setPower(0); +// } +// +// if (robot.liftMotor.motor.getCurrentPosition() < 0 && +// Math.abs(robot.liftMotor.motor.getPower()) != robot.liftMotor.motor.getPower()) { +// robot.liftMotor.motor.setPower(0); +// } +// +// if (robot.liftMotor.motor.getCurrentPosition() >= 0 && +// robot.liftMotor.motor.getCurrentPosition() <= 50) { +// +// } +// +// if (robot.liftMotor.motor.getCurrentPosition() >= 50 && +// robot.liftMotor.motor.getCurrentPosition() <= 250) { +// +// } +// +// } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java index ed2851a..182ff88 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java @@ -14,7 +14,7 @@ public class ProtoBotStateSodi extends CyberarmState { private int testSequence; private int targetPos; private int currentPos; - private int totalDist; // + private int totalDist; public ProtoBotStateSodi(ProtoBotSodi robot) { this.robot = robot; @@ -37,14 +37,14 @@ public class ProtoBotStateSodi extends CyberarmState { robot.frDrive.motor.setPower(0); robot.blDrive.motor.setPower(0); robot.brDrive.motor.setPower(0); - robot.liftMotor.motor.setPower(0); - - robot.grabJaw.setPosition(0); - robot.grabElbow.setPosition(0); - robot.grabShoulder.setPosition(0); - robot.dropShoulder.setPosition(0); - robot.dropElbow.setPosition(0); - robot.dropJaw.setPosition(0); +// robot.liftMotor.motor.setPower(0); +// +// robot.grabJaw.setPosition(0); +// robot.grabElbow.setPosition(0); +// robot.grabShoulder.setPosition(0); +// robot.dropShoulder.setPosition(0); +// robot.dropElbow.setPosition(0); +// robot.dropJaw.setPosition(0); lastTimeChecked = System.currentTimeMillis(); testSequence = 0; @@ -57,55 +57,42 @@ public class ProtoBotStateSodi extends CyberarmState { @Override public void exec() { - currentPos = robot.liftMotor.motor.getCurrentPosition(); -// -// if (System.currentTimeMillis() - lastTimeChecked >= 500 && System.currentTimeMillis() - lastTimeChecked < 2500) { -// robot.flDrive.motor.setPower(0.5); -// robot.frDrive.motor.setPower(0.5); -// robot.blDrive.motor.setPower(0.5); -// robot.brDrive.motor.setPower(0.5); -// robot.liftMotor.motor.setPower(0.5); -// } else if (System.currentTimeMillis() - lastTimeChecked >= 2500 && System.currentTimeMillis() - lastTimeChecked < 4500) { -// robot.flDrive.motor.setPower(-0.5); -// robot.frDrive.motor.setPower(-0.5); -// robot.blDrive.motor.setPower(-0.5); -// robot.brDrive.motor.setPower(-0.5); -// robot.liftMotor.motor.setPower(-0.5); -// } else if (System.currentTimeMillis() - lastTimeChecked >= 4500 && System.currentTimeMillis() - lastTimeChecked < 6500) { -// robot.flDrive.motor.setPower(0.5); -// robot.frDrive.motor.setPower(0.5); -// robot.blDrive.motor.setPower(-0.5); -// robot.brDrive.motor.setPower(-0.5); -// robot.liftMotor.motor.setPower(0); -// } else if (System.currentTimeMillis() - lastTimeChecked >= 6500 && System.currentTimeMillis() - lastTimeChecked < 8500) { -// robot.flDrive.motor.setPower(-0.5); -// robot.frDrive.motor.setPower(-0.5); -// robot.blDrive.motor.setPower(0.5); -// robot.brDrive.motor.setPower(0.5); -// robot.liftMotor.motor.setPower(0); -// } else if (System.currentTimeMillis() - lastTimeChecked >= 8600){ -// robot.flDrive.motor.setPower(0); -// robot.frDrive.motor.setPower(0); -// robot.blDrive.motor.setPower(0); -// robot.brDrive.motor.setPower(0); -// robot.liftMotor.motor.setPower(0); -// setHasFinished(true); -// } + currentPos = robot.flDrive.motor.getCurrentPosition(); + + if (testSequence < 1) { + testSequence = 1; + } switch (testSequence) { case 1: - robot.liftMotor.motor.setPower(0.4); - robot.liftMotor.motor.setTargetPosition(targetPos); - //lift motor go up for some way - //wait for about 0.25 + robot.flDrive.motor.setTargetPosition(500); + robot.frDrive.motor.setTargetPosition(500); + robot.blDrive.motor.setTargetPosition(500); + robot.brDrive.motor.setTargetPosition(500); + + if (robot.flDrive.motor.getCurrentPosition() < robot.flDrive.motor.getTargetPosition() - 50) { + robot.flDrive.motor.setPower(0.5 * (robot.flDrive.motor.getTargetPosition() - robot.flDrive.motor.getCurrentPosition())); + robot.frDrive.motor.setPower(0.5 * (robot.frDrive.motor.getTargetPosition() - robot.frDrive.motor.getCurrentPosition())); + robot.blDrive.motor.setPower(0.5 * (robot.blDrive.motor.getTargetPosition() - robot.blDrive.motor.getCurrentPosition())); + robot.brDrive.motor.setPower(0.5 * (robot.brDrive.motor.getTargetPosition() - robot.brDrive.motor.getCurrentPosition())); + } + else if (robot.flDrive.motor.getCurrentPosition() < robot.flDrive.motor.getTargetPosition() + 50 || + robot.flDrive.motor.getCurrentPosition() > robot.flDrive.motor.getTargetPosition() - 50) { + + robot.flDrive.motor.setPower(0); + robot.frDrive.motor.setPower(0); + robot.blDrive.motor.setPower(0); + robot.brDrive.motor.setPower(0); + +// testSequence = 2; + } + + break; +// case 2: + - case 2: - robot.liftMotor.motor.setTargetPosition(targetPos); - //lift motor go down - //repeat - //wait for about 0.25 } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java index 793834d..8630b1d 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java @@ -25,8 +25,8 @@ import dev.cyberarm.engine.V2.CyberarmEngine; public class ProtoBotSodi extends Robot { public HardwareMap hardwareMap; - public MotorEx flDrive, frDrive, blDrive, brDrive, liftMotor; - public Servo grabJaw, grabElbow, grabShoulder, dropShoulder, dropElbow, dropJaw; + public MotorEx flDrive, frDrive, blDrive, brDrive /*liftMotor*/; +// public Servo grabJaw, grabElbow, grabShoulder, dropShoulder, dropElbow, dropJaw; private String string; private CyberarmEngine engine; @@ -43,40 +43,40 @@ public class ProtoBotSodi extends Robot { this.hardwareMap = CyberarmEngine.instance.hardwareMap; this.engine = CyberarmEngine.instance; - TimeCraftersConfiguration configuration = new TimeCraftersConfiguration("Robbie"); +// TimeCraftersConfiguration configuration = new TimeCraftersConfiguration(); //Motors frDrive = new MotorEx(hardwareMap, "FrontRight"); flDrive = new MotorEx(hardwareMap, "FrontLeft"); brDrive = new MotorEx(hardwareMap, "BackRight"); blDrive = new MotorEx(hardwareMap, "BackLeft"); - liftMotor = new MotorEx(hardwareMap, "Lift"); +// liftMotor = new MotorEx(hardwareMap, "Lift"); - flDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - frDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - blDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - brDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - liftMotor.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// flDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// frDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// blDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// brDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// liftMotor.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); flDrive.motor.setDirection(FORWARD); - frDrive.motor.setDirection(REVERSE); +// frDrive.motor.setDirection(REVERSE); blDrive.motor.setDirection(FORWARD); - brDrive.motor.setDirection(REVERSE); +// brDrive.motor.setDirection(REVERSE); //Servos - grabJaw = hardwareMap.servo.get("GrabJaw"); - grabElbow = hardwareMap.servo.get("GrabElbow"); - grabShoulder = hardwareMap.servo.get("GrabShoulder"); - dropShoulder = hardwareMap.servo.get("DropShoulder"); - dropElbow = hardwareMap.servo.get("DropElbow"); - dropJaw = hardwareMap.servo.get("DropJaw"); - - grabElbow.setDirection(Servo.Direction.FORWARD); - grabJaw.setDirection(Servo.Direction.FORWARD); - grabShoulder.setDirection(Servo.Direction.FORWARD); - dropShoulder.setDirection(Servo.Direction.FORWARD); - dropElbow.setDirection(Servo.Direction.FORWARD); - dropJaw.setDirection(Servo.Direction.FORWARD); +// grabJaw = hardwareMap.servo.get("GrabJaw"); +// grabElbow = hardwareMap.servo.get("GrabElbow"); +// grabShoulder = hardwareMap.servo.get("GrabShoulder"); +// dropShoulder = hardwareMap.servo.get("DropShoulder"); +// dropElbow = hardwareMap.servo.get("DropElbow"); +// dropJaw = hardwareMap.servo.get("DropJaw"); +// +// grabElbow.setDirection(Servo.Direction.FORWARD); +// grabJaw.setDirection(Servo.Direction.FORWARD); +// grabShoulder.setDirection(Servo.Direction.FORWARD); +// dropShoulder.setDirection(Servo.Direction.FORWARD); +// dropElbow.setDirection(Servo.Direction.FORWARD); +// dropJaw.setDirection(Servo.Direction.FORWARD); } From d3b6ac873e8b8208619577567dfbe78bff71281a Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Mon, 30 Oct 2023 18:21:58 -0500 Subject: [PATCH 3/5] Minibot drive test:Added stop and reset encoder on all motors, need to work on game strategy --- .../Autonomous/States/ProtoBotStateSodi.java | 57 +++++++++---------- .../CenterStage/Common/ProtoBotSodi.java | 15 +++-- 2 files changed, 38 insertions(+), 34 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java index 182ff88..5384597 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java @@ -1,5 +1,6 @@ package org.timecrafters.CenterStage.Autonomous.States; +import com.arcrobotics.ftclib.hardware.motors.Motor; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; @@ -28,15 +29,23 @@ public class ProtoBotStateSodi extends CyberarmState { engine.telemetry.addData("Front Right Power", robot.frDrive.motor.getPower()); engine.telemetry.addData("Back Left Power", robot.blDrive.motor.getPower()); engine.telemetry.addData("Back Right Power", robot.brDrive.motor.getPower()); + engine.telemetry.addData("FL Position",robot.flDrive.motor.getCurrentPosition()); + engine.telemetry.addData("Test Sequence", testSequence); } + @Override + public void start() { + testSequence = 0; + } + @Override public void init() { robot.flDrive.motor.setPower(0); robot.frDrive.motor.setPower(0); robot.blDrive.motor.setPower(0); robot.brDrive.motor.setPower(0); + // robot.liftMotor.motor.setPower(0); // // robot.grabJaw.setPosition(0); @@ -47,7 +56,7 @@ public class ProtoBotStateSodi extends CyberarmState { // robot.dropJaw.setPosition(0); lastTimeChecked = System.currentTimeMillis(); - testSequence = 0; + @@ -63,37 +72,27 @@ public class ProtoBotStateSodi extends CyberarmState { testSequence = 1; } - switch (testSequence) { - case 1: - - robot.flDrive.motor.setTargetPosition(500); - robot.frDrive.motor.setTargetPosition(500); - robot.blDrive.motor.setTargetPosition(500); - robot.brDrive.motor.setTargetPosition(500); - - if (robot.flDrive.motor.getCurrentPosition() < robot.flDrive.motor.getTargetPosition() - 50) { - robot.flDrive.motor.setPower(0.5 * (robot.flDrive.motor.getTargetPosition() - robot.flDrive.motor.getCurrentPosition())); - robot.frDrive.motor.setPower(0.5 * (robot.frDrive.motor.getTargetPosition() - robot.frDrive.motor.getCurrentPosition())); - robot.blDrive.motor.setPower(0.5 * (robot.blDrive.motor.getTargetPosition() - robot.blDrive.motor.getCurrentPosition())); - robot.brDrive.motor.setPower(0.5 * (robot.brDrive.motor.getTargetPosition() - robot.brDrive.motor.getCurrentPosition())); - } - else if (robot.flDrive.motor.getCurrentPosition() < robot.flDrive.motor.getTargetPosition() + 50 || - robot.flDrive.motor.getCurrentPosition() > robot.flDrive.motor.getTargetPosition() - 50) { - - robot.flDrive.motor.setPower(0); - robot.frDrive.motor.setPower(0); - robot.blDrive.motor.setPower(0); - robot.brDrive.motor.setPower(0); - -// testSequence = 2; - } - - break; -// case 2: - + if (testSequence == 1) { + robot.flDrive.motor.setTargetPosition(1000); + robot.frDrive.motor.setTargetPosition(1000); + robot.blDrive.motor.setTargetPosition(1000); + robot.brDrive.motor.setTargetPosition(1000); + if (robot.flDrive.motor.getCurrentPosition() < robot.flDrive.motor.getTargetPosition() - 50) { + robot.flDrive.motor.setPower(0.5 * (robot.flDrive.motor.getTargetPosition() - robot.flDrive.motor.getCurrentPosition())); + robot.frDrive.motor.setPower(0.5 * (robot.frDrive.motor.getTargetPosition() - robot.frDrive.motor.getCurrentPosition())); + robot.blDrive.motor.setPower(0.5 * (robot.blDrive.motor.getTargetPosition() - robot.blDrive.motor.getCurrentPosition())); + robot.brDrive.motor.setPower(0.5 * (robot.brDrive.motor.getTargetPosition() - robot.brDrive.motor.getCurrentPosition())); + } else if (robot.flDrive.motor.getCurrentPosition() < robot.flDrive.motor.getTargetPosition() + 50 || + robot.flDrive.motor.getCurrentPosition() > robot.flDrive.motor.getTargetPosition() - 50) { + robot.flDrive.motor.setPower(0); + robot.frDrive.motor.setPower(0); + robot.blDrive.motor.setPower(0); + robot.brDrive.motor.setPower(0); + testSequence = 2; + } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java index 8630b1d..ee6b8a0 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java @@ -52,11 +52,16 @@ public class ProtoBotSodi extends Robot { blDrive = new MotorEx(hardwareMap, "BackLeft"); // liftMotor = new MotorEx(hardwareMap, "Lift"); -// flDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); -// frDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); -// blDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); -// brDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); -// liftMotor.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + flDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + frDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + blDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + brDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + flDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + blDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + brDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); +// liftMotor.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); flDrive.motor.setDirection(FORWARD); // frDrive.motor.setDirection(REVERSE); From 22c4c5149e01da28c4cfa790d2010e742ce66df8 Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Tue, 31 Oct 2023 16:12:43 -0500 Subject: [PATCH 4/5] Minibot teleop basic forward drive --- .../CenterStage/Common/MinibotTeleOPBot.java | 58 +++++++++++++ .../CenterStage/Common/ProtoBotSodi.java | 35 ++++---- .../TeleOp/Engines/MiniBotTeleOPEngine.java | 21 +++++ .../TeleOp/States/YellowMinibotTeleOP.java | 86 +++++++++++++++++++ 4 files changed, 183 insertions(+), 17 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MinibotTeleOPBot.java create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBotTeleOPEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MinibotTeleOPBot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MinibotTeleOPBot.java new file mode 100644 index 0000000..2fcd5f5 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MinibotTeleOPBot.java @@ -0,0 +1,58 @@ +package org.timecrafters.CenterStage.Common; + +import com.arcrobotics.ftclib.hardware.motors.MotorEx; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; + +import org.timecrafters.Library.Robot; +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +public class MinibotTeleOPBot extends Robot { + + public HardwareMap hardwareMap; + public MotorEx flDrive, frDrive, blDrive, brDrive; + public IMU imu; + private String string; + private CyberarmEngine engine; + + public TimeCraftersConfiguration configuration; + + public MinibotTeleOPBot() { + } + + @Override + public void setup() { + this.hardwareMap = CyberarmEngine.instance.hardwareMap; + this.engine = CyberarmEngine.instance; + + imu = engine.hardwareMap.get(IMU.class, "imu"); + + IMU.Parameters parameters = new IMU.Parameters( + new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.LEFT, + RevHubOrientationOnRobot.UsbFacingDirection.UP)); + + imu.initialize(parameters); + + //Motors + frDrive = new MotorEx(hardwareMap, "FrontRight"); + flDrive = new MotorEx(hardwareMap, "FrontLeft"); + brDrive = new MotorEx(hardwareMap, "BackRight"); + blDrive = new MotorEx(hardwareMap, "BackLeft"); + + flDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + frDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + blDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + brDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + flDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + blDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + brDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java index ee6b8a0..1591788 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java @@ -25,8 +25,8 @@ import dev.cyberarm.engine.V2.CyberarmEngine; public class ProtoBotSodi extends Robot { public HardwareMap hardwareMap; - public MotorEx flDrive, frDrive, blDrive, brDrive /*liftMotor*/; -// public Servo grabJaw, grabElbow, grabShoulder, dropShoulder, dropElbow, dropJaw; + public MotorEx flDrive, frDrive, blDrive, brDrive, liftMotor; + public Servo grabJaw, grabElbow, grabShoulder, dropShoulder, dropElbow, dropJaw; private String string; private CyberarmEngine engine; @@ -50,18 +50,19 @@ public class ProtoBotSodi extends Robot { flDrive = new MotorEx(hardwareMap, "FrontLeft"); brDrive = new MotorEx(hardwareMap, "BackRight"); blDrive = new MotorEx(hardwareMap, "BackLeft"); -// liftMotor = new MotorEx(hardwareMap, "Lift"); + liftMotor = new MotorEx(hardwareMap, "Lift"); flDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); blDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); brDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + liftMotor.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); flDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); frDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); blDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); brDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); -// liftMotor.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + liftMotor.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); flDrive.motor.setDirection(FORWARD); // frDrive.motor.setDirection(REVERSE); @@ -69,19 +70,19 @@ public class ProtoBotSodi extends Robot { // brDrive.motor.setDirection(REVERSE); //Servos -// grabJaw = hardwareMap.servo.get("GrabJaw"); -// grabElbow = hardwareMap.servo.get("GrabElbow"); -// grabShoulder = hardwareMap.servo.get("GrabShoulder"); -// dropShoulder = hardwareMap.servo.get("DropShoulder"); -// dropElbow = hardwareMap.servo.get("DropElbow"); -// dropJaw = hardwareMap.servo.get("DropJaw"); -// -// grabElbow.setDirection(Servo.Direction.FORWARD); -// grabJaw.setDirection(Servo.Direction.FORWARD); -// grabShoulder.setDirection(Servo.Direction.FORWARD); -// dropShoulder.setDirection(Servo.Direction.FORWARD); -// dropElbow.setDirection(Servo.Direction.FORWARD); -// dropJaw.setDirection(Servo.Direction.FORWARD); + grabJaw = hardwareMap.servo.get("GrabJaw"); + grabElbow = hardwareMap.servo.get("GrabElbow"); + grabShoulder = hardwareMap.servo.get("GrabShoulder"); + dropShoulder = hardwareMap.servo.get("DropShoulder"); + dropElbow = hardwareMap.servo.get("DropElbow"); + dropJaw = hardwareMap.servo.get("DropJaw"); + + grabElbow.setDirection(Servo.Direction.FORWARD); + grabJaw.setDirection(Servo.Direction.FORWARD); + grabShoulder.setDirection(Servo.Direction.FORWARD); + dropShoulder.setDirection(Servo.Direction.FORWARD); + dropElbow.setDirection(Servo.Direction.FORWARD); + dropJaw.setDirection(Servo.Direction.FORWARD); } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBotTeleOPEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBotTeleOPEngine.java new file mode 100644 index 0000000..c17c93f --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBotTeleOPEngine.java @@ -0,0 +1,21 @@ +package org.timecrafters.CenterStage.TeleOp.Engines; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.timecrafters.CenterStage.Common.MinibotTeleOPBot; +import org.timecrafters.CenterStage.TeleOp.States.YellowMinibotTeleOP; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@TeleOp(name = "A Yellow Minibot") + + public class MiniBotTeleOPEngine extends CyberarmEngine { + private MinibotTeleOPBot robot; + @Override + public void setup() { + this.robot = new MinibotTeleOPBot(); + this.robot.setup(); + + addState(new YellowMinibotTeleOP(robot)); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java new file mode 100644 index 0000000..9144105 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java @@ -0,0 +1,86 @@ +package org.timecrafters.CenterStage.TeleOp.States; + +import dev.cyberarm.engine.V2.CyberarmState; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.Servo; + +import dev.cyberarm.engine.V2.GamepadChecker; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.timecrafters.CenterStage.Common.MinibotTeleOPBot; + +public class YellowMinibotTeleOP extends CyberarmState { + private final MinibotTeleOPBot robot; + public float angleDelta, drivePower; + YawPitchRollAngles imuInitAngle; + private GamepadChecker gamepad1Checker, gamepad2Checker; + + public YellowMinibotTeleOP(MinibotTeleOPBot robot) { + this.robot = robot; + } + + + public float getAngleDelta() { + robot.imu.getRobotYawPitchRollAngles(); + + return angleDelta; + } + + @Override + public void telemetry() { + engine.telemetry.addData("Front Left Velocity", robot.flDrive.getVelocity()); + engine.telemetry.addData("Front Right Velocity", robot.frDrive.getVelocity()); + engine.telemetry.addData("Back Left Velocity", robot.blDrive.getVelocity()); + engine.telemetry.addData("Back Right Velocity", robot.brDrive.getVelocity()); + engine.telemetry.addData("Front Left Power", robot.flDrive.motor.getPower()); + engine.telemetry.addData("Front Right Power", robot.frDrive.motor.getPower()); + engine.telemetry.addData("Back Left Power", robot.blDrive.motor.getPower()); + engine.telemetry.addData("Back Right Power", robot.brDrive.motor.getPower()); + engine.telemetry.addData("FL Position",robot.flDrive.motor.getCurrentPosition()); + engine.telemetry.addData("IMU Angles", robot.imu.getRobotYawPitchRollAngles()); + + } + + @Override + public void init() { + robot.flDrive.motor.setPower(0); + robot.frDrive.motor.setPower(0); + robot.blDrive.motor.setPower(0); + robot.brDrive.motor.setPower(0); + + robot.imu.resetYaw(); + imuInitAngle = robot.imu.getRobotYawPitchRollAngles(); + + gamepad1Checker = new GamepadChecker(engine, engine.gamepad1); + gamepad2Checker = new GamepadChecker(engine, engine.gamepad2); + } + + @Override + public void exec() { + + if (engine.gamepad1.right_stick_y < 0.1) { + drivePower = 0; + } + + if (engine.gamepad1.start && !engine.gamepad1.a) { + robot.flDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.frDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.blDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.brDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.imu.resetYaw(); + } + + if (engine.gamepad1.right_stick_y > 0.1) { + drivePower = engine.gamepad1.right_stick_y; + robot.flDrive.motor.setPower(drivePower); + robot.frDrive.motor.setPower(drivePower); + robot.blDrive.motor.setPower(drivePower); + robot.brDrive.motor.setPower(drivePower); + } + + + + } +} From 09280688fb0bfc176041d09cf56de8c3e4d3b4ed Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Wed, 1 Nov 2023 20:24:34 -0500 Subject: [PATCH 5/5] Minibot teleop drive, rotation buggy (Acts like it was told to strafe) --- ...ibotTeleOPBot.java => MiniYTeleOPBot.java} | 13 ++++-- .../CenterStage/Common/ProtoBotSodi.java | 4 +- ...leOPEngine.java => MiniYTeleOPEngine.java} | 8 ++-- .../TeleOp/States/YellowMinibotTeleOP.java | 40 ++++++++++++++----- 4 files changed, 43 insertions(+), 22 deletions(-) rename TeamCode/src/main/java/org/timecrafters/CenterStage/Common/{MinibotTeleOPBot.java => MiniYTeleOPBot.java} (80%) rename TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/{MiniBotTeleOPEngine.java => MiniYTeleOPEngine.java} (66%) diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MinibotTeleOPBot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniYTeleOPBot.java similarity index 80% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MinibotTeleOPBot.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniYTeleOPBot.java index 2fcd5f5..cb1c400 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MinibotTeleOPBot.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniYTeleOPBot.java @@ -3,6 +3,7 @@ package org.timecrafters.CenterStage.Common; import com.arcrobotics.ftclib.hardware.motors.MotorEx; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; @@ -11,23 +12,22 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig import dev.cyberarm.engine.V2.CyberarmEngine; -public class MinibotTeleOPBot extends Robot { +public class MiniYTeleOPBot extends Robot { public HardwareMap hardwareMap; public MotorEx flDrive, frDrive, blDrive, brDrive; public IMU imu; private String string; - private CyberarmEngine engine; public TimeCraftersConfiguration configuration; - public MinibotTeleOPBot() { + public MiniYTeleOPBot() { } @Override public void setup() { this.hardwareMap = CyberarmEngine.instance.hardwareMap; - this.engine = CyberarmEngine.instance; + CyberarmEngine engine = CyberarmEngine.instance; imu = engine.hardwareMap.get(IMU.class, "imu"); @@ -44,6 +44,11 @@ public class MinibotTeleOPBot extends Robot { brDrive = new MotorEx(hardwareMap, "BackRight"); blDrive = new MotorEx(hardwareMap, "BackLeft"); + flDrive.motor.setDirection(DcMotorSimple.Direction.FORWARD); + frDrive.motor.setDirection(DcMotorSimple.Direction.FORWARD); + blDrive.motor.setDirection(DcMotorSimple.Direction.FORWARD); + brDrive.motor.setDirection(DcMotorSimple.Direction.FORWARD); + flDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); blDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java index 1591788..88c188b 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java @@ -28,20 +28,18 @@ public class ProtoBotSodi extends Robot { public MotorEx flDrive, frDrive, blDrive, brDrive, liftMotor; public Servo grabJaw, grabElbow, grabShoulder, dropShoulder, dropElbow, dropJaw; private String string; - private CyberarmEngine engine; public TimeCraftersConfiguration configuration; public ProtoBotSodi(String string) { - this.engine = engine; this.string = string; } @Override public void setup() {System.out.println("Bacon: " + this.string); this.hardwareMap = CyberarmEngine.instance.hardwareMap; - this.engine = CyberarmEngine.instance; + CyberarmEngine engine = CyberarmEngine.instance; // TimeCraftersConfiguration configuration = new TimeCraftersConfiguration(); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBotTeleOPEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniYTeleOPEngine.java similarity index 66% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBotTeleOPEngine.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniYTeleOPEngine.java index c17c93f..cf2b61a 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBotTeleOPEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniYTeleOPEngine.java @@ -2,18 +2,18 @@ package org.timecrafters.CenterStage.TeleOp.Engines; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.timecrafters.CenterStage.Common.MinibotTeleOPBot; +import org.timecrafters.CenterStage.Common.MiniYTeleOPBot; import org.timecrafters.CenterStage.TeleOp.States.YellowMinibotTeleOP; import dev.cyberarm.engine.V2.CyberarmEngine; @TeleOp(name = "A Yellow Minibot") - public class MiniBotTeleOPEngine extends CyberarmEngine { - private MinibotTeleOPBot robot; + public class MiniYTeleOPEngine extends CyberarmEngine { + private MiniYTeleOPBot robot; @Override public void setup() { - this.robot = new MinibotTeleOPBot(); + this.robot = new MiniYTeleOPBot(); this.robot.setup(); addState(new YellowMinibotTeleOP(robot)); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java index 9144105..17f7883 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java @@ -9,21 +9,20 @@ import dev.cyberarm.engine.V2.GamepadChecker; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; -import org.timecrafters.CenterStage.Common.MinibotTeleOPBot; +import org.timecrafters.CenterStage.Common.MiniYTeleOPBot; public class YellowMinibotTeleOP extends CyberarmState { - private final MinibotTeleOPBot robot; + private final MiniYTeleOPBot robot; public float angleDelta, drivePower; YawPitchRollAngles imuInitAngle; - private GamepadChecker gamepad1Checker, gamepad2Checker; - public YellowMinibotTeleOP(MinibotTeleOPBot robot) { + public YellowMinibotTeleOP(MiniYTeleOPBot robot) { this.robot = robot; } public float getAngleDelta() { - robot.imu.getRobotYawPitchRollAngles(); + robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); return angleDelta; } @@ -39,12 +38,13 @@ public class YellowMinibotTeleOP extends CyberarmState { engine.telemetry.addData("Back Left Power", robot.blDrive.motor.getPower()); engine.telemetry.addData("Back Right Power", robot.brDrive.motor.getPower()); engine.telemetry.addData("FL Position",robot.flDrive.motor.getCurrentPosition()); - engine.telemetry.addData("IMU Angles", robot.imu.getRobotYawPitchRollAngles()); + engine.telemetry.addData("IMU Angles", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); } @Override public void init() { + drivePower = 0; robot.flDrive.motor.setPower(0); robot.frDrive.motor.setPower(0); robot.blDrive.motor.setPower(0); @@ -53,15 +53,19 @@ public class YellowMinibotTeleOP extends CyberarmState { robot.imu.resetYaw(); imuInitAngle = robot.imu.getRobotYawPitchRollAngles(); - gamepad1Checker = new GamepadChecker(engine, engine.gamepad1); - gamepad2Checker = new GamepadChecker(engine, engine.gamepad2); + GamepadChecker gamepad1Checker = new GamepadChecker(engine, engine.gamepad1); + GamepadChecker gamepad2Checker = new GamepadChecker(engine, engine.gamepad2); } @Override public void exec() { - if (engine.gamepad1.right_stick_y < 0.1) { + if (Math.abs(engine.gamepad1.right_stick_y) < 0.1 && Math.abs(engine.gamepad1.right_stick_x) < 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1) { drivePower = 0; + robot.flDrive.motor.setPower(0); + robot.frDrive.motor.setPower(0); + robot.blDrive.motor.setPower(0); + robot.brDrive.motor.setPower(0); } if (engine.gamepad1.start && !engine.gamepad1.a) { @@ -72,7 +76,7 @@ public class YellowMinibotTeleOP extends CyberarmState { robot.imu.resetYaw(); } - if (engine.gamepad1.right_stick_y > 0.1) { + if (Math.abs(engine.gamepad1.right_stick_y) > 0.1 && Math.abs(engine.gamepad1.right_stick_x) < 0.1) { drivePower = engine.gamepad1.right_stick_y; robot.flDrive.motor.setPower(drivePower); robot.frDrive.motor.setPower(drivePower); @@ -80,7 +84,21 @@ public class YellowMinibotTeleOP extends CyberarmState { robot.brDrive.motor.setPower(drivePower); } + if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) { + drivePower = engine.gamepad1.right_stick_x; + robot.flDrive.motor.setPower(-drivePower); + robot.frDrive.motor.setPower(drivePower); + robot.blDrive.motor.setPower(drivePower); + robot.brDrive.motor.setPower(-drivePower); + } - + if (Math.abs(engine.gamepad1.left_stick_x) > 0.1){ + drivePower = engine.gamepad1.left_stick_x; + robot.flDrive.motor.setPower(-drivePower); + robot.frDrive.motor.setPower(drivePower); + robot.blDrive.motor.setPower(-drivePower); + robot.brDrive.motor.setPower(drivePower); + } + } }