diff --git a/TeamCode/src/main/java/opmodes/MainTeleOp.java b/TeamCode/src/main/java/opmodes/MainTeleOp.java index ef6ae0e..7ff6a09 100644 --- a/TeamCode/src/main/java/opmodes/MainTeleOp.java +++ b/TeamCode/src/main/java/opmodes/MainTeleOp.java @@ -2,12 +2,15 @@ package opmodes; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_ARM_DELTA; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PICKUP_ARM_MAX; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_MAX; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_MIN; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.hardware.Robot; @TeleOp(name = "MainTeleOp", group = "Main") @@ -22,10 +25,11 @@ public class MainTeleOp extends OpMode { private boolean previousRobotLiftExtend = false; private boolean liftArmShouldBeUp = false; private boolean screwArmIsMoving = false; + private Telemetry dashboard; @Override public void init() { - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + this.dashboard = FtcDashboard.getInstance().getTelemetry(); this.clawArmPosition = PICKUP_ARM_MAX; this.robot = new Robot(this.hardwareMap, telemetry); @@ -35,18 +39,22 @@ public class MainTeleOp extends OpMode { @Override public void loop() { // Drive - boolean slowmode = gamepad1.right_bumper; + boolean slowmode = gamepad1.right_bumper || gamepad1.y; this.robot.getDrive().setInput(gamepad1, gamepad2, slowmode); // Button Mappings + // Claw / Pickup boolean openClaw = gamepad2.b; // B boolean clawUp = gamepad2.y; // Y - boolean clawDown = gamepad2.a; // A + boolean clawDownSafe = gamepad2.dpad_down; // dpad-down + boolean clawDown = gamepad2.a || clawDownSafe; // A + // Robot Lift boolean robotLiftRotation = gamepad2.right_trigger > 0.05; // RT boolean robotLiftExtend = gamepad2.right_trigger > 0.5; // RT boolean robotLiftReset = gamepad2.right_stick_button; + // Gantry boolean screwArmToggle = gamepad2.x; // X boolean screwDeposit = gamepad2.left_trigger > 0.25; // LT boolean screwIntake = gamepad2.left_bumper; // LB @@ -68,7 +76,9 @@ public class MainTeleOp extends OpMode { this.robot.getClaw().setArmPosition(clawArmPosition); } else if (clawDown) { this.screwArmIsMoving = false; - this.robot.getClaw().open(); + if (!clawDownSafe) { + this.robot.getClaw().open(); + } this.clawArmPosition = Math.max(0, this.clawArmPosition - CLAW_ARM_DELTA); this.robot.getClaw().setArmPosition(clawArmPosition); } @@ -97,11 +107,13 @@ public class MainTeleOp extends OpMode { // this.robot.getGantry().setTarget(currentPosition + GANTRY_LIFT_DELTA); // } // -// if (gantryLeft) { -// this.robot.getGantry().setX(this.robot.getGantry().getX() + GANTRY_X_DELTA); -// } else if (gantryRight) { -// this.robot.getGantry().setX(this.robot.getGantry().getX() - GANTRY_X_DELTA); -// } + if (gantryRight) { + this.robot.getGantry().setX(X_MIN); + } else if (gantryLeft) { + this.robot.getGantry().setX(X_MAX); + } else { + this.robot.getGantry().center(); + } // Robot Lift diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java index b4112c1..f97d1be 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java @@ -13,10 +13,13 @@ import com.arcrobotics.ftclib.controller.PController; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; -public class Claw { +import org.firstinspires.ftc.robotcore.external.Telemetry; + +public class Claw implements Updatable { private final Servo claw; private final Servo armLeft; private final Servo armRight; + private Telemetry telemetry; PController clawController = new PController(CLAW_KP); private double clawControllerTarget; @@ -29,6 +32,11 @@ public class Claw { this.close(); } + public Claw(HardwareMap hardwareMap, Telemetry telemetry) { + this(hardwareMap); + this.telemetry = telemetry; + } + public void open() { this.clawControllerTarget = CLAW_MAX; } 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 85c07f9..62c2b59 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 @@ -5,33 +5,51 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_ 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.X_CENTER; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_SCREW_NAME; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_X_NAME; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LEFT_SLIDE_MOTOR_NAME; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.RIGHT_SLIDE_MOTOR_NAME; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_KP; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_MAX; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_MIN; import com.arcrobotics.ftclib.controller.PController; import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.robotcore.external.Telemetry; -public class Gantry extends Slide { +public class Gantry { private final Servo xServo; private final Servo armServo; private final CRServo screwServo; + private final DcMotor liftLeft; + private final DcMotor right; PController armController = new PController(GANTRY_ARM_KP); private double armControllerTarget; - + PController xController = new PController(X_KP); + private double xControllerTarget; private Telemetry telemetry; public Gantry(HardwareMap hardwareMap) { - super(hardwareMap); this.xServo = hardwareMap.get(Servo.class, GANTRY_X_NAME); this.armServo = hardwareMap.get(Servo.class, GANTRY_ARM_NAME); this.screwServo = hardwareMap.get(CRServo.class, GANTRY_SCREW_NAME); this.armServo.setPosition(GANTRY_ARM_MIN); + + this.liftLeft = hardwareMap.get(DcMotor.class, LEFT_SLIDE_MOTOR_NAME); + this.liftLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + this.liftLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + this.right = hardwareMap.get(DcMotor.class, RIGHT_SLIDE_MOTOR_NAME); + this.right.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + this.right.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + this.xControllerTarget = X_MIN; } public Gantry(HardwareMap hardwareMap, Telemetry telemetry) { @@ -39,8 +57,19 @@ public class Gantry extends Slide { this.telemetry = telemetry; } - public void setX(double x) { - this.xServo.setPosition(x); + public void setSlideTarget(int target) { + this.liftLeft.setTargetPosition(target); + this.liftLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.liftLeft.setPower(1); + + this.right.setTargetPosition(target); + this.right.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.right.setPower(1); + } + + public void setX(double x) + { + this.xControllerTarget = Math.max(X_MIN, Math.min(x, X_MAX)); } public double getX() { @@ -70,7 +99,7 @@ public class Gantry extends Slide { } public void center() { - this.setX(GANTRY_CENTER); + this.setX(X_CENTER); } public boolean isIn() { @@ -83,14 +112,20 @@ public class Gantry extends Slide { this.armController.setTolerance(0.001); this.armController.setP(GANTRY_ARM_KP); - double output = 0; + this.xController.setSetPoint(this.xControllerTarget); + this.xController.setTolerance(0.001); + this.xController.setP(X_KP); + + double armOutput = 0; if (!this.armController.atSetPoint()) { - output = Math.max(-1 * GANTRY_ARM_DELTA_MAX, Math.min(GANTRY_ARM_DELTA_MAX, this.armController.calculate(armServo.getPosition()))); - this.armServo.setPosition(this.armServo.getPosition() + output); + armOutput = Math.max(-1 * GANTRY_ARM_DELTA_MAX, Math.min(GANTRY_ARM_DELTA_MAX, this.armController.calculate(armServo.getPosition()))); + this.armServo.setPosition(this.armServo.getPosition() + armOutput); } - this.telemetry.addData("Arm P Controller", output); - this.telemetry.addData("Arm P Setpoint", this.armControllerTarget); - this.telemetry.addData("Arm In", this.isIn()); + double xOutput = 0; + if (!this.xController.atSetPoint()) { + xOutput = this.xController.calculate(this.xServo.getPosition()); + this.xServo.setPosition(this.xServo.getPosition() + xOutput); + } } } 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 affa31e..d799b15 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 @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.hardware; import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -22,7 +23,7 @@ public class Robot { @Getter private RobotLift lift; - private Telemetry telemetry; + private final Telemetry telemetry; public Robot(HardwareMap hardwareMap, Telemetry telemetry) { this.telemetry = telemetry; @@ -31,20 +32,18 @@ public class Robot { private void init(HardwareMap hardwareMap) { this.drive = new MecanumDrive(hardwareMap); + this.drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); this.gantry = new Gantry(hardwareMap, telemetry); - this.claw = new Claw(hardwareMap); + this.claw = new Claw(hardwareMap, telemetry); this.lift = new RobotLift(hardwareMap, telemetry); } public void update() { this.gantry.update(); this.lift.update(); - this.telemetry.update(); this.drive.update(); this.claw.update(); -// Pose2d pose = this.drive.getLocalizer().getPoseEstimate(); -// this.telemetry.addData("x", pose.getX()); -// this.telemetry.addData("y", pose.getY()); + this.telemetry.update(); } } 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 73b7e4c..40c40f6 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 @@ -13,14 +13,14 @@ public class RobotConstants { 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_X_NAME = "gantry_x"; 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"; public static final String WEBCAM_NAME = "webcam"; public static final String PARALLEL_DRIVE_ODOMETRY = BACK_RIGHT_NAME; - public static final String PERPENDICULAR_DRIVE_ODOMETRY = FRONT_LEFT_NAME; + public static final String PERPENDICULAR_DRIVE_ODOMETRY = FRONT_RIGHT_NAME; // Drive public static double SLOW_MODE_SPEED_PCT = 0.25; @@ -29,31 +29,32 @@ public class RobotConstants { public static double TURN = 1f; // Slide - public static int SLIDE_UP = 100; // Arm public static double PICKUP_ARM_MIN = 0.185; - public static double PICKUP_ARM_MAX = 0.76; + public static double PICKUP_ARM_MAX = 0.755; public static double CLAW_MIN = 0.92; public static double CLAW_MAX = 0.6; public static double CLAW_ARM_DELTA = 0.03; // Gantry public static double GANTRY_ARM_MIN = 0.435; - public static double GANTRY_ARM_MAX = 0.92; + public static double GANTRY_ARM_MAX = 0.94; public static int GANTRY_LIFT_DELTA = 50; - public static double GANTRY_X_DELTA = 0.01; - public static double GANTRY_CENTER = 0.5; public static double GANTRY_ARM_KP = 0.06; - public static double GANTRY_ARM_KI = 0f; - public static double GANTRY_ARM_KD = 0f; + public static double X_KP = 0.1; + public static double X_MAX = 0.84; + public static double X_MIN = 0.16; + public static double X_CENTER = 0.54; public static double GANTRY_ARM_DELTA_MAX = 0.013; + public static int SLIDE_UP = 100; + // Robot Lift public static double LIFT_ROTATION_UP = 0.4; public static double LIFT_ROTATION_DOWN = 0; public static int LIFT_EXTEND_MAX = 13100; - public static double LIFT_RETRACT_PCT = 0.3; + public static double LIFT_RETRACT_PCT = 0.4; public static double LIFT_ARM_KP = 0.1; public static double LIFT_POWER = 1f; 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 index 7735619..afce43f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotLift.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotLift.java @@ -15,7 +15,7 @@ import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.robotcore.external.Telemetry; -public class RobotLift { +public class RobotLift implements Updatable{ private Servo rotation; private DcMotor lift; PController armController = new PController(LIFT_ARM_KP); @@ -78,8 +78,6 @@ public class RobotLift { this.armController.setSetPoint(this.armControllerTarget); double output = this.armController.calculate(this.rotation.getPosition()); this.rotation.setPosition(this.rotation.getPosition() + output); - - this.telemetry.addData("Lift P Controller", output); } public boolean isUp() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slide.java deleted file mode 100644 index 4734d7d..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slide.java +++ /dev/null @@ -1,40 +0,0 @@ -package org.firstinspires.ftc.teamcode.hardware; - -import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LEFT_SLIDE_MOTOR_NAME; -import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.RIGHT_SLIDE_MOTOR_NAME; - -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.HardwareMap; - -public class Slide { - protected final DcMotor left; - protected final DcMotor right; - - protected Slide(HardwareMap hardwareMap) { - this.left = hardwareMap.get(DcMotor.class, LEFT_SLIDE_MOTOR_NAME); - this.left.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - this.left.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - this.right = hardwareMap.get(DcMotor.class, RIGHT_SLIDE_MOTOR_NAME); - this.right.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - this.right.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - } - - public void setTarget(int target) { - this.left.setTargetPosition(target); - this.left.setMode(DcMotor.RunMode.RUN_TO_POSITION); - this.left.setPower(1); - - this.right.setTargetPosition(target); - this.right.setMode(DcMotor.RunMode.RUN_TO_POSITION); - this.right.setPower(1); - } - - public void setInput(double x) { - - } - - public int getSlidePosition() { - return this.left.getCurrentPosition(); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Updatable.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Updatable.java new file mode 100644 index 0000000..144035b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Updatable.java @@ -0,0 +1,5 @@ +package org.firstinspires.ftc.teamcode.hardware; + +public interface Updatable { + void update(); +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java index b3277de..671234e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java @@ -41,11 +41,11 @@ public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer { public static double WHEEL_RADIUS = 0.944882; // in public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed - public static double PARALLEL_X = 0.269685; // X is the up and down direction - public static double PARALLEL_Y = 4.409449; // Y is the strafe direction + public static double PARALLEL_X = -0.269685; // X is the up and down direction + public static double PARALLEL_Y = -4.409449; // Y is the strafe direction - public static double PERPENDICULAR_X = -0.6299213; - public static double PERPENDICULAR_Y = 0.1122047; + public static double PERPENDICULAR_X = 0.6299213; + public static double PERPENDICULAR_Y = -0.1122047; public static double X_MULTIPLIER = 1; // Multiplier in the X direction public static double Y_MULTIPLIER = 1; // Multiplier in the Y direction @@ -69,7 +69,7 @@ public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer { perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, PERPENDICULAR_DRIVE_ODOMETRY)); // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) - parallelEncoder.setDirection(Encoder.Direction.REVERSE); + parallelEncoder.setDirection(Encoder.Direction.FORWARD); } public static double encoderTicksToInches(double ticks) {