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 d7b4979..bee1a42 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java @@ -45,7 +45,7 @@ public class DevTeleop extends OpMode { public DcMotor frontRightMotor; public DcMotor backRightMotor; - private double MAX_POWER = .55; + private double MAX_POWER = .45; @Override public void init() { claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED); @@ -74,22 +74,24 @@ public class DevTeleop extends OpMode { } public void theDrop(ArmSubsystem arm, WristSubsystem wrist) { - - if (currentGamepad1.a && !previousGamepad1.a) { +//pick up + if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) { wrist.floorWrist(); arm.engageArm(); } + + } public void thePickup(ClawSubsystem claw) { - - if (currentGamepad1.x && !previousGamepad1.x) { +//claw open close + if (currentGamepad2.right_bumper && !previousGamepad2.right_bumper) { claw.switchState(); } } - public void theLift(ArmSubsystem arm, WristSubsystem wrist) { + /* public void theLift(ArmSubsystem arm, WristSubsystem wrist) { if (currentGamepad1.b && !previousGamepad1.b) { arm.parkArm(); @@ -97,22 +99,44 @@ public class DevTeleop extends OpMode { } } - +*/ public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) { - if (currentGamepad2.y && !previousGamepad2.y) { +//low bucket + if (currentGamepad2.a && !previousGamepad2.a) { lift.toLowBucket(); arm.bucketArm(); wrist.bucketWrist(); } } - public void theHighBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) { - if (currentGamepad2.a && !previousGamepad2.a) { + public void theHighBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) { +//high basket + if (currentGamepad2.b && !previousGamepad2.b) { lift.toHighBucket(); arm.bucketArm(); wrist.bucketWrist(); } } + + public void theTravel(LiftSubsystem lift, ArmSubsystem arm, WristSubsystem wrist){ +// + if (currentGamepad2.dpad_right && !previousGamepad2.dpad_right){ + lift.toFloor(); + arm.bucketArm(); + wrist.floorWrist(); + } + } + + public void hoverState(ArmSubsystem arm, WristSubsystem wrist, LiftSubsystem lift){ + if (currentGamepad1.dpad_left && !previousGamepad2.dpad_left){ + lift.toHover(); + wrist.floorWrist(); + arm.engageArm(); + } + + + } + @Override public void loop() { previousGamepad1.copy(currentGamepad1); @@ -123,9 +147,10 @@ public class DevTeleop extends OpMode { theDrop(arm, wrist); thePickup(claw); - theLift(arm, wrist); + // theLift(arm, wrist); theLowBucketScore(lift, wrist, arm); theHighBucketScore(lift, wrist, arm); + theTravel(lift, arm, wrist); double max; @@ -164,6 +189,7 @@ public class DevTeleop extends OpMode { // 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.addData("Current Lift Position", lift.getPosition()); telemetry.update(); } 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 d6591c8..4f2f6ed 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 @@ -5,7 +5,7 @@ import com.acmerobotics.dashboard.config.Config; @Config public class RobotConstants { public static double clawClose = 1.00; - public static double clawOpen = 0.25; + public static double clawOpen = 0.05; public static double armEngage = 0.5; public static double armPark = 0.125; @@ -18,4 +18,5 @@ public class RobotConstants { public static int liftToLowBucketPos = 2250; public static int liftToHighBucketPos = 3900; public static double liftPower = .45; + public static int liftToHoverState = 60; } \ No newline at end of file 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 ed54f00..635bac2 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 @@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloatP import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloorPos; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos; import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos; +import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHoverState; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; @@ -18,7 +19,7 @@ public class LiftSubsystem { public RunAction toFloor, toLowBucket, toHighBucket; public enum LiftState { - FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT + FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT, HOVER } private LiftState liftState; @@ -47,6 +48,11 @@ public class LiftSubsystem { } } + public void toHover() { + this.setTarget(liftToHoverState); + this.setState(LiftState.HOVER); + } + public void toFloor() { this.setTarget(liftToFloorPos); this.setState(LiftState.FLOOR);