From 90ff225bfa5f2500d0b127a3b4833567bfb148ce Mon Sep 17 00:00:00 2001 From: robotics1 Date: Mon, 27 Jan 2025 16:52:16 -0800 Subject: [PATCH] working drive code arm needs work --- .../paths/CometBotTeleopCompetition.java | 336 ++++++++++++++++++ .../paths/CometBotTeleopDriveV2.java | 24 ++ 2 files changed, 360 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopCompetition.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopDriveV2.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopCompetition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopCompetition.java new file mode 100644 index 0000000..a9a56b9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopCompetition.java @@ -0,0 +1,336 @@ +package org.firstinspires.ftc.teamcode.cometbots.paths; + +import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER; + +import com.acmerobotics.roadrunner.SequentialAction; +import com.acmerobotics.roadrunner.SleepAction; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; +import org.firstinspires.ftc.teamcode.states.FieldStates; +import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; + +public class CometBotTeleopCompetition { + + /* + Subsystems + */ + private MotorsSubsystem motors; + private ClawSubsystem claw; + private ArmSubsystem arm; + private WristSubsystem wrist; + private DualMotorSliderSubsystem lift; + + /* + Controllers + */ + public Gamepad GP1; + public Gamepad GP2; + public Gamepad currentGP1; + public Gamepad previousGP1; + public Gamepad currentGP2; + public Gamepad previousGP2; + + /* + Pedro/FTC Components + */ + private Follower follower; + private Telemetry telemetry; + + /* + States + */ + public FieldStates fieldStates; + + /* + Configurations + */ + public double currentPower = MAX_POWER; + + public CometBotTeleopCompetition(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) { + this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55); + + this.GP1 = gp1; + this.GP2 = gp2; + this.telemetry = telemetry; + this.currentGP1 = new Gamepad(); + this.currentGP2 = new Gamepad(); + this.previousGP1 = new Gamepad(); + this.previousGP2 = new Gamepad(); + this.fieldStates = new FieldStates(); + this.follower = new Follower(hardwareMap); + claw = new ClawSubsystem(hardwareMap); + arm = new ArmSubsystem(hardwareMap); + wrist = new WristSubsystem(hardwareMap); + lift = new DualMotorSliderSubsystem(hardwareMap); + } + + public void init() { + this.motors.init(); + claw.init(); + arm.initTeleOp(); + lift.init(); + ; + this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING); + + follower.setMaxPower(MAX_POWER); + follower.startTeleopDrive(); + claw.closeClaw(); + wrist.initTeleOp(); + } + + public void initCloseClaw(){ + this.motors.init(); + this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING); + follower.setMaxPower(MAX_POWER); + follower.startTeleopDrive(); + } + + public void update() { + this.previousGP1.copy(currentGP1); + this.currentGP1.copy(this.GP1); + this.previousGP2.copy(currentGP2); + this.currentGP2.copy(this.GP2); +// this.moveSkyHook(); + this.toHighBucketScore(); + this.toLowBucketScore(); + this.toArmParkPosition(); + this.toArmParkThenSwitchBetweenSubmarineAndFloorPosition(); + this.clawControl(); +// this.decreaseMaxPower(); +// this.increaseMaxPower(); + + +// Actions.runBlocking(this.lift.toFloorPosition()); + + follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x); + follower.update(); + lift.update(); + this.telemetry.addData("Field State", this.fieldStates.getFieldLocation()); +// this.telemetry.addData("Claw State", this.claw.getState()); +// this.telemetry.addData("Claw Position", this.claw.getPosition()); +// this.telemetry.addData("Wrist State", this.wrist.getState()); +// this.telemetry.addData("Arm State", this.arm.getState()); +// this.telemetry.addData("Lift State", this.lift.getState()); +// this.telemetry.addData("Lift Position", this.lift.getPosition()); + this.telemetry.addData("MaxPower", MAX_POWER); + } + + /* + Type: PS4 / Logitech + Controller: 1 + Button: Left Bumper + Assumption: Working motor mechanism + Action: Decreases maximum speed by -.05 + */ +// public void decreaseMaxPower() { +// if (this.currentGP1.left_bumper && !this.previousGP1.left_bumper) { +// this.currentPower = this.currentPower - .05; +// this.follower.setMaxPower(this.currentPower); +// } +// } + + /* + Type: PS4 / Logitech + Controller: 1 + Button: Left Bumper + Assumption: Working motor mechanism + Action: Increases maximum speed by +.05 + */ +// public void increaseMaxPower() { +// if (this.currentGP1.right_bumper && !this.previousGP1.right_bumper) { +// this.currentPower = this.currentPower + .05; +// this.follower.setMaxPower(this.currentPower); +// } +// } +// public void moveSkyHook() { +// if (this.currentGP2.dpad_down) { +// hook.moveSkyHook(-1.00); +// } +// else if (this.currentGP2.dpad_up) { +// hook.moveSkyHook(1.00); +// } +// else{ +// hook.moveSkyHook(0.00); +// } +// +// } + + /* + Type: PS4 / Logitech + Controller: 2 + Button: TRIANGLE / Y + Assumption: Claw is holding specimen, robot is facing buckets ready to score + Action: On button press, enter BUCKET state, arm is lifted up, wrist is lifted up and lift + raises to high bucket. Once at high bucket position, move arm forward, wrist forward + and open claw to drop specimen into bucket. Finally, put arm back up and wrist back up, + retract lift all the way down to floor position and back to TRAVELING state. + */ + public void toHighBucketScore() { + if (this.currentGP2.y && !this.previousGP2.y) { + lift.toHighBucketPosition(); + if (lift.getFixedPosition() >= 4450) { + arm.toBucketPosition(); + if(arm.getPosition() == 0.45) + wrist.toBucketPosition(); + if(wrist.getPosition() == 0.56) + claw.openClaw(); + } + } + } + + public void highBucketDrop() { + Actions.runBlocking(new SequentialAction( +// new SleepAction(.5), +// this.lift.toHighBucketPosition(), +// new SleepAction(.5), +// this.arm.toBucketPosition(), +// new SleepAction(.5), +// this.wrist.toBucketPosition(), +// new SleepAction(.5), +// this.claw.openClaw(), +// new SleepAction(.5), +// this.wrist.toFloorPosition(), +// new SleepAction(.5), +// this.arm.toParkPosition(), +// this.lift.toFloorPosition(), +// new SleepAction(.5) + )); + } + public void highBucketDropAuto() { + Actions.runBlocking(new SequentialAction( +// new SleepAction(.1), +// this.lift.toHighBucketPosition(), +// new SleepAction(.25), +// this.arm.toBucketPosition(), +// new SleepAction(.25), +// this.wrist.toBucketPosition(), +// new SleepAction(.25), +// this.claw.openClaw(), +// new SleepAction(.25), +// this.wrist.toFloorPosition(), +// new SleepAction(.25), +// this.arm.toParkPosition(), +// this.lift.toZeroPosition(), +// new SleepAction(.25) + )); + } + /* + Type: PS4 / Logitech + Controller: 2 + Button: CIRCLE / B + Assumption: Claw is holding specimen, robot is facing buckets ready to score + Action: On button press, enter BUCKET state, arm is lifted up, wrist is lifted up and lift + raises to low bucket. Once at low bucket position, move arm forward, wrist forward + and open claw to drop specimen into bucket. Finally, put arm back up and wrist back up, + retract lift all the way down to floor position and back to TRAVELING state. + */ + public void toLowBucketScore() { + if (this.currentGP2.b && !this.previousGP2.b) { + lift.toLowBucketPosition(); + if (lift.getFixedPosition() >= 1700); { + arm.toBucketPosition(); + if(arm.getPosition() == 0.45) + wrist.toBucketPosition(); + if(wrist.getPosition() == 0.56) + claw.openClaw(); + } + } + } + + /* + Type: PS4 / Logitech + Controller: 2 + Button: RIGHT BUMPER + Assumption: Working claw mechanism + Action: On button press, claw switches state from OPEN to CLOSE + */ + public void clawControl() { + if (this.currentGP2.right_bumper && !this.previousGP2.right_bumper) { + claw.switchState(); + } + } + + /* + Type: PS4 / Logitech + Controller: 2 + Button: SQUARE / X + Assumption: Working arm mechanism + Action: On button press, pulls arm up and wrist up, ideal for traveling the field when + holding a specimen in claws + */ + public void toArmParkPosition() { + if (this.currentGP2.square && !this.previousGP2.square) { + Actions.runBlocking(new SequentialAction( +// this.wrist.toFloorPosition(), +// this.arm.toParkPosition(), +// this.lift.toFloorPosition() + )); + } + } + + + + + /* + Type: PS4 + Controller: 2 + Button: CROSS / A + Assumption: Working claw, arm and wrist mechanisms + Action: On button press, if arm is in PARK (toArmParkPosition), drop the arm to SUBMARINE + position. SUBMARINE position means the arm and wrist are parallel to the floor, raised + 3 INCHES off the ground. This state is ideal for moving the arm into the SUBMARINE + area of the field. + + When arm is in SUBMARINE position, pressing the button again puts the arm and wrist into + FLOOR state. This angles the arm and wrist down so that it is able to pick specimens + from within the SUBMARINE floor. + */ + public void toArmParkThenSwitchBetweenSubmarineAndFloorPosition() { + if (this.currentGP2.cross && !previousGP2.cross) { +// if (this.arm.getState() == ArmActionsSubsystem.ArmState.PARK) { +// Actions.runBlocking( +// new SequentialAction( +// this.arm.toSubmarinePosition(), +// this.wrist.toFloorPosition() +// ) +// ); +// } else if (this.arm.getState() == ArmActionsSubsystem.ArmState.SUBMARINE && this.wrist.getState() == WristActionsSubsystem.WristState.FLOOR) { +// Actions.runBlocking( +// new SequentialAction( +//// this.lift.toFloorPosition(), +// this.arm.toSubmarinePosition(), +// this.wrist.toPickupPosition() +// ) +// ); +// } else if (this.wrist.getState() == WristActionsSubsystem.WristState.PICKUP) { +// Actions.runBlocking( +// new SequentialAction( +// // this.arm.toSubmarinePosition(), +// this.wrist.toFloorPosition() +// ) +// ); +// } + } + } + + + + + + + + + + +} + + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopDriveV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopDriveV2.java new file mode 100644 index 0000000..a398d3f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopDriveV2.java @@ -0,0 +1,24 @@ +package org.firstinspires.ftc.teamcode.cometbots.paths; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + + +@TeleOp(name = "ComeBot Teleop Drive V2", group = "Competition") +public class CometBotTeleopDriveV2 extends OpMode { + + public CometBotTeleopCompetition runMode; + + @Override + public void init() { + this.runMode = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2); + this.runMode.init(); + + } + + @Override + public void loop() { + this.runMode.update(); + telemetry.update(); + } +} \ No newline at end of file