From 22c4c5149e01da28c4cfa790d2010e742ce66df8 Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Tue, 31 Oct 2023 16:12:43 -0500 Subject: [PATCH] Minibot teleop basic forward drive --- .../CenterStage/Common/MinibotTeleOPBot.java | 58 +++++++++++++ .../CenterStage/Common/ProtoBotSodi.java | 35 ++++---- .../TeleOp/Engines/MiniBotTeleOPEngine.java | 21 +++++ .../TeleOp/States/YellowMinibotTeleOP.java | 86 +++++++++++++++++++ 4 files changed, 183 insertions(+), 17 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MinibotTeleOPBot.java create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBotTeleOPEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MinibotTeleOPBot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MinibotTeleOPBot.java new file mode 100644 index 0000000..2fcd5f5 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MinibotTeleOPBot.java @@ -0,0 +1,58 @@ +package org.timecrafters.CenterStage.Common; + +import com.arcrobotics.ftclib.hardware.motors.MotorEx; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; + +import org.timecrafters.Library.Robot; +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +public class MinibotTeleOPBot extends Robot { + + public HardwareMap hardwareMap; + public MotorEx flDrive, frDrive, blDrive, brDrive; + public IMU imu; + private String string; + private CyberarmEngine engine; + + public TimeCraftersConfiguration configuration; + + public MinibotTeleOPBot() { + } + + @Override + public void setup() { + this.hardwareMap = CyberarmEngine.instance.hardwareMap; + this.engine = CyberarmEngine.instance; + + imu = engine.hardwareMap.get(IMU.class, "imu"); + + IMU.Parameters parameters = new IMU.Parameters( + new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.LEFT, + RevHubOrientationOnRobot.UsbFacingDirection.UP)); + + imu.initialize(parameters); + + //Motors + frDrive = new MotorEx(hardwareMap, "FrontRight"); + flDrive = new MotorEx(hardwareMap, "FrontLeft"); + brDrive = new MotorEx(hardwareMap, "BackRight"); + blDrive = new MotorEx(hardwareMap, "BackLeft"); + + flDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + frDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + blDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + brDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + flDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + blDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + brDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java index ee6b8a0..1591788 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java @@ -25,8 +25,8 @@ import dev.cyberarm.engine.V2.CyberarmEngine; public class ProtoBotSodi extends Robot { public HardwareMap hardwareMap; - public MotorEx flDrive, frDrive, blDrive, brDrive /*liftMotor*/; -// public Servo grabJaw, grabElbow, grabShoulder, dropShoulder, dropElbow, dropJaw; + public MotorEx flDrive, frDrive, blDrive, brDrive, liftMotor; + public Servo grabJaw, grabElbow, grabShoulder, dropShoulder, dropElbow, dropJaw; private String string; private CyberarmEngine engine; @@ -50,18 +50,19 @@ public class ProtoBotSodi extends Robot { flDrive = new MotorEx(hardwareMap, "FrontLeft"); brDrive = new MotorEx(hardwareMap, "BackRight"); blDrive = new MotorEx(hardwareMap, "BackLeft"); -// liftMotor = new MotorEx(hardwareMap, "Lift"); + liftMotor = new MotorEx(hardwareMap, "Lift"); flDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); frDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); blDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); brDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + liftMotor.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); flDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); frDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); blDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); brDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); -// liftMotor.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + liftMotor.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); flDrive.motor.setDirection(FORWARD); // frDrive.motor.setDirection(REVERSE); @@ -69,19 +70,19 @@ public class ProtoBotSodi extends Robot { // brDrive.motor.setDirection(REVERSE); //Servos -// grabJaw = hardwareMap.servo.get("GrabJaw"); -// grabElbow = hardwareMap.servo.get("GrabElbow"); -// grabShoulder = hardwareMap.servo.get("GrabShoulder"); -// dropShoulder = hardwareMap.servo.get("DropShoulder"); -// dropElbow = hardwareMap.servo.get("DropElbow"); -// dropJaw = hardwareMap.servo.get("DropJaw"); -// -// grabElbow.setDirection(Servo.Direction.FORWARD); -// grabJaw.setDirection(Servo.Direction.FORWARD); -// grabShoulder.setDirection(Servo.Direction.FORWARD); -// dropShoulder.setDirection(Servo.Direction.FORWARD); -// dropElbow.setDirection(Servo.Direction.FORWARD); -// dropJaw.setDirection(Servo.Direction.FORWARD); + grabJaw = hardwareMap.servo.get("GrabJaw"); + grabElbow = hardwareMap.servo.get("GrabElbow"); + grabShoulder = hardwareMap.servo.get("GrabShoulder"); + dropShoulder = hardwareMap.servo.get("DropShoulder"); + dropElbow = hardwareMap.servo.get("DropElbow"); + dropJaw = hardwareMap.servo.get("DropJaw"); + + grabElbow.setDirection(Servo.Direction.FORWARD); + grabJaw.setDirection(Servo.Direction.FORWARD); + grabShoulder.setDirection(Servo.Direction.FORWARD); + dropShoulder.setDirection(Servo.Direction.FORWARD); + dropElbow.setDirection(Servo.Direction.FORWARD); + dropJaw.setDirection(Servo.Direction.FORWARD); } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBotTeleOPEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBotTeleOPEngine.java new file mode 100644 index 0000000..c17c93f --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBotTeleOPEngine.java @@ -0,0 +1,21 @@ +package org.timecrafters.CenterStage.TeleOp.Engines; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.timecrafters.CenterStage.Common.MinibotTeleOPBot; +import org.timecrafters.CenterStage.TeleOp.States.YellowMinibotTeleOP; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@TeleOp(name = "A Yellow Minibot") + + public class MiniBotTeleOPEngine extends CyberarmEngine { + private MinibotTeleOPBot robot; + @Override + public void setup() { + this.robot = new MinibotTeleOPBot(); + this.robot.setup(); + + addState(new YellowMinibotTeleOP(robot)); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java new file mode 100644 index 0000000..9144105 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java @@ -0,0 +1,86 @@ +package org.timecrafters.CenterStage.TeleOp.States; + +import dev.cyberarm.engine.V2.CyberarmState; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.Servo; + +import dev.cyberarm.engine.V2.GamepadChecker; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.timecrafters.CenterStage.Common.MinibotTeleOPBot; + +public class YellowMinibotTeleOP extends CyberarmState { + private final MinibotTeleOPBot robot; + public float angleDelta, drivePower; + YawPitchRollAngles imuInitAngle; + private GamepadChecker gamepad1Checker, gamepad2Checker; + + public YellowMinibotTeleOP(MinibotTeleOPBot robot) { + this.robot = robot; + } + + + public float getAngleDelta() { + robot.imu.getRobotYawPitchRollAngles(); + + return angleDelta; + } + + @Override + public void telemetry() { + engine.telemetry.addData("Front Left Velocity", robot.flDrive.getVelocity()); + engine.telemetry.addData("Front Right Velocity", robot.frDrive.getVelocity()); + engine.telemetry.addData("Back Left Velocity", robot.blDrive.getVelocity()); + engine.telemetry.addData("Back Right Velocity", robot.brDrive.getVelocity()); + engine.telemetry.addData("Front Left Power", robot.flDrive.motor.getPower()); + engine.telemetry.addData("Front Right Power", robot.frDrive.motor.getPower()); + engine.telemetry.addData("Back Left Power", robot.blDrive.motor.getPower()); + engine.telemetry.addData("Back Right Power", robot.brDrive.motor.getPower()); + engine.telemetry.addData("FL Position",robot.flDrive.motor.getCurrentPosition()); + engine.telemetry.addData("IMU Angles", robot.imu.getRobotYawPitchRollAngles()); + + } + + @Override + public void init() { + robot.flDrive.motor.setPower(0); + robot.frDrive.motor.setPower(0); + robot.blDrive.motor.setPower(0); + robot.brDrive.motor.setPower(0); + + robot.imu.resetYaw(); + imuInitAngle = robot.imu.getRobotYawPitchRollAngles(); + + gamepad1Checker = new GamepadChecker(engine, engine.gamepad1); + gamepad2Checker = new GamepadChecker(engine, engine.gamepad2); + } + + @Override + public void exec() { + + if (engine.gamepad1.right_stick_y < 0.1) { + drivePower = 0; + } + + if (engine.gamepad1.start && !engine.gamepad1.a) { + robot.flDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.frDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.blDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.brDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.imu.resetYaw(); + } + + if (engine.gamepad1.right_stick_y > 0.1) { + drivePower = engine.gamepad1.right_stick_y; + robot.flDrive.motor.setPower(drivePower); + robot.frDrive.motor.setPower(drivePower); + robot.blDrive.motor.setPower(drivePower); + robot.brDrive.motor.setPower(drivePower); + } + + + + } +}