From 0990edb0382340d7b6a62e7c7590a925ebb3d84a Mon Sep 17 00:00:00 2001 From: Carlos Rivas Date: Sun, 3 Nov 2024 15:11:23 -0800 Subject: [PATCH] Added motors subsystem plus 2 sample DevOps files --- .../ftc/teamcode/DevTeleOpRemix.java | 97 +++++++++++++++ .../ftc/teamcode/DevTeleOpRemixDeux.java | 107 ++++++++++++++++ .../teamcode/subsystem/MotorsSubsystem.java | 117 ++++++++++++++++++ 3 files changed, 321 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java new file mode 100644 index 0000000..cf22479 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemix.java @@ -0,0 +1,97 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Gamepad; + +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.MotorsSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; + +@TeleOp(name = "Dev Teleop Remix", group = "Debug") +@Disabled +public class DevTeleOpRemix extends OpMode { + + public ClawSubsystem claw; + public ArmSubsystem arm; + public WristSubsystem wrist; + public LiftSubsystem lift; + public MotorsSubsystem motors; + + public Gamepad currentGamepad1; + public Gamepad previousGamepad1; + public Gamepad currentGamepad2; + public Gamepad previousGamepad2; + + public double power = .6; + + @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); + motors = new MotorsSubsystem(hardwareMap, telemetry, power); + + claw.init(); + arm.init(); + wrist.init(); + lift.init(); + motors.init(); + + currentGamepad1 = new Gamepad(); + previousGamepad1 = new Gamepad(); + currentGamepad2 = new Gamepad(); + previousGamepad2 = new Gamepad(); + } + + public void theDrop(ArmSubsystem arm, WristSubsystem wrist) { + if (currentGamepad1.a && !previousGamepad1.a) { + wrist.floorWrist(); + arm.engageArm(); + } + } + + 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(); + wrist.bucketWrist(); + } + } + + 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); + + previousGamepad2.copy(currentGamepad2); + currentGamepad2.copy(gamepad2); + + theDrop(arm, wrist); + thePickup(claw); + theLift(arm, wrist); + theLowBucketScore(lift, wrist, arm); + + motors.calculateTrajectory(gamepad1); + + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java new file mode 100644 index 0000000..5748d16 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleOpRemixDeux.java @@ -0,0 +1,107 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +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.MotorsSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; + +@TeleOp(name = "Dev Teleop Remix", group = "Debug") +@Disabled +public class DevTeleOpRemixDeux extends OpMode { + + private Follower follower; + + public ClawSubsystem claw; + public ArmSubsystem arm; + public WristSubsystem wrist; + public LiftSubsystem lift; + public MotorsSubsystem motors; + + public Gamepad currentGamepad1; + public Gamepad previousGamepad1; + public Gamepad currentGamepad2; + public Gamepad previousGamepad2; + + public double power = .6; + + @Override + public void init() { + + follower = new Follower(hardwareMap); + + claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED); + arm = new ArmSubsystem(hardwareMap, ArmSubsystem.ArmState.PARK); + wrist = new WristSubsystem(hardwareMap, WristSubsystem.WristState.FLOOR); + motors = new MotorsSubsystem(hardwareMap, telemetry); + lift = new LiftSubsystem(hardwareMap); + + claw.init(); + arm.init(); + wrist.init(); + lift.init(); + motors.init(); + + currentGamepad1 = new Gamepad(); + previousGamepad1 = new Gamepad(); + currentGamepad2 = new Gamepad(); + previousGamepad2 = new Gamepad(); + + follower.setMaxPower(this.power); + follower.startTeleopDrive(); + + } + + public void theDrop(ArmSubsystem arm, WristSubsystem wrist) { + if (currentGamepad1.a && !previousGamepad1.a) { + wrist.floorWrist(); + arm.engageArm(); + } + } + + 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(); + wrist.bucketWrist(); + } + } + + 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); + + previousGamepad2.copy(currentGamepad2); + currentGamepad2.copy(gamepad2); + + theDrop(arm, wrist); + thePickup(claw); + theLift(arm, wrist); + theLowBucketScore(lift, wrist, arm); + + follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x); + follower.update(); + + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java new file mode 100644 index 0000000..30fb764 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java @@ -0,0 +1,117 @@ +package org.firstinspires.ftc.teamcode.subsystem; + +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.Gamepad; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +public class MotorsSubsystem { + + public HardwareMap hardwareMap; + + public Telemetry telemetry; + + public DcMotor frontLeftMotor; + public DcMotor backLeftMotor; + public DcMotor frontRightMotor; + public DcMotor backRightMotor; + + public double power; + + public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { + this.hardwareMap = hardwareMap; + this.telemetry = telemetry; + this.power = 1.0; + } + + public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry, double power) { + this.hardwareMap = hardwareMap; + this.telemetry = telemetry; + this.power = power; + } + + public void 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); + + frontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + frontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + + public void setFrontLeftMotorPower(double power) { + frontLeftMotor.setPower(power); + } + + public void setBackLeftMotorPower(double power) { + backLeftMotor.setPower(power); + } + + public void setFrontRightMotorPower(double power) { + frontRightMotor.setPower(power); + } + + public void setBackRightMotorPower(double power) { + backRightMotor.setPower(power); + } + + public void calculateTrajectory(Gamepad gamepad1) { + 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 leftBackPower = axial - lateral + yaw; + + double rightFrontPower = 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 + this.setFrontLeftMotorPower(leftFrontPower * this.power); + this.setFrontRightMotorPower(rightFrontPower * this.power); + this.setBackLeftMotorPower(leftBackPower * this.power); + this.setBackRightMotorPower(rightBackPower * this.power); + + // Show the elapsed game time and wheel power. + this.telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); + this.telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); + this.telemetry.update(); + } + +}