From a380321dd1e3582ade0d0527395eb4e9c3cbea07 Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Thu, 25 Aug 2022 19:29:07 -0500 Subject: [PATCH 1/2] I completely forgot to commit any of the new stuff I have done. Basically it is just states and engines for my not so mini "minibot" with a couple TeleOp states and engines and a new autonomous drive state and engine. and theres a distance sensor test to give me telemetry on distance from each ball amount when the hopper is filled up or empty or filled with one or two or three balls. which the logic was very in efficient the way i chose to do it and then when implementing caused problems in my standard TeleOp (Fancy Drive TeleOp) and then came up with a better solution. with proper comparisons and now it works amazingly --- .../cyberarm/engines/SlowModeTestEngine.java | 22 ++++++ .../cyberarm/states/DemoPingPongState.java | 5 ++ .../cyberarm/states/DistanceSensorTest.java | 5 ++ .../states/Mecanum_Fancy_Drive_State.java | 53 ++++++++------ .../cyberarm/states/Mecanum_Robot_State.java | 5 ++ .../cyberarm/states/SlowModeTestPingPong.java | 71 +++++++++++++++++++ 6 files changed, 138 insertions(+), 23 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/SlowModeTestEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/SlowModeTestPingPong.java diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/SlowModeTestEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/SlowModeTestEngine.java new file mode 100644 index 0000000..b5fd4ce --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/SlowModeTestEngine.java @@ -0,0 +1,22 @@ +package org.timecrafters.minibots.cyberarm.engines; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.minibots.cyberarm.states.MecanumRobot; +import org.timecrafters.minibots.cyberarm.states.SlowModeTestPingPong; + +@TeleOp (name = "Slow Mode Test") + +public class SlowModeTestEngine extends CyberarmEngine { + MecanumRobot robot; + + @Override + public void setup() { + + robot = new MecanumRobot(this); + + addState(new SlowModeTestPingPong(robot)); + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DemoPingPongState.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DemoPingPongState.java index 3c24fa3..dc42ac8 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DemoPingPongState.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DemoPingPongState.java @@ -85,5 +85,10 @@ public class DemoPingPongState extends CyberarmState { } } + + @Override + public void exac() { + + } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DistanceSensorTest.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DistanceSensorTest.java index 1fe0646..5a6a500 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DistanceSensorTest.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DistanceSensorTest.java @@ -136,4 +136,9 @@ public class DistanceSensorTest extends CyberarmState { engine.telemetry.addData("Amount", ballAmount); } + + @Override + public void exac() { + + } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Fancy_Drive_State.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Fancy_Drive_State.java index 88ed1ce..e57f762 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Fancy_Drive_State.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Fancy_Drive_State.java @@ -13,19 +13,17 @@ public class Mecanum_Fancy_Drive_State extends CyberarmState { public boolean A; public boolean X; public boolean Y; + private double drivePower = 1; + private boolean bprev; public Mecanum_Fancy_Drive_State(MecanumRobot robot) { this.robot = robot; } - @Override public void init() { - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GREEN); - } - @Override public void exec() { @@ -35,16 +33,21 @@ public class Mecanum_Fancy_Drive_State extends CyberarmState { X = engine.gamepad1.x; Y = engine.gamepad1.y; - //Drivetrain Variables double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing double rx = engine.gamepad1.right_stick_x; - - //Shooter Variables - - //Gamepad variables + //toggle for drive speed + boolean b = engine.gamepad1.b; + if (b && !bprev) { + if (drivePower == 1) { + drivePower = 0.5; + } else { + drivePower = 1; + } + } + bprev = b; // Denominator is the largest motor power (absolute value) or 1 // This ensures all the powers maintain the same ratio, but only when @@ -61,10 +64,10 @@ public class Mecanum_Fancy_Drive_State extends CyberarmState { // directions on drive motors or put a negative in behind the joystick power to reverse it. // I put negatives in to reverse it because it was the easiest at the moment. - robot.frontLeftDrive.setPower(-frontLeftPower); - robot.backLeftDrive.setPower(-backLeftPower); - robot.frontRightDrive.setPower(-frontRightPower); - robot.backRightDrive.setPower(-backRightPower); + robot.frontLeftDrive.setPower(-frontLeftPower * drivePower); + robot.backLeftDrive.setPower(-backLeftPower * drivePower); + robot.frontRightDrive.setPower(-frontRightPower * drivePower); + robot.backRightDrive.setPower(-backRightPower * drivePower); //----------------------------------------------------------------- // Shooter program @@ -72,14 +75,10 @@ public class Mecanum_Fancy_Drive_State extends CyberarmState { if (A) { robot.shooterWheel.setPower(1); robot.collectorRight.setPower(0); - } - - if (Y) { + } if (Y) { robot.shooterWheel.setPower(0); robot.collectorRight.setPower(1); - } - - if (X) { + } if (X) { robot.shooterWheel.setPower(0); robot.collectorRight.setPower(0); } @@ -117,11 +116,11 @@ public class Mecanum_Fancy_Drive_State extends CyberarmState { } else if (robot.shooterWheel.getPower() == 1) { robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.RED); } else if (ballAmount.equals("3 balls")) { - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GREEN); + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.RED); } else if (ballAmount.equals("2 balls")) { - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE_GREEN); + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.ORANGE); } else if (ballAmount.equals("1 balls")) { - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.CP1_2_END_TO_END_BLEND); + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.YELLOW); } else { robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE); } @@ -133,9 +132,17 @@ public class Mecanum_Fancy_Drive_State extends CyberarmState { @Override public void telemetry() { - engine.telemetry.addData("Distance", robot.ballPositionSensor.getDistance(DistanceUnit.CM)); engine.telemetry.addData("Amount", ballAmount); + engine.telemetry.addData("Speed", drivePower); + engine.telemetry.addData("FrontLeftEncoder", robot.frontLeftDrive.getCurrentPosition()); + engine.telemetry.addData("FrontRightEncoder", robot.frontRightDrive.getCurrentPosition()); + engine.telemetry.addData("BackLeftEncoder", robot.backLeftDrive.getCurrentPosition()); + engine.telemetry.addData("BackRightEncoder", robot.backRightDrive.getCurrentPosition()); + + } + @Override + public void exac() { } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Robot_State.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Robot_State.java index cd840e6..8fa63af 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Robot_State.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Robot_State.java @@ -111,4 +111,9 @@ public class Mecanum_Robot_State extends CyberarmState { } } + + @Override + public void exac() { + + } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/SlowModeTestPingPong.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/SlowModeTestPingPong.java new file mode 100644 index 0000000..662d1a3 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/SlowModeTestPingPong.java @@ -0,0 +1,71 @@ +package org.timecrafters.minibots.cyberarm.states; + +import org.cyberarm.engine.V2.CyberarmState; + +public class SlowModeTestPingPong extends CyberarmState { + + MecanumRobot robot; + private double drivePower = 1; + private boolean bprev; + + + + public SlowModeTestPingPong (MecanumRobot robot){ + this.robot = robot; + } + + @Override + public void exec() { + + //toggle for drive speed + boolean b = engine.gamepad1.b; + if (b && !bprev) { + if (drivePower == 1) { + drivePower = 0.5; + } else { + drivePower = 1; + } + } + bprev = b; + + + + //Drivetrain Variables + double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative + double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing + double rx = engine.gamepad1.right_stick_x; + + //Gamepad variables + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, but only when + // at least one is out of the range [-1, 1] + + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + double frontLeftPower = (y + x + rx) / denominator; + double backLeftPower = (y - x + rx) / denominator; + double frontRightPower = (y - x - rx) / denominator; + double backRightPower = (y + x - rx) / denominator; + + // As I programed this and ran it, I realized everything was backwards + // in direction so to fix that i either went in the robot object state and reversed + // directions on drive motors or put a negative in behind the joystick power to reverse it. + // I put negatives in to reverse it because it was the easiest at the moment. + + robot.frontLeftDrive.setPower(-frontLeftPower * drivePower); + robot.backLeftDrive.setPower(-backLeftPower * drivePower); + robot.frontRightDrive.setPower(-frontRightPower * drivePower); + robot.backRightDrive.setPower(-backRightPower * drivePower); + + //----------------------------------------------------------------- + + + + + } + + @Override + public void exac() { + + } +} From 1cdf8615cb45f9bf90bf2a0afab73d64d64789cb Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Sun, 11 Sep 2022 18:06:28 -0500 Subject: [PATCH 2/2] got rid of shooter and collector stuff in my old minibot robot centric drive tele op --- .../cyberarm/engines/DemoPingPongEngine.java | 20 --- .../cyberarm/engines/DistanceSensorTest.java | 25 --- .../cyberarm/engines/DriveAutonomous.java | 11 -- ...stEngine.java => FieldOrientedEngine.java} | 12 +- .../cyberarm/states/DemoPingPongState.java | 94 ------------ .../cyberarm/states/DistanceSensorTest.java | 144 ------------------ .../cyberarm/states/FieldOrientedDrive.java | 67 ++++++++ .../cyberarm/states/MecanumRobot.java | 24 +-- .../states/Mecanum_Fancy_Drive_State.java | 71 ++------- .../cyberarm/states/Mecanum_Robot_State.java | 8 +- .../cyberarm/states/SlowModeTestPingPong.java | 71 --------- build.gradle | 2 +- gradle/wrapper/gradle-wrapper.properties | 5 +- 13 files changed, 100 insertions(+), 454 deletions(-) delete mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/DemoPingPongEngine.java delete mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/DistanceSensorTest.java delete mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/DriveAutonomous.java rename TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/{SlowModeTestEngine.java => FieldOrientedEngine.java} (56%) delete mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DemoPingPongState.java delete mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DistanceSensorTest.java create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/FieldOrientedDrive.java delete mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/SlowModeTestPingPong.java diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/DemoPingPongEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/DemoPingPongEngine.java deleted file mode 100644 index 25960f3..0000000 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/DemoPingPongEngine.java +++ /dev/null @@ -1,20 +0,0 @@ -package org.timecrafters.minibots.cyberarm.engines; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.cyberarm.engine.V2.CyberarmEngine; -import org.timecrafters.minibots.cyberarm.states.DemoPingPongState; -import org.timecrafters.minibots.cyberarm.states.MecanumRobot; - -@TeleOp(name = "DemoTeleOp") - -public class DemoPingPongEngine extends CyberarmEngine { - - @Override - public void setup() { - - MecanumRobot robot; - - robot = new MecanumRobot(this); - - addState(new DemoPingPongState(robot)); - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/DistanceSensorTest.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/DistanceSensorTest.java deleted file mode 100644 index 2a48a85..0000000 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/DistanceSensorTest.java +++ /dev/null @@ -1,25 +0,0 @@ -package org.timecrafters.minibots.cyberarm.engines; - -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.cyberarm.engine.V2.CyberarmEngine; -import org.timecrafters.minibots.cyberarm.states.DemoPingPongState; -import org.timecrafters.minibots.cyberarm.states.MecanumRobot; - -@TeleOp( name = "Distance Sensor Test") - -public class DistanceSensorTest extends CyberarmEngine { - - @Override - public void setup() { - - MecanumRobot robot; - - robot = new MecanumRobot(this); - - addState(new org.timecrafters.minibots.cyberarm.states.DistanceSensorTest(robot)); - - } - - -} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/DriveAutonomous.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/DriveAutonomous.java deleted file mode 100644 index 89e3fdb..0000000 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/DriveAutonomous.java +++ /dev/null @@ -1,11 +0,0 @@ -package org.timecrafters.minibots.cyberarm.engines; - -import org.cyberarm.engine.V2.CyberarmEngine; - -public class DriveAutonomous extends CyberarmEngine { - - @Override - public void setup() { - - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/SlowModeTestEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/FieldOrientedEngine.java similarity index 56% rename from TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/SlowModeTestEngine.java rename to TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/FieldOrientedEngine.java index b5fd4ce..25c5bdf 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/SlowModeTestEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/FieldOrientedEngine.java @@ -3,20 +3,22 @@ package org.timecrafters.minibots.cyberarm.engines; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.cyberarm.engine.V2.CyberarmEngine; +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.minibots.cyberarm.states.FieldOrientedDrive; import org.timecrafters.minibots.cyberarm.states.MecanumRobot; -import org.timecrafters.minibots.cyberarm.states.SlowModeTestPingPong; -@TeleOp (name = "Slow Mode Test") +@TeleOp(name = "Field Oriented Drive") + +public class FieldOrientedEngine extends CyberarmEngine { -public class SlowModeTestEngine extends CyberarmEngine { MecanumRobot robot; - @Override public void setup() { robot = new MecanumRobot(this); - addState(new SlowModeTestPingPong(robot)); + addState(new FieldOrientedDrive(robot)); } } + diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DemoPingPongState.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DemoPingPongState.java deleted file mode 100644 index dc42ac8..0000000 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DemoPingPongState.java +++ /dev/null @@ -1,94 +0,0 @@ -package org.timecrafters.minibots.cyberarm.states; - -import com.qualcomm.robotcore.hardware.Gamepad; - -import org.cyberarm.engine.V2.CyberarmState; - -public class DemoPingPongState extends CyberarmState { - - private final MecanumRobot robot; - private boolean A; - private boolean B; - private boolean X; - private boolean Y; - - public DemoPingPongState(MecanumRobot robot) { - this.robot = robot; - } - - @Override - public void exec() { - - //Gamepad Read - - A = engine.gamepad2.a; - B = engine.gamepad2.b; - X = engine.gamepad2.x; - Y = engine.gamepad2.y; - - Gamepad gamepad = engine.gamepad1; - if (engine.gamepad2.right_trigger != 0) { - gamepad = engine.gamepad2; - } - - double y = -gamepad.left_stick_y; // Remember, this is reversed! because of the negative - double x = gamepad.left_stick_x * 1.1; // Counteract imperfect strafing - double rx = gamepad.right_stick_x; - - // Denominator is the largest motor power (absolute value) or 1 - // This ensures all the powers maintain the same ratio, but only when - // at least one is out of the range [-1, 1] - - double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); - double frontLeftPower = (y + x + rx) / denominator; - double backLeftPower = (y - x + rx) / denominator; - double frontRightPower = (y - x - rx) / denominator; - double backRightPower = (y + x - rx) / denominator; - - // As I programed this and ran it, I realized everything was backwards - // in direction so to fix that i either went in the robot object state and reversed - // directions on drive motors or put a negative in behind the joystick power to reverse it. - // I put negatives in to reverse it because it was the easiest at the moment. - - robot.frontLeftDrive.setPower(-frontLeftPower); - robot.backLeftDrive.setPower(-backLeftPower); - robot.frontRightDrive.setPower(-frontRightPower); - robot.backRightDrive.setPower(-backRightPower); - -//-------------------------------------------------------------------------------------------------------------------------------- -// GamePad 2 Controls (GamePad 1 Has no control with these) - - if (A){ - robot.shooterWheel.setPower(1); - robot.collectorRight.setPower(0); - } - - if (Y){ - robot.shooterWheel.setPower(0); - robot.collectorRight.setPower(1); - } - - if (X){ - robot.shooterWheel.setPower(0); - robot.collectorRight.setPower(0); - } - - // Ball blocker Servo Program - // This says if the trigger is pressed down 50% or more the servo brings the blocker down - if (engine.gamepad2.left_trigger >= 0.5){ - robot.ballBlocker.setPosition(0.45); - } - // this else statement says if nothing is pressed then it stays in the up position - else { - robot.ballBlocker.setPosition(0.52); - - - } - } - - @Override - public void exac() { - - } -} - diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DistanceSensorTest.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DistanceSensorTest.java deleted file mode 100644 index 5a6a500..0000000 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DistanceSensorTest.java +++ /dev/null @@ -1,144 +0,0 @@ -package org.timecrafters.minibots.cyberarm.states; - -import org.cyberarm.engine.V2.CyberarmState; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; - -public class DistanceSensorTest extends CyberarmState { - - private final MecanumRobot robot; - public String ballAmount; - public DistanceSensorTest(MecanumRobot robot) { - this.robot = robot; - } - - @Override - public void exec() { - - if (robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 5.0 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 5.1 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 5.2 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 5.3 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 5.4 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 5.5 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 5.6 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 5.7 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 5.8 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 5.9 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 6.0 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 6.1 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 6.2 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 6.3 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 6.4 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 6.5 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 6.6 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 6.7 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 6.8 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 6.9 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 7.0 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 7.1 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 7.2 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 7.3 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 7.4 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 7.5) { - - ballAmount = "3 balls"; - - } - - - if (robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 10.0 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 10.1 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 10.2 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 10.3 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 10.4 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 10.5 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 10.6 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 10.7 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 10.8 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 10.9 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 11.0 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 11.1 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 11.2 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 11.3 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 11.4 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 11.5 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 11.6 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 11.7 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 11.8 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 11.9) { - - ballAmount = "2 balls"; - - } - - - if (robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 14.0 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 14.1 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 14.2 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 14.3 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 14.4 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 14.5 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 14.6 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 14.7 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 14.8 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 14.9 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 15.0 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 15.1 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 15.2 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 15.3 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 15.4 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 15.5 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 15.6 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 15.7 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 15.8 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 15.9 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 16.0 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 16.1 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 16.2 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 16.3 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 16.4 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 16.5 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 16.6 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 16.7 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 16.8 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 16.9) { - - ballAmount = "1 ball"; - - } - - - if (robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 19.0 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 19.1 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 19.2 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 19.3 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 19.4 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 19.5 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 19.6 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 19.7 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 19.8 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 19.9 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 20.0 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 20.1 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 20.2 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 20.3 - || robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 20.4){ - - ballAmount = "no balls"; - - } - } - - @Override - public void telemetry() { - - engine.telemetry.addData("distance", robot.ballPositionSensor.getDistance(DistanceUnit.CM)); - engine.telemetry.addData("Amount", ballAmount); - - } - - @Override - public void exac() { - - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/FieldOrientedDrive.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/FieldOrientedDrive.java new file mode 100644 index 0000000..198ea3b --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/FieldOrientedDrive.java @@ -0,0 +1,67 @@ +package org.timecrafters.minibots.cyberarm.states; + +//adb connect 192.168.43.1 + +import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; + +import org.cyberarm.engine.V2.CyberarmState; + +public class FieldOrientedDrive extends CyberarmState { + + MecanumRobot robot; + BNO055IMU imu; + + public FieldOrientedDrive(MecanumRobot robot) { + this.robot = robot; + } + + @Override + public void init() { + + BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); +// parameters.loggingEnabled = true; +// parameters.loggingTag = "IMU"; + imu = engine.hardwareMap.get(BNO055IMU.class, "imu"); + imu.initialize(parameters); + + parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS; + + imu.initialize(parameters); + + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GREEN); + } + + @Override + public void exec() { + + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.LIGHT_CHASE_BLUE); + + double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! + double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing + double rx = engine.gamepad1.right_stick_x; + + // Read inverse IMU heading, as the IMU heading is CW positive + + double botHeading = -imu.getAngularOrientation().firstAngle; + + double rotX = x * Math.cos(botHeading) - y * Math.sin(botHeading); + double rotY = x * Math.sin(botHeading) + y * Math.cos(botHeading); + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, but only when + // at least one is out of the range [-1, 1] + + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + double frontLeftPower = (rotY + rotX + rx) / denominator; + double backLeftPower = (rotY - rotX + rx) / denominator; + double frontRightPower = (rotY - rotX - rx) / denominator; + double backRightPower = (rotY + rotX - rx) / denominator; + + robot.frontLeftDrive.setPower(frontLeftPower * .95); + robot.backRightDrive.setPower(backLeftPower * .95); + robot.frontRightDrive.setPower(frontRightPower * 1); + robot.backRightDrive.setPower(backRightPower * 1); + + } + } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumRobot.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumRobot.java index e30f876..cb6f65b 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumRobot.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumRobot.java @@ -1,5 +1,6 @@ package org.timecrafters.minibots.cyberarm.states; +import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.hardware.rev.Rev2mDistanceSensor; import com.qualcomm.hardware.rev.RevBlinkinLedDriver; import com.qualcomm.robotcore.hardware.CRServo; @@ -13,12 +14,9 @@ public class MecanumRobot { private CyberarmEngine engine; - public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, shooterWheel, collectorRight; - - public Servo ballBlocker; + public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive; public RevBlinkinLedDriver ledDriver; - public Rev2mDistanceSensor ballPositionSensor; public MecanumRobot(CyberarmEngine engine) { this.engine = engine; @@ -33,30 +31,20 @@ public class MecanumRobot { frontRightDrive = engine.hardwareMap.dcMotor.get("front right"); backRightDrive = engine.hardwareMap.dcMotor.get("back left"); backLeftDrive = engine.hardwareMap.dcMotor.get("back right"); - shooterWheel = engine.hardwareMap.dcMotor.get("shooter wheel"); - collectorRight = engine.hardwareMap.dcMotor.get("collector motor"); ledDriver = engine.hardwareMap.get(RevBlinkinLedDriver.class, "lights"); //motors direction and encoders - frontLeftDrive.setDirection(DcMotorSimple.Direction.FORWARD); + frontLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE); frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - frontRightDrive.setDirection(DcMotorSimple.Direction.REVERSE); + frontRightDrive.setDirection(DcMotorSimple.Direction.FORWARD); frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - backLeftDrive.setDirection(DcMotorSimple.Direction.FORWARD); + backLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE); backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - backRightDrive.setDirection(DcMotorSimple.Direction.REVERSE); + backRightDrive.setDirection(DcMotorSimple.Direction.FORWARD); backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - - // Sensor Configuration - - ballPositionSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "ball position"); - - //Servo configuration - ballBlocker = engine.hardwareMap.servo.get("ball blocker"); } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Fancy_Drive_State.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Fancy_Drive_State.java index e57f762..1996661 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Fancy_Drive_State.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Fancy_Drive_State.java @@ -9,7 +9,6 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; public class Mecanum_Fancy_Drive_State extends CyberarmState { private final MecanumRobot robot; - public String ballAmount; public boolean A; public boolean X; public boolean Y; @@ -64,66 +63,28 @@ public class Mecanum_Fancy_Drive_State extends CyberarmState { // directions on drive motors or put a negative in behind the joystick power to reverse it. // I put negatives in to reverse it because it was the easiest at the moment. - robot.frontLeftDrive.setPower(-frontLeftPower * drivePower); - robot.backLeftDrive.setPower(-backLeftPower * drivePower); - robot.frontRightDrive.setPower(-frontRightPower * drivePower); - robot.backRightDrive.setPower(-backRightPower * drivePower); + robot.frontLeftDrive.setPower(frontLeftPower * drivePower); + robot.backLeftDrive.setPower(backLeftPower * drivePower); + robot.frontRightDrive.setPower(frontRightPower * drivePower); + robot.backRightDrive.setPower(backRightPower * drivePower); - //----------------------------------------------------------------- - // Shooter program - if (A) { - robot.shooterWheel.setPower(1); - robot.collectorRight.setPower(0); - } if (Y) { - robot.shooterWheel.setPower(0); - robot.collectorRight.setPower(1); - } if (X) { - robot.shooterWheel.setPower(0); - robot.collectorRight.setPower(0); - } - - // Ball blocker Servo Program - // This says if the trigger is pressed down 50% or more the servo brings the blocker down - if (engine.gamepad1.left_trigger >= 0.5) { - robot.ballBlocker.setPosition(0.45); - } - // this else statement says if nothing is pressed then it stays in the up position - else { - robot.ballBlocker.setPosition(0.52); - } - - //-------------------------------------------------------------------------------- - // ball Amount Sensor Program - - if (robot.ballPositionSensor.getDistance(DistanceUnit.CM) < 9) { - ballAmount = "3 balls"; - } else if (robot.ballPositionSensor.getDistance(DistanceUnit.CM) < 13){ - ballAmount = "2 balls"; - } else if (robot.ballPositionSensor.getDistance(DistanceUnit.CM) < 17){ - ballAmount = "1 balls"; - } else { - ballAmount = "no balls"; - } //------------------------------------------------------------------------------------------------------------------- // LIGHT CONTROLS - if (robot.ballBlocker.getPosition() == 0.45 && robot.shooterWheel.getPower() == 1) { - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.STROBE_RED); - } else if (robot.shooterWheel.getPower() == 1) { - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.RED); - } else if (ballAmount.equals("3 balls")) { - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.RED); - } else if (ballAmount.equals("2 balls")) { - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.ORANGE); - } else if (ballAmount.equals("1 balls")) { - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.YELLOW); - } else { - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE); + + if (drivePower == 1) { + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.LIGHT_CHASE_BLUE); } + + else if (drivePower == 0.5){ + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.YELLOW); + + } + } //------------------------------------------------------------------------------------------------------------------------------------ @@ -132,18 +93,12 @@ public class Mecanum_Fancy_Drive_State extends CyberarmState { @Override public void telemetry() { - engine.telemetry.addData("Distance", robot.ballPositionSensor.getDistance(DistanceUnit.CM)); - engine.telemetry.addData("Amount", ballAmount); engine.telemetry.addData("Speed", drivePower); engine.telemetry.addData("FrontLeftEncoder", robot.frontLeftDrive.getCurrentPosition()); engine.telemetry.addData("FrontRightEncoder", robot.frontRightDrive.getCurrentPosition()); engine.telemetry.addData("BackLeftEncoder", robot.backLeftDrive.getCurrentPosition()); engine.telemetry.addData("BackRightEncoder", robot.backRightDrive.getCurrentPosition()); - } - @Override - public void exac() { - } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Robot_State.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Robot_State.java index 8fa63af..81d0622 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Robot_State.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Robot_State.java @@ -4,6 +4,8 @@ import org.cyberarm.engine.V2.CyberarmState; public class Mecanum_Robot_State extends CyberarmState { + // adb connect 192.168.43.1 + private final MecanumRobot robot; private float maxSpeed = 1; private double halfSpeed = 0.5; @@ -111,9 +113,5 @@ public class Mecanum_Robot_State extends CyberarmState { } } - - @Override - public void exac() { - } -} + diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/SlowModeTestPingPong.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/SlowModeTestPingPong.java deleted file mode 100644 index 662d1a3..0000000 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/SlowModeTestPingPong.java +++ /dev/null @@ -1,71 +0,0 @@ -package org.timecrafters.minibots.cyberarm.states; - -import org.cyberarm.engine.V2.CyberarmState; - -public class SlowModeTestPingPong extends CyberarmState { - - MecanumRobot robot; - private double drivePower = 1; - private boolean bprev; - - - - public SlowModeTestPingPong (MecanumRobot robot){ - this.robot = robot; - } - - @Override - public void exec() { - - //toggle for drive speed - boolean b = engine.gamepad1.b; - if (b && !bprev) { - if (drivePower == 1) { - drivePower = 0.5; - } else { - drivePower = 1; - } - } - bprev = b; - - - - //Drivetrain Variables - double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative - double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing - double rx = engine.gamepad1.right_stick_x; - - //Gamepad variables - - // Denominator is the largest motor power (absolute value) or 1 - // This ensures all the powers maintain the same ratio, but only when - // at least one is out of the range [-1, 1] - - double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); - double frontLeftPower = (y + x + rx) / denominator; - double backLeftPower = (y - x + rx) / denominator; - double frontRightPower = (y - x - rx) / denominator; - double backRightPower = (y + x - rx) / denominator; - - // As I programed this and ran it, I realized everything was backwards - // in direction so to fix that i either went in the robot object state and reversed - // directions on drive motors or put a negative in behind the joystick power to reverse it. - // I put negatives in to reverse it because it was the easiest at the moment. - - robot.frontLeftDrive.setPower(-frontLeftPower * drivePower); - robot.backLeftDrive.setPower(-backLeftPower * drivePower); - robot.frontRightDrive.setPower(-frontRightPower * drivePower); - robot.backRightDrive.setPower(-backRightPower * drivePower); - - //----------------------------------------------------------------- - - - - - } - - @Override - public void exac() { - - } -} diff --git a/build.gradle b/build.gradle index 2ce4e62..4f9be88 100644 --- a/build.gradle +++ b/build.gradle @@ -10,7 +10,7 @@ buildscript { google() } dependencies { - classpath 'com.android.tools.build:gradle:7.2.0' + classpath 'com.android.tools.build:gradle:7.2.2' } } diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index aa991fc..9c697d0 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,6 @@ +#Fri Sep 02 20:25:51 CDT 2022 distributionBase=GRADLE_USER_HOME +distributionUrl=https\://services.gradle.org/distributions/gradle-7.3.3-bin.zip distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.4.2-bin.zip -zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists +zipStoreBase=GRADLE_USER_HOME