diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/black/BlackMinibot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/black/BlackMinibot.java new file mode 100644 index 0000000..2a0f2db --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/black/BlackMinibot.java @@ -0,0 +1,18 @@ +package dev.cyberarm.minibots.black; + +import com.arcrobotics.ftclib.hardware.motors.MotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +public class BlackMinibot { + final public MotorEx leftDrive, RightDrive; + final private HardwareMap hardwareMap; + + public BlackMinibot() { + hardwareMap = CyberarmEngine.instance.hardwareMap; + + leftDrive = new MotorEx(hardwareMap, "leftDrive"); + RightDrive = new MotorEx(hardwareMap, "rightDrive"); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/black/engines/BlackMinibotTeleOpEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/black/engines/BlackMinibotTeleOpEngine.java new file mode 100644 index 0000000..3be8156 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/black/engines/BlackMinibotTeleOpEngine.java @@ -0,0 +1,19 @@ +package dev.cyberarm.minibots.black.engines; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import dev.cyberarm.minibots.black.BlackMinibot; +import dev.cyberarm.minibots.black.states.BlackMinibotTeleOpState; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@TeleOp(name = "Cyberarm Black Minibot", group = "MINIBOT") +public class BlackMinibotTeleOpEngine extends CyberarmEngine { + private BlackMinibot minibot; + @Override + public void setup() { + minibot = new BlackMinibot(); + + addState(new BlackMinibotTeleOpState(minibot)); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/black/states/BlackMinibotTeleOpState.java b/TeamCode/src/main/java/dev/cyberarm/minibots/black/states/BlackMinibotTeleOpState.java new file mode 100644 index 0000000..04a133d --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/black/states/BlackMinibotTeleOpState.java @@ -0,0 +1,61 @@ +package dev.cyberarm.minibots.black.states; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.minibots.black.BlackMinibot; + +public class BlackMinibotTeleOpState extends CyberarmState { + final private BlackMinibot minibot; + + double maxPower = 0.2; + double forward = 0; + double right = 0; + + double leftPower = 0.0; + double rightPower = 0.0; + + public BlackMinibotTeleOpState(BlackMinibot minibot) { + this.minibot = minibot; + } + + @Override + public void exec() { + forward = -engine.gamepad1.left_stick_y; + right = engine.gamepad1.right_stick_x; + + leftPower = forward * maxPower; + rightPower = forward * maxPower; + + // Simple TANK DRIVE + if (forward > -0.01 && forward < 0.01) { + if (right > 0) { + leftPower = right * maxPower; + rightPower = -right * maxPower; + } else if (right < 0) { + leftPower = -right * maxPower; + rightPower = right * maxPower; + } + } else { + if (right > 0) { + leftPower += right * maxPower; + rightPower -= right * maxPower; + } else if (right < 0) { + leftPower -= right * maxPower; + rightPower += right * maxPower; + } + } + + minibot.leftDrive.motorEx.setPower(leftPower); + minibot.RightDrive.motorEx.setPower(rightPower); + } + + @Override + public void telemetry() { + engine.telemetry.addLine("Cyberarm BLACK MINIBOT"); + engine.telemetry.addData("Forward", forward); + engine.telemetry.addData("Right", right); + engine.telemetry.addLine(); + engine.telemetry.addData("Max Power", maxPower); + engine.telemetry.addData("Left Power", leftPower); + engine.telemetry.addData("Right Power", rightPower); + } +}