diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java index 5c4137f..66a55f4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java @@ -26,7 +26,7 @@ public class ArmSubsystem { } public void grabBlueberrySkyhook(){ setState(ArmState.GRAB_BLUEBERRY_SKYHOOK); - arm.setPosition(armGrabBlueberrySkyhook); + arm.setPosition(armGrabBlueberrySkyhook+ 0.3); } public void hangBlueberrySkyhook(){ setState(ArmState.HANG_BLUEBERRY_SKYHOOK); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java index b3e4792..414fdb3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java @@ -15,10 +15,7 @@ public class HangMotorSubsystem { private DcMotorEx hang; public HangMotorSubsystem(HardwareMap hardwareMap) { - hang = hardwareMap.get(DcMotorEx.class, HOOK); - - } public int getCurrentPosition(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HookTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HookTest.java new file mode 100644 index 0000000..20114d6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HookTest.java @@ -0,0 +1,62 @@ +package org.firstinspires.ftc.teamcode.subsystem; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem; + +public class HookTest extends OpMode { + private HangMotorSubsystem skyhook; + private ArmSubsystem arm; + private WristSubsystem wrist; + private ClawSubsystem claw; + private DualMotorSliderSubsystem slides; + @Override + public void init() { + skyhook = new HangMotorSubsystem(hardwareMap); + arm = new ArmSubsystem(hardwareMap); + wrist = new WristSubsystem(hardwareMap); + claw = new ClawSubsystem(hardwareMap); + slides = new DualMotorSliderSubsystem(hardwareMap); + claw.init(); + arm.initTeleOp(); + wrist.initTeleOp(); + slides.init(); + } + + @Override + public void loop() { + telemetry.addData("arm", arm.getState()); + telemetry.addData("wrist", wrist.getState()); + telemetry.update(); + if(gamepad1.a){ + arm.grabBlueberrySkyhook(); + telemetry.update(); + try { + Thread.sleep(500); + } catch (InterruptedException e) { + throw new RuntimeException(e); + } + //claw Open small amount + wrist.grabBlueberrySkyhook(); + //wrist grab in strange way + telemetry.update(); + } + if(gamepad1.b){ + //after confirmed grab, close claw + claw.closeClaw(); + } + if(gamepad1.x){ + //now slap on bar, first wrist, then arm, then claw then driver must drive away + } + if(gamepad1.y){ + //go up + } + + } + +}