From 0f42160c4f866f365a1b55395d4ecb5211e37733 Mon Sep 17 00:00:00 2001 From: robotics1 Date: Sun, 3 Nov 2024 12:08:14 -0800 Subject: [PATCH] We scored one point! --- .../firstinspires/ftc/teamcode/DevTeleop.java | 123 +++++++++++++++--- .../firstinspires/ftc/teamcode/LiftTest.java | 1 + .../ftc/teamcode/configs/RobotConstants.java | 6 +- .../ftc/teamcode/subsystem/LiftSubsystem.java | 12 +- 4 files changed, 113 insertions(+), 29 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java index 341b24a..3b6a043 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java @@ -1,53 +1,87 @@ package org.firstinspires.ftc.teamcode; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION; +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 static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER; +import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER; +import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION; + import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.Gamepad; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.LiftSubsystem; import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; -@TeleOp(name = "ArmTelop", group = "Debug") +@TeleOp(name = "Dev Teleop", group = "Debug") public class DevTeleop extends OpMode { public ClawSubsystem claw; public ArmSubsystem arm; public WristSubsystem wrist; + public LiftSubsystem lift; public Gamepad currentGamepad1; public Gamepad previousGamepad1; - + public DcMotor frontLeftMotor; + public DcMotor backLeftMotor; + public DcMotor frontRightMotor; + public DcMotor backRightMotor; @Override public void init() { claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED); arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK); wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR); + lift = new LiftSubsystem(hardwareMap); claw.init(); arm.init(); wrist.init(); - + lift.init(); + + frontLeftMotor = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR); + backLeftMotor = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR); + frontRightMotor = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR); + backRightMotor = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR); + + frontLeftMotor.setDirection(FRONT_LEFT_MOTOR_DIRECTION); + backLeftMotor.setDirection(BACK_LEFT_MOTOR_DIRECTION); + frontRightMotor.setDirection(FRONT_RIGHT_MOTOR_DIRECTION); + backRightMotor.setDirection(BACK_RIGHT_MOTOR_DIRECTION); + currentGamepad1 = new Gamepad(); previousGamepad1 = new Gamepad(); } - public void theDrop(ClawSubsystem claw, ArmSubsystem arm, WristSubsystem wrist) { - - if (currentGamepad1.a && !previousGamepad1.a) { - claw.openClaw(); - wrist.floorWrist(); - arm.engageArm(); - } - - } - public void thePickup(ClawSubsystem claw, ArmSubsystem arm, WristSubsystem wrist) { - - if (currentGamepad1.x && !previousGamepad1.x) { - claw.switchState(); + public void theDrop(ArmSubsystem arm, WristSubsystem wrist) { + if (currentGamepad1.a && !previousGamepad1.a) { + wrist.floorWrist(); + arm.engageArm(); } } - public void theLift(ClawSubsystem claw, ArmSubsystem arm, WristSubsystem wrist) { + public void thePickup(ClawSubsystem claw) { + + if (currentGamepad1.x && !previousGamepad1.x) { + claw.switchState(); + } + + } + + public void theLift(ArmSubsystem arm, WristSubsystem wrist) { if (currentGamepad1.b && !previousGamepad1.b) { arm.parkArm(); @@ -55,13 +89,62 @@ public class DevTeleop extends OpMode { } } + + public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) { + if (currentGamepad1.y && !previousGamepad1.y) { + lift.toLowBucket(); + wrist.bucketWrist(); + } + } + @Override public void loop() { previousGamepad1.copy(currentGamepad1); currentGamepad1.copy(gamepad1); - theDrop(claw, arm, wrist); - thePickup(claw, arm, wrist); - theLift(claw, arm, wrist); + theDrop(arm, wrist); + thePickup(claw); + theLift(arm, wrist); + theLowBucketScore(lift, wrist, arm); + + double max; + + // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. + double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value + double lateral = gamepad1.left_stick_x; + double yaw = gamepad1.right_stick_x; + + // Combine the joystick requests for each axis-motion to determine each wheel's power. + // Set up a variable for each drive wheel to save the power level for telemetry. + double leftFrontPower = axial + lateral + yaw; + double rightFrontPower = axial - lateral - yaw; + double leftBackPower = axial - lateral + yaw; + double rightBackPower = axial + lateral - yaw; + + // Normalize the values so no wheel power exceeds 100% + // This ensures that the robot maintains the desired motion. + max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); + max = Math.max(max, Math.abs(leftBackPower)); + max = Math.max(max, Math.abs(rightBackPower)); + + if (max > 1.0) { + leftFrontPower /= max; + rightFrontPower /= max; + leftBackPower /= max; + rightBackPower /= max; + } + + + // Send calculated power to wheels + frontLeftMotor.setPower(leftFrontPower); + frontRightMotor.setPower(rightFrontPower); + backLeftMotor.setPower(leftBackPower); + backRightMotor.setPower(rightBackPower); + + // Show the elapsed game time and wheel power. + telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); + telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + telemetry.update(); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java index fa085a0..30b4dc3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftTest.java @@ -64,6 +64,7 @@ public class LiftTest extends LinearOpMode { Gamepad currentGamepad1 = new Gamepad(); Gamepad previousGamepad1 = new Gamepad(); + lift.init(); waitForStart(); runtime.reset(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java index 18d77d4..aa62e1c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java @@ -4,15 +4,15 @@ import com.acmerobotics.dashboard.config.Config; @Config public class RobotConstants { - public static double clawClose = 0.95; - public static double clawOpen = 0.5; + public static double clawClose = 1.00; + public static double clawOpen = 0.25; public static double armEngage = 0.5; public static double armPark = 0.125; public static double armBucket = 0.175; public static double wristFloor = 0.625; - public static double wristBucket = 0.25; + public static double wristBucket = 0.215; public static int liftZeroPos = 0; public static int liftToLowBucketPos = 2250; public static int liftToHighBucketPos = 3700; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java index 08c5e5b..33bd585 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java @@ -34,8 +34,8 @@ public class LiftSubsystem { } public void toZero() { - setTarget(liftZeroPos); - setState(LiftState.FLOOR); + lift.setTargetPosition(liftZeroPos); + lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); } public void switchState() { @@ -49,13 +49,13 @@ public class LiftSubsystem { } public void toLowBucket() { - setTarget(liftToLowBucketPos); - setState(LiftState.LOW_BUCKET); + lift.setTargetPosition(liftToLowBucketPos); + lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); } public void toHighBucket() { - setTarget(liftToHighBucketPos); - setState(LiftState.HIGH_BUCKET); + lift.setTargetPosition(liftToHighBucketPos); + lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); } public void init() {