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 new file mode 100644 index 0000000..25960f3 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/DemoPingPongEngine.java @@ -0,0 +1,20 @@ +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 new file mode 100644 index 0000000..2a48a85 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/DistanceSensorTest.java @@ -0,0 +1,25 @@ +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 new file mode 100644 index 0000000..89e3fdb --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/DriveAutonomous.java @@ -0,0 +1,11 @@ +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/Mecanum_Robot_Engine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/Mecanum_Robot_Engine.java new file mode 100644 index 0000000..6c7dd31 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/Mecanum_Robot_Engine.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.Mecanum_Robot_State; + +@TeleOp(name = "Mecanum Robot TeleOp") + +public class Mecanum_Robot_Engine extends CyberarmEngine { + MecanumRobot robot; + + @Override + public void setup() { + + robot = new MecanumRobot(this); + + addState(new Mecanum_Robot_State(robot)); + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/Mecanum_fancy_drive_TeleOp.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/Mecanum_fancy_drive_TeleOp.java new file mode 100644 index 0000000..13fe54f --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/Mecanum_fancy_drive_TeleOp.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.Mecanum_Fancy_Drive_State; + +@TeleOp(name = "Fancy Drive TeleOp") + +public class Mecanum_fancy_drive_TeleOp extends CyberarmEngine { + MecanumRobot robot; + + @Override + public void setup() { + + robot = new MecanumRobot(this); + + addState(new Mecanum_Fancy_Drive_State(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 new file mode 100644 index 0000000..3c24fa3 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DemoPingPongState.java @@ -0,0 +1,89 @@ +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 new file mode 100644 index 0000000..1fe0646 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DistanceSensorTest.java @@ -0,0 +1,139 @@ +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/DriveState.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DriveState.java new file mode 100644 index 0000000..e69de29 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 new file mode 100644 index 0000000..e30f876 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumRobot.java @@ -0,0 +1,62 @@ +package org.timecrafters.minibots.cyberarm.states; + +import com.qualcomm.hardware.rev.Rev2mDistanceSensor; +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; + +import org.cyberarm.engine.V2.CyberarmEngine; + +public class MecanumRobot { + + private CyberarmEngine engine; + + public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, shooterWheel, collectorRight; + + public Servo ballBlocker; + + public RevBlinkinLedDriver ledDriver; + public Rev2mDistanceSensor ballPositionSensor; + + public MecanumRobot(CyberarmEngine engine) { + this.engine = engine; + + setupRobot(); + } + + private void setupRobot() { + + //motors configuration + frontLeftDrive = engine.hardwareMap.dcMotor.get("front left"); + 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.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + frontRightDrive.setDirection(DcMotorSimple.Direction.REVERSE); + frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + backLeftDrive.setDirection(DcMotorSimple.Direction.FORWARD); + backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + backRightDrive.setDirection(DcMotorSimple.Direction.REVERSE); + 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 new file mode 100644 index 0000000..88ed1ce --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Fancy_Drive_State.java @@ -0,0 +1,143 @@ +package org.timecrafters.minibots.cyberarm.states; + +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; + +import org.cyberarm.engine.V2.CyberarmState; +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; + + + public Mecanum_Fancy_Drive_State(MecanumRobot robot) { + this.robot = robot; + } + + @Override + public void init() { + + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GREEN); + + } + + @Override + public void exec() { + + //Gamepad Read + + A = engine.gamepad1.a; + 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 + + // 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); + + //----------------------------------------------------------------- + // 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); + } + } + +//------------------------------------------------------------------------------------------------------------------------------------ +// Telemetry Data + + + @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/Mecanum_Robot_State.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Robot_State.java new file mode 100644 index 0000000..cd840e6 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Robot_State.java @@ -0,0 +1,114 @@ +package org.timecrafters.minibots.cyberarm.states; + +import org.cyberarm.engine.V2.CyberarmState; + +public class Mecanum_Robot_State extends CyberarmState { + + private final MecanumRobot robot; + private float maxSpeed = 1; + private double halfSpeed = 0.5; + + public Mecanum_Robot_State(MecanumRobot robot) { + this.robot = robot; + } + + @Override + public void exec() { + + if (engine.gamepad1.left_trigger > 0.5){ + + if (engine.gamepad1.left_bumper) { + + robot.backRightDrive.setPower(halfSpeed); + robot.frontRightDrive.setPower(-halfSpeed); + robot.backLeftDrive.setPower(-halfSpeed); + robot.frontLeftDrive.setPower(halfSpeed); + + } else if (engine.gamepad1.right_bumper) { + + robot.backRightDrive.setPower(-halfSpeed); + robot.frontRightDrive.setPower(halfSpeed); + robot.backLeftDrive.setPower(halfSpeed); + robot.frontLeftDrive.setPower(-halfSpeed); + + } else if (engine.gamepad1.dpad_right && engine.gamepad1.dpad_up) { + + robot.frontLeftDrive.setPower(-halfSpeed); + robot.backRightDrive.setPower(-halfSpeed); + + } else if (engine.gamepad1.dpad_left && engine.gamepad1.dpad_up) { + + robot.frontRightDrive.setPower(-halfSpeed); + robot.backLeftDrive.setPower(-halfSpeed); + + } else if (engine.gamepad1.dpad_right && engine.gamepad1.dpad_down) { + + robot.backLeftDrive.setPower(halfSpeed); + robot.frontRightDrive.setPower(halfSpeed); + + } else if (engine.gamepad1.dpad_left && engine.gamepad1.dpad_down) { + + robot.backRightDrive.setPower(halfSpeed); + robot.frontLeftDrive.setPower(halfSpeed); + + } + + else { + + robot.backRightDrive.setPower(engine.gamepad1.right_stick_y * halfSpeed) ; + robot.frontRightDrive.setPower(engine.gamepad1.right_stick_y * halfSpeed); + robot.backLeftDrive.setPower(engine.gamepad1.left_stick_y * halfSpeed); + robot.frontLeftDrive.setPower(engine.gamepad1.left_stick_y * halfSpeed); + + } + } + + else { + + if (engine.gamepad1.left_bumper) { + + robot.backRightDrive.setPower(maxSpeed); + robot.frontRightDrive.setPower(-maxSpeed); + robot.backLeftDrive.setPower(-maxSpeed); + robot.frontLeftDrive.setPower(maxSpeed); + + } else if (engine.gamepad1.right_bumper) { + + robot.backRightDrive.setPower(-maxSpeed); + robot.frontRightDrive.setPower(maxSpeed); + robot.backLeftDrive.setPower(maxSpeed); + robot.frontLeftDrive.setPower(-maxSpeed); + + } else if (engine.gamepad1.dpad_right && engine.gamepad1.dpad_up) { + + robot.frontLeftDrive.setPower(-maxSpeed); + robot.backRightDrive.setPower(-maxSpeed); + + } else if (engine.gamepad1.dpad_left && engine.gamepad1.dpad_up) { + + robot.frontRightDrive.setPower(-maxSpeed); + robot.backLeftDrive.setPower(-maxSpeed); + + } else if (engine.gamepad1.dpad_right && engine.gamepad1.dpad_down) { + + robot.backLeftDrive.setPower(maxSpeed); + robot.frontRightDrive.setPower(maxSpeed); + + } else if (engine.gamepad1.dpad_left && engine.gamepad1.dpad_down) { + + robot.backRightDrive.setPower(maxSpeed); + robot.frontLeftDrive.setPower(maxSpeed); + + } else { + + robot.backRightDrive.setPower(engine.gamepad1.right_stick_y * maxSpeed) ; + robot.frontRightDrive.setPower(engine.gamepad1.right_stick_y * maxSpeed); + robot.backLeftDrive.setPower(engine.gamepad1.left_stick_y * maxSpeed); + robot.frontLeftDrive.setPower(engine.gamepad1.left_stick_y * maxSpeed); + + } + + } + + } +}