From 23ce09ea937eaddb09601f4f83acb389702eacc8 Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Thu, 2 Nov 2023 20:31:33 -0500 Subject: [PATCH] Minibot teleop drive, black as well, need to add servo movement (why?) --- .../CenterStage/Common/MiniBTeleOPBot.java | 57 +++++++++++++++++++ .../TeleOp/Engines/MiniBTeleOPEngine.java | 22 +++++++ .../TeleOp/States/BlackMiniTeleOP.java | 41 +++++++++++++ .../TeleOp/States/YellowMinibotTeleOP.java | 14 ++--- 4 files changed, 127 insertions(+), 7 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniBTeleOPBot.java create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBTeleOPEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/BlackMiniTeleOP.java diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniBTeleOPBot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniBTeleOPBot.java new file mode 100644 index 0000000..2ea5457 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniBTeleOPBot.java @@ -0,0 +1,57 @@ +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 com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.ServoImpl; + +import org.timecrafters.Library.Robot; +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +public class MiniBTeleOPBot extends Robot { + +public HardwareMap hardwareMap; +public MotorEx leftDrive, rightDrive; +public Servo servLowLeft, servLowRight, servTop; +public IMU imu; + +public TimeCraftersConfiguration configuration; + +public MiniBTeleOPBot() { +} + + @Override + public void setup() { + this.hardwareMap = CyberarmEngine.instance.hardwareMap; + CyberarmEngine 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 + leftDrive = new MotorEx(hardwareMap, "LeftDrive"); + rightDrive = new MotorEx(hardwareMap, "RightDrive"); + + leftDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + leftDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + //Servos + servLowLeft = hardwareMap.servo.get("ServoLowLeft"); + servLowRight = hardwareMap.servo.get("ServoLowRight"); + servTop = hardwareMap.servo.get("ServoTop"); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBTeleOPEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBTeleOPEngine.java new file mode 100644 index 0000000..804234a --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBTeleOPEngine.java @@ -0,0 +1,22 @@ +package org.timecrafters.CenterStage.TeleOp.Engines; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.timecrafters.CenterStage.Common.MiniBTeleOPBot; +import org.timecrafters.CenterStage.TeleOp.States.BlackMiniTeleOP; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@TeleOp(name = "Black Minibot") + +public class MiniBTeleOPEngine extends CyberarmEngine { + private MiniBTeleOPBot robot; + + @Override + public void setup() { + this.robot = new MiniBTeleOPBot(); + this.robot.setup(); + + addState(new BlackMiniTeleOP()); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/BlackMiniTeleOP.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/BlackMiniTeleOP.java new file mode 100644 index 0000000..083d51a --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/BlackMiniTeleOP.java @@ -0,0 +1,41 @@ +package org.timecrafters.CenterStage.TeleOp.States; + +import org.timecrafters.CenterStage.Common.MiniBTeleOPBot; + +import dev.cyberarm.engine.V2.CyberarmState; + +public class BlackMiniTeleOP extends CyberarmState { + private MiniBTeleOPBot robot; + public BlackMiniTeleOP(MiniBTeleOPBot robot) {this.robot = robot;} + + + private double rPower, lPower; + + public BlackMiniTeleOP() { + } + + @Override + public void init() { + rPower = 0; + lPower = 0; + robot.rightDrive.motor.setPower(rPower); + robot.leftDrive.motor.setPower(lPower); + + } + @Override + public void exec() { + + if (engine.gamepad1.right_trigger > 0.1) { + rPower = engine.gamepad1.right_trigger; + robot.rightDrive.motor.setPower(rPower); + } + + if (engine.gamepad1.left_trigger > 0.1) { + lPower = engine.gamepad1.left_trigger; + robot.leftDrive.motor.setPower(lPower); + } + + + + } +} 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 index 17f7883..68c697a 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java @@ -60,7 +60,7 @@ public class YellowMinibotTeleOP extends CyberarmState { @Override public void exec() { - if (Math.abs(engine.gamepad1.right_stick_y) < 0.1 && Math.abs(engine.gamepad1.right_stick_x) < 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1) { + if (Math.abs(engine.gamepad1.left_stick_y) < 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1 && Math.abs(engine.gamepad1.right_stick_x) < 0.1) { drivePower = 0; robot.flDrive.motor.setPower(0); robot.frDrive.motor.setPower(0); @@ -76,24 +76,24 @@ public class YellowMinibotTeleOP extends CyberarmState { robot.imu.resetYaw(); } - if (Math.abs(engine.gamepad1.right_stick_y) > 0.1 && Math.abs(engine.gamepad1.right_stick_x) < 0.1) { - drivePower = engine.gamepad1.right_stick_y; + if (Math.abs(engine.gamepad1.left_stick_y) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1) { + drivePower = engine.gamepad1.left_stick_y; robot.flDrive.motor.setPower(drivePower); robot.frDrive.motor.setPower(drivePower); robot.blDrive.motor.setPower(drivePower); robot.brDrive.motor.setPower(drivePower); } - if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) { - drivePower = engine.gamepad1.right_stick_x; + if (Math.abs(engine.gamepad1.left_stick_x) > 0.1) { + drivePower = engine.gamepad1.left_stick_x; robot.flDrive.motor.setPower(-drivePower); robot.frDrive.motor.setPower(drivePower); robot.blDrive.motor.setPower(drivePower); robot.brDrive.motor.setPower(-drivePower); } - if (Math.abs(engine.gamepad1.left_stick_x) > 0.1){ - drivePower = engine.gamepad1.left_stick_x; + if (Math.abs(engine.gamepad1.right_stick_x) > 0.1){ + drivePower = engine.gamepad1.right_stick_x; robot.flDrive.motor.setPower(-drivePower); robot.frDrive.motor.setPower(drivePower); robot.blDrive.motor.setPower(-drivePower);