diff --git a/TeamCode/src/main/java/opmodes/MainTeleOp.java b/TeamCode/src/main/java/opmodes/MainTeleOp.java index a4a96d6..180136d 100644 --- a/TeamCode/src/main/java/opmodes/MainTeleOp.java +++ b/TeamCode/src/main/java/opmodes/MainTeleOp.java @@ -1,5 +1,8 @@ package opmodes; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_ARM_DELTA; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_LIFT_DELTA; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.OpMode; @@ -9,8 +12,15 @@ import org.firstinspires.ftc.teamcode.hardware.Robot; @TeleOp(name = "MainTeleOp", group = "Main") public class MainTeleOp extends OpMode { - private Robot robot; + private double clawArmPosition = 0; + private boolean screwArmPos = false; + private boolean previousScrewArmToggle = false; + private boolean previousScrewDeposit = false; + private boolean previousScrewIntake = false; + private boolean previousScrewReset = false; + private boolean previousSlideUp = false; + private boolean previousSlideDown = false; @Override public void init() { @@ -20,27 +30,77 @@ public class MainTeleOp extends OpMode { telemetry.addData("Status", "Initialized"); } + // Gamepad 2 - Arm Control + // RT - Hanger Raise + // RB - Hanger Lift Robot + // DPad Left/Right - Gantry Left/Right + // DPad Up/Down - Lift Up/Down @Override public void loop() { + // Drive double x = gamepad1.left_stick_x; double y = -gamepad1.left_stick_y; double z = gamepad1.right_stick_x; - - // Set the drive input this.robot.getDrive().setInput(x, y, z); - // Pickup -// this.robot.getClaw().close(); -// this.robot.getClaw().open(); -// this.robot.getClaw().up(); -// this.robot.getClaw().down(); + + // Claw + boolean openClaw = false; + boolean clawUp = false; + boolean clawDown = false; + if (openClaw) { + this.robot.getClaw().open(); + } else { + this.robot.getClaw().close(); + } + if (clawUp) { + this.clawArmPosition += CLAW_ARM_DELTA; + this.robot.getClaw().setArmPosition(clawArmPosition); + } else if (clawDown) { + this.clawArmPosition -= CLAW_ARM_DELTA; + this.robot.getClaw().setArmPosition(clawArmPosition); + } + + // Robot Lift + boolean raiseRobotLift = false; + boolean liftRobot = false; + if (raiseRobotLift) { + this.robot.getLift().raise(); + } else if (liftRobot) { + this.robot.getLift().lift(); + } // Gantry -// this.robot.getGantry().up(); -// this.robot.getGantry().down(); -// this.robot.getGantry().armIn(); -// this.robot.getGantry().armOut(); -// this.robot.getGantry().intake(); -// this.robot.getGantry().deposit(); + boolean screwArmToggle = false; + boolean screwDeposit = false; + boolean screwIntake = false; + boolean screwReset = false; + boolean slideUp = false; + boolean slideDown = false; + if (!previousScrewArmToggle && screwArmToggle) { + if (screwArmPos) { + this.robot.getGantry().armOut(); + } else { + this.robot.getGantry().armIn(); + } + screwArmPos = !screwArmPos; + } + if (!previousScrewDeposit && screwDeposit) { + this.robot.getGantry().deposit(); + } else if (!previousScrewIntake && screwIntake) { + this.robot.getGantry().intake(); + } else if (screwReset) { + this.robot.getGantry().resetScrew(); + } + if ((!previousSlideUp && slideUp) || (!previousSlideDown && slideDown)) { + int currentPosition = this.robot.getGantry().getSlidePosition(); + this.robot.getGantry().setTarget(currentPosition + GANTRY_LIFT_DELTA); + } + + this.previousSlideUp = slideUp; + this.previousScrewArmToggle = screwArmToggle; + this.previousScrewDeposit = screwDeposit; + this.previousScrewIntake = screwIntake; + this.previousScrewReset = screwReset; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java index 354bde5..f94d4df 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java @@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode.hardware; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_MAX; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_MIN; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_NAME; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_CENTER; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_SCREW_NAME; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_SCREW_POSITIONS; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_X_NAME; @@ -10,9 +11,7 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_X_NA import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; -import lombok.Getter; - -public class Gantry extends Lift { +public class Gantry extends Slide { private final Servo xServo; private final Servo armServo; private final Servo screwServo; @@ -49,4 +48,8 @@ public class Gantry extends Lift { public void deposit() { this.screwServo.setPosition(GANTRY_SCREW_POSITIONS[this.currentScrewIndex--]); } + + public void center() { + this.setX(GANTRY_CENTER); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java index 05aefab..52b0cfe 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java @@ -17,6 +17,9 @@ public class Robot { @Getter private Claw claw; + @Getter + private RobotLift lift; + public Robot(HardwareMap hardwareMap) { this.init(hardwareMap); } @@ -25,5 +28,6 @@ public class Robot { this.drive = new MecanumDrive(hardwareMap); this.gantry = new Gantry(hardwareMap); this.claw = new Claw(hardwareMap); + this.lift = new RobotLift(hardwareMap); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java index 42ef648..0a6b3dd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java @@ -4,25 +4,34 @@ import com.acmerobotics.dashboard.config.Config; @Config public class RobotConstants { - public static final String LIFT_MOTOR_NAME = "lift"; + public static final String SLIDE_MOTOR_NAME = "slide"; public static final String CLAW_ARM_LEFT_NAME = "clawArmLeft"; public static final String CLAW_ARM_RIGHT_NAME = "clawArmRight"; public static final String CLAW_NAME = "claw"; public static final String GANTRY_X_NAME = "gantryX"; public static final String GANTRY_ARM_NAME = "gantryArm"; public static final String GANTRY_SCREW_NAME = "screw"; + public static final String ROBOT_LIFT_ROTATION_NAME = "liftRotation"; + public static final String ROBOT_LIFT_LIFT_NAME = "liftLift"; - // Lift - public static int LIFT_UP = 100; + // Slide + public static int SLIDE_UP = 100; // Arm public static double PICKUP_ARM_MIN = 0; public static double PICKUP_ARM_MAX = 1; public static double CLAW_MIN = 0; public static double CLAW_MAX = 1; + public static double CLAW_ARM_DELTA = 0.05; // Gantry public static double GANTRY_ARM_MIN = 0; public static double GANTRY_ARM_MAX = 0; public static double[] GANTRY_SCREW_POSITIONS = new double[] { 0, 0.1, 0.2, 1.0 }; + public static int GANTRY_LIFT_DELTA = 50; + public static double GANTRY_CENTER = 0.5; + + // Robot Lift + public static int LIFT_ROTATION_UP = 100; + public static int LIFT_EXTEND_MAX = 100; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotLift.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotLift.java new file mode 100644 index 0000000..34a489b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotLift.java @@ -0,0 +1,51 @@ +package org.firstinspires.ftc.teamcode.hardware; + +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_EXTEND_MAX; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_ROTATION_UP; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.ROBOT_LIFT_LIFT_NAME; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.ROBOT_LIFT_ROTATION_NAME; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; + +public class RobotLift { + private DcMotor rotation; + private DcMotor lift; + + public RobotLift(HardwareMap hardwareMap) { + this.init(hardwareMap); + } + public void init(HardwareMap hardwareMap) { + this.rotation = hardwareMap.get(DcMotor.class, ROBOT_LIFT_ROTATION_NAME); + this.rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + this.lift = hardwareMap.get(DcMotor.class, ROBOT_LIFT_LIFT_NAME); + this.lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.lift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + + public void raise() { + this.rotation.setTargetPosition(LIFT_ROTATION_UP); + this.lift.setTargetPosition(LIFT_EXTEND_MAX); + + this.rotation.setPower(1); + this.lift.setPower(1); + } + + public void lift() { + this.rotation.setTargetPosition(LIFT_ROTATION_UP); + this.lift.setTargetPosition(0); + + this.lift.setPower(1); + this.rotation.setPower(1); + } + + public void reset() { + this.rotation.setTargetPosition(0); + this.lift.setTargetPosition(0); + + this.lift.setPower(0.25); + this.rotation.setPower(0.25); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Lift.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slide.java similarity index 54% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Lift.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slide.java index b4c4a17..07326d6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Lift.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slide.java @@ -1,33 +1,30 @@ package org.firstinspires.ftc.teamcode.hardware; -import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_MOTOR_NAME; -import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_UP; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.SLIDE_MOTOR_NAME; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; -import lombok.Getter; - -public class Lift { +public class Slide { protected final DcMotor lift; - protected Lift(HardwareMap hardwareMap) { - this.lift = hardwareMap.get(DcMotor.class, LIFT_MOTOR_NAME); + protected Slide(HardwareMap hardwareMap) { + this.lift = hardwareMap.get(DcMotor.class, SLIDE_MOTOR_NAME); this.lift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); this.lift.setMode(DcMotor.RunMode.RUN_USING_ENCODER); } - public void up() { + public void setTarget(int target) { this.lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); - this.lift.setTargetPosition(LIFT_UP); - } - - public void down() { - this.lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); - this.lift.setTargetPosition(0); + this.lift.setTargetPosition(target); + this.lift.setPower(1); } public void setInput(double x) { this.lift.setPower(x); } + + public int getSlidePosition() { + return this.lift.getCurrentPosition(); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java index 9a4afca..c75ec03 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java @@ -35,20 +35,4 @@ public class Constants { public static final Point INVALID_POINT = new Point(Double.MIN_VALUE, Double.MIN_VALUE); public static final double INVALID_AREA = -1; public static final Detection INVALID_DETECTION = new Detection(new Size(0, 0), 0); - - // Hardware Name Constants - public static final String WHEEL_FRONT_LEFT = "frontLeft"; - public static final String WHEEL_FRONT_RIGHT = "frontRight"; - public static final String WHEEL_BACK_LEFT = "backLeft"; - public static final String WHEEL_BACK_RIGHT = "backRight"; - public static final String ARM = "wobbler"; - public static final String CLAW = "claw"; - public static final String INTAKE = "intake"; - public static final String INTAKE_SECONDARY = "secondary"; - public static final String INTAKE_SHIELD = "shield"; - public static final String SHOOTER = "wheel"; - public static final String PUSHER = "pusher"; - public static final String STACK_WEBCAM = "Stack Webcam"; - public static final String TARGETING_WEBCAM = "Targeting Webcam"; - public static final String IMU_SENSOR = "imu"; } \ No newline at end of file