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] 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() { + + } +}