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/DemoPingPongEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/FieldOrientedEngine.java similarity index 51% rename from TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/DemoPingPongEngine.java rename to TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/FieldOrientedEngine.java index 25960f3..25c5bdf 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/DemoPingPongEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/FieldOrientedEngine.java @@ -1,20 +1,24 @@ 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.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.minibots.cyberarm.states.FieldOrientedDrive; import org.timecrafters.minibots.cyberarm.states.MecanumRobot; -@TeleOp(name = "DemoTeleOp") +@TeleOp(name = "Field Oriented Drive") -public class DemoPingPongEngine extends CyberarmEngine { +public class FieldOrientedEngine extends CyberarmEngine { + MecanumRobot robot; @Override public void setup() { - MecanumRobot robot; - robot = new MecanumRobot(this); - addState(new DemoPingPongState(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 3c24fa3..0000000 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DemoPingPongState.java +++ /dev/null @@ -1,89 +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); - - - } - } -} - 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 1fe0646..0000000 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DistanceSensorTest.java +++ /dev/null @@ -1,139 +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); - - } -} 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 88ed1ce..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,23 +9,20 @@ 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; + 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 +32,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,70 +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); - 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 - 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.GREEN); - } else if (ballAmount.equals("2 balls")) { - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE_GREEN); - } else if (ballAmount.equals("1 balls")) { - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.CP1_2_END_TO_END_BLEND); - } 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); + + } + } //------------------------------------------------------------------------------------------------------------------------------------ @@ -133,9 +93,11 @@ 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()); } } 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..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,4 +113,5 @@ public class Mecanum_Robot_State extends CyberarmState { } } -} + } + 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