diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDrive.java similarity index 77% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometDrive.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDrive.java index e193378..4411c43 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDrive.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.cometbots; +package org.firstinspires.ftc.teamcode; import static org.firstinspires.ftc.teamcode.util.action.FieldConstants.blueBucketStartPose; @@ -9,9 +9,9 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; import org.firstinspires.ftc.teamcode.runmodes.Teleop; -@TeleOp(name="Drive", group="A") +@TeleOp(name="ComeBot Drive", group="Debug") @Disabled -public class CometDrive extends OpMode { +public class CometBotDrive extends OpMode { private Teleop teleop; @@ -20,12 +20,7 @@ public class CometDrive extends OpMode { teleop = new Teleop(hardwareMap, telemetry, new Follower(hardwareMap), - blueBucketStartPose, gamepad1); - } - - @Override - public void start() { teleop.start(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java index 1124102..40195e4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java @@ -1,5 +1,16 @@ package org.firstinspires.ftc.teamcode.runmodes; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -12,51 +23,36 @@ import org.firstinspires.ftc.teamcode.util.action.RunAction; public class Teleop { private ClawSubsystem claw; - private ClawSubsystem.ClawState clawState; - private Follower follower; - private Pose startPose; - + private DcMotorEx leftFront; + private DcMotorEx leftRear; + private DcMotorEx rightFront; + private DcMotorEx rightRear; private Telemetry telemetry; private Gamepad gamepad1; private Gamepad currentGamepad1; private Gamepad previousGamepad1; - public RunAction stopDrive, startDrive; + public Teleop(HardwareMap hardwareMap, Telemetry telemetry, Follower follower, Gamepad gamepad1) { - public double speed = 0.5; - - public Teleop(HardwareMap hardwareMap, Telemetry telemetry, Follower follower, Pose startPose, Gamepad gamepad1) { - claw = new ClawSubsystem(hardwareMap, clawState); + claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED); + initMotors(hardwareMap); this.follower = follower; - this.startPose = startPose; this.telemetry = telemetry; this.gamepad1 = gamepad1; this.currentGamepad1 = new Gamepad(); this.previousGamepad1 = new Gamepad(); - - stopDrive = new RunAction(this::stopDrive); - startDrive = new RunAction(this::startDrive); } public void start() { claw.start(); - follower.setPose(startPose); follower.startTeleopDrive(); } - private void startDrive() { - follower.startTeleopDrive(); - } - - private void stopDrive() { - follower.breakFollowing(); - } - public void update() { previousGamepad1.copy(currentGamepad1); currentGamepad1.copy(gamepad1); @@ -64,14 +60,30 @@ public class Teleop { if (currentGamepad1.a && !previousGamepad1.a) claw.switchState(); - follower.setTeleOpMovementVectors(-gamepad1.left_stick_y * speed, -gamepad1.left_stick_x * speed, -gamepad1.right_stick_x * speed); + follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x); follower.update(); telemetry.addData("X", follower.getPose().getX()); telemetry.addData("Y", follower.getPose().getY()); telemetry.addData("Heading", Math.toDegrees(follower.getPose().getHeading())); - - telemetry.addData("Claw State", clawState); + telemetry.addData("Claw State", claw.getState()); telemetry.update(); } + + private void initMotors(HardwareMap hardwareMap) { + rightFront = hardwareMap.get(DcMotorEx.class, FRONT_RIGHT_MOTOR); + rightRear = hardwareMap.get(DcMotorEx.class, BACK_RIGHT_MOTOR); + leftFront = hardwareMap.get(DcMotorEx.class, FRONT_LEFT_MOTOR); + leftRear = hardwareMap.get(DcMotorEx.class, BACK_LEFT_MOTOR); + + rightFront.setDirection(FRONT_RIGHT_MOTOR_DIRECTION); + rightRear.setDirection(BACK_RIGHT_MOTOR_DIRECTION); + leftFront.setDirection(FRONT_LEFT_MOTOR_DIRECTION); + leftRear.setDirection(BACK_LEFT_MOTOR_DIRECTION); + + leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } }