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] 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