diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MinibotTeleOPBot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniYTeleOPBot.java similarity index 80% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MinibotTeleOPBot.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniYTeleOPBot.java index 2fcd5f5..cb1c400 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MinibotTeleOPBot.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniYTeleOPBot.java @@ -3,6 +3,7 @@ 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.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; @@ -11,23 +12,22 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig import dev.cyberarm.engine.V2.CyberarmEngine; -public class MinibotTeleOPBot extends Robot { +public class MiniYTeleOPBot 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() { + public MiniYTeleOPBot() { } @Override public void setup() { this.hardwareMap = CyberarmEngine.instance.hardwareMap; - this.engine = CyberarmEngine.instance; + CyberarmEngine engine = CyberarmEngine.instance; imu = engine.hardwareMap.get(IMU.class, "imu"); @@ -44,6 +44,11 @@ public class MinibotTeleOPBot extends Robot { brDrive = new MotorEx(hardwareMap, "BackRight"); blDrive = new MotorEx(hardwareMap, "BackLeft"); + flDrive.motor.setDirection(DcMotorSimple.Direction.FORWARD); + frDrive.motor.setDirection(DcMotorSimple.Direction.FORWARD); + blDrive.motor.setDirection(DcMotorSimple.Direction.FORWARD); + brDrive.motor.setDirection(DcMotorSimple.Direction.FORWARD); + 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); 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 1591788..88c188b 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java @@ -28,20 +28,18 @@ public class ProtoBotSodi extends Robot { public MotorEx flDrive, frDrive, blDrive, brDrive, liftMotor; public Servo grabJaw, grabElbow, grabShoulder, dropShoulder, dropElbow, dropJaw; private String string; - private CyberarmEngine engine; public TimeCraftersConfiguration configuration; public ProtoBotSodi(String string) { - this.engine = engine; this.string = string; } @Override public void setup() {System.out.println("Bacon: " + this.string); this.hardwareMap = CyberarmEngine.instance.hardwareMap; - this.engine = CyberarmEngine.instance; + CyberarmEngine engine = CyberarmEngine.instance; // TimeCraftersConfiguration configuration = new TimeCraftersConfiguration(); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBotTeleOPEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniYTeleOPEngine.java similarity index 66% rename from TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBotTeleOPEngine.java rename to TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniYTeleOPEngine.java index c17c93f..cf2b61a 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBotTeleOPEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniYTeleOPEngine.java @@ -2,18 +2,18 @@ package org.timecrafters.CenterStage.TeleOp.Engines; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.timecrafters.CenterStage.Common.MinibotTeleOPBot; +import org.timecrafters.CenterStage.Common.MiniYTeleOPBot; 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; + public class MiniYTeleOPEngine extends CyberarmEngine { + private MiniYTeleOPBot robot; @Override public void setup() { - this.robot = new MinibotTeleOPBot(); + this.robot = new MiniYTeleOPBot(); 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 index 9144105..17f7883 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 @@ -9,21 +9,20 @@ 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; +import org.timecrafters.CenterStage.Common.MiniYTeleOPBot; public class YellowMinibotTeleOP extends CyberarmState { - private final MinibotTeleOPBot robot; + private final MiniYTeleOPBot robot; public float angleDelta, drivePower; YawPitchRollAngles imuInitAngle; - private GamepadChecker gamepad1Checker, gamepad2Checker; - public YellowMinibotTeleOP(MinibotTeleOPBot robot) { + public YellowMinibotTeleOP(MiniYTeleOPBot robot) { this.robot = robot; } public float getAngleDelta() { - robot.imu.getRobotYawPitchRollAngles(); + robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); return angleDelta; } @@ -39,12 +38,13 @@ public class YellowMinibotTeleOP extends CyberarmState { 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()); + engine.telemetry.addData("IMU Angles", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)); } @Override public void init() { + drivePower = 0; robot.flDrive.motor.setPower(0); robot.frDrive.motor.setPower(0); robot.blDrive.motor.setPower(0); @@ -53,15 +53,19 @@ public class YellowMinibotTeleOP extends CyberarmState { robot.imu.resetYaw(); imuInitAngle = robot.imu.getRobotYawPitchRollAngles(); - gamepad1Checker = new GamepadChecker(engine, engine.gamepad1); - gamepad2Checker = new GamepadChecker(engine, engine.gamepad2); + GamepadChecker gamepad1Checker = new GamepadChecker(engine, engine.gamepad1); + GamepadChecker gamepad2Checker = new GamepadChecker(engine, engine.gamepad2); } @Override public void exec() { - if (engine.gamepad1.right_stick_y < 0.1) { + 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) { drivePower = 0; + robot.flDrive.motor.setPower(0); + robot.frDrive.motor.setPower(0); + robot.blDrive.motor.setPower(0); + robot.brDrive.motor.setPower(0); } if (engine.gamepad1.start && !engine.gamepad1.a) { @@ -72,7 +76,7 @@ public class YellowMinibotTeleOP extends CyberarmState { robot.imu.resetYaw(); } - if (engine.gamepad1.right_stick_y > 0.1) { + 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; robot.flDrive.motor.setPower(drivePower); robot.frDrive.motor.setPower(drivePower); @@ -80,7 +84,21 @@ public class YellowMinibotTeleOP extends CyberarmState { robot.brDrive.motor.setPower(drivePower); } + 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); + robot.brDrive.motor.setPower(-drivePower); + } - + 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); + } + } }