From bd0f1305ce7e4fc4c2dbc17c3f6af3def8ade4b2 Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Thu, 9 Nov 2023 20:05:31 -0600 Subject: [PATCH] Drive still needs work --- TeamCode/build.gradle | 1 + .../src/main/java/opmodes/MainTeleOp.java | 98 ++++++++++++------- .../ftc/teamcode/hardware/Claw.java | 11 +-- .../ftc/teamcode/hardware/Gantry.java | 64 +++++++++--- .../ftc/teamcode/hardware/Robot.java | 16 ++- .../ftc/teamcode/hardware/RobotConstants.java | 25 +++-- .../ftc/teamcode/hardware/RobotLift.java | 69 ++++++++++--- .../ftc/teamcode/hardware/Slide.java | 4 +- build.common.gradle | 5 +- 9 files changed, 204 insertions(+), 89 deletions(-) diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 442471b..81297b0 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -30,4 +30,5 @@ dependencies { implementation 'org.apache.commons:commons-math3:3.6.1' implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7' implementation 'com.acmerobotics.roadrunner:core:0.5.6' + implementation 'org.ftclib.ftclib:core:2.1.1' // core } diff --git a/TeamCode/src/main/java/opmodes/MainTeleOp.java b/TeamCode/src/main/java/opmodes/MainTeleOp.java index bb1dd5b..24f7a9d 100644 --- a/TeamCode/src/main/java/opmodes/MainTeleOp.java +++ b/TeamCode/src/main/java/opmodes/MainTeleOp.java @@ -1,9 +1,6 @@ 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 static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_SCREW_DELTA; -import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_X_DELTA; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PICKUP_ARM_MAX; import com.acmerobotics.dashboard.FtcDashboard; @@ -19,18 +16,19 @@ public class MainTeleOp extends OpMode { private double clawArmPosition = 0; private boolean screwArmPos = false; private boolean previousScrewArmToggle = false; - private boolean previousScrewDeposit = false; - private boolean previousScrewReset = false; private boolean previousSlideUp = false; private boolean previousSlideDown = false; - private double currentScrewPosition = 1f; + private boolean previousRobotLiftReset = false; + private boolean previousRobotLiftExtend = false; + private boolean liftArmShouldBeUp = false; + private boolean screwArmIsMoving = false; @Override public void init() { telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); this.clawArmPosition = PICKUP_ARM_MAX; - this.robot = new Robot(this.hardwareMap); + this.robot = new Robot(this.hardwareMap, telemetry); telemetry.addData("Status", "Initialized"); } @@ -50,12 +48,13 @@ public class MainTeleOp extends OpMode { boolean clawUp = gamepad2.y; // Y boolean clawDown = gamepad2.a; // A - boolean raiseRobotLift = gamepad2.right_trigger > 0.25; // RT - boolean liftRobot = gamepad2.right_bumper; // RB + boolean robotLiftRotation = gamepad2.right_trigger > 0.05; // RT + boolean robotLiftExtend = gamepad2.right_trigger > 0.5; // RT + boolean robotLiftReset = gamepad2.right_stick_button; boolean screwArmToggle = gamepad2.x; // X boolean screwDeposit = gamepad2.left_trigger > 0.25; // LT - boolean screwReset = gamepad2.left_bumper; // LB + boolean screwIntake = gamepad2.left_bumper; // LB boolean slideUp = gamepad2.left_stick_y < -0.25; // Left Y Axis Joystick boolean slideDown = gamepad2.left_stick_y > 0.25; // Left Y Axis Joystick boolean gantryLeft = gamepad2.dpad_left; // dpad left @@ -64,56 +63,81 @@ public class MainTeleOp extends OpMode { // Claw if (openClaw) { this.robot.getClaw().open(); - } else if (!clawUp && !clawDown){ + this.screwArmIsMoving = false; + } else if (!clawUp && !clawDown && !this.screwArmIsMoving){ this.robot.getClaw().close(); } if (clawUp) { + this.screwArmIsMoving = false; this.clawArmPosition = Math.min(1, this.clawArmPosition + CLAW_ARM_DELTA); this.robot.getClaw().setArmPosition(clawArmPosition); } else if (clawDown) { + this.screwArmIsMoving = false; this.robot.getClaw().open(); this.clawArmPosition = Math.max(0, this.clawArmPosition - CLAW_ARM_DELTA); this.robot.getClaw().setArmPosition(clawArmPosition); } -/* - // Gantry if (!previousScrewArmToggle && screwArmToggle) { + this.screwArmIsMoving = true; if (screwArmPos) { - this.robot.getGantry().armOut(); - } else { this.robot.getGantry().armIn(); + } else { + this.robot.getClaw().open(); + this.robot.getGantry().armOut(); } + this.robot.getClaw().open(); screwArmPos = !screwArmPos; } - if (!previousScrewDeposit && screwDeposit) { - this.currentScrewPosition += GANTRY_SCREW_DELTA; - this.robot.getGantry().setScrew(currentScrewPosition); - } else if (!previousScrewReset && screwReset) { - this.robot.getGantry().resetScrew(); - } - if ((!previousSlideUp && slideUp) || (!previousSlideDown && slideDown)) { - int currentPosition = this.robot.getGantry().getSlidePosition(); - 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 (screwDeposit) { + this.robot.getGantry().deposit(); + } else if (screwIntake) { + this.robot.getGantry().intake(); + } else { + this.robot.getGantry().stop(); } +// if ((!previousSlideUp && slideUp) || (!previousSlideDown && slideDown)) { +// int currentPosition = this.robot.getGantry().getSlidePosition(); +// 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); +// } // Robot Lift - if (raiseRobotLift) { - this.robot.getLift().raise(); - } else if (liftRobot) { - this.robot.getLift().lift(); + + if (robotLiftRotation || this.liftArmShouldBeUp) { + this.liftArmShouldBeUp = true; + if (this.robot.getGantry().isIn()) { + this.robot.getGantry().armOut(); + } else { + if (robotLiftExtend) { + this.robot.getLift().extend(); + } + this.robot.getLift().up(); + } } - */ + + if (!robotLiftExtend && previousRobotLiftExtend) { + this.robot.getLift().retract(); + } + + if (robotLiftReset) { + this.robot.getLift().startReset(); + } else if (previousRobotLiftReset) { + this.liftArmShouldBeUp = false; + this.robot.getLift().stopReset(); + } + + this.robot.update(); + this.previousSlideUp = slideUp; this.previousScrewArmToggle = screwArmToggle; - this.previousScrewDeposit = screwDeposit; - this.previousScrewReset = screwReset; - + this.previousRobotLiftReset = robotLiftReset; + this.previousRobotLiftExtend = robotLiftExtend; } } 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 ca299a2..e978035 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 @@ -8,9 +8,12 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_MAX; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_MIN; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_NAME; +import com.arcrobotics.ftclib.controller.PController; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; +import org.firstinspires.ftc.robotcore.external.Telemetry; + public class Claw { private final Servo claw; private final Servo armLeft; @@ -33,14 +36,6 @@ public class Claw { this.claw.setPosition(CLAW_MIN); } - public void up() { - this.setArmPosition(PICKUP_ARM_MAX); - } - - public void down() { - this.setArmPosition(PICKUP_ARM_MIN); - } - public void setArmPosition(double target) { target = Math.min(PICKUP_ARM_MAX, Math.max(PICKUP_ARM_MIN, target)); this.armLeft.setPosition(target); 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 d5adaad..85c07f9 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 @@ -1,28 +1,42 @@ package org.firstinspires.ftc.teamcode.hardware; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_DELTA_MAX; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_KP; 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; +import com.arcrobotics.ftclib.controller.PController; +import com.qualcomm.robotcore.hardware.CRServo; +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 { private final Servo xServo; private final Servo armServo; - private final Servo screwServo; - private int currentScrewIndex = 0; + private final CRServo screwServo; + PController armController = new PController(GANTRY_ARM_KP); + private double armControllerTarget; + + 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(Servo.class, GANTRY_SCREW_NAME); - this.screwServo.setPosition(1); + this.screwServo = hardwareMap.get(CRServo.class, GANTRY_SCREW_NAME); + this.armServo.setPosition(GANTRY_ARM_MIN); + } + + public Gantry(HardwareMap hardwareMap, Telemetry telemetry) { + this(hardwareMap); + this.telemetry = telemetry; } public void setX(double x) { @@ -34,23 +48,49 @@ public class Gantry extends Slide { } public void armOut() { - this.armServo.setPosition(GANTRY_ARM_MAX); + this.armControllerTarget = GANTRY_ARM_MAX; } public void armIn() { - this.armServo.setPosition(GANTRY_ARM_MIN); + this.armControllerTarget = GANTRY_ARM_MIN; } - public void resetScrew() { - this.screwServo.setPosition(0); + public void intake() { + this.screwServo.setPower(1); + this.screwServo.setDirection(DcMotorSimple.Direction.FORWARD); } - public void setScrew(double target) { - double clamped = Math.min(1, Math.max(0, target)); - this.screwServo.setPosition(clamped); + public void deposit() { + this.screwServo.setPower(1); + this.screwServo.setDirection(DcMotorSimple.Direction.REVERSE); + } + + public void stop() { + this.screwServo.setPower(0); } public void center() { this.setX(GANTRY_CENTER); } + + public boolean isIn() { + double fudge = (GANTRY_ARM_MAX - GANTRY_ARM_MIN) * .75; + return this.armServo.getPosition() < GANTRY_ARM_MIN + fudge; + } + + public void update() { + this.armController.setSetPoint(this.armControllerTarget); + this.armController.setTolerance(0.001); + this.armController.setP(GANTRY_ARM_KP); + + double output = 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); + } + + this.telemetry.addData("Arm P Controller", output); + this.telemetry.addData("Arm P Setpoint", this.armControllerTarget); + this.telemetry.addData("Arm In", this.isIn()); + } } 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 22c2aef..caf6a7b 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 @@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.hardware; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; import lombok.Getter; @@ -23,15 +24,24 @@ public class Robot { @Getter private RobotLift lift; - public Robot(HardwareMap hardwareMap) { + private Telemetry telemetry; + + public Robot(HardwareMap hardwareMap, Telemetry telemetry) { + this.telemetry = telemetry; this.init(hardwareMap); } private void init(HardwareMap hardwareMap) { // this.drive = new MecanumDrive(hardwareMap); this.drive = new Drive(hardwareMap); -// this.gantry = new Gantry(hardwareMap); + this.gantry = new Gantry(hardwareMap, telemetry); this.claw = new Claw(hardwareMap); -// this.lift = new RobotLift(hardwareMap); + this.lift = new RobotLift(hardwareMap, telemetry); + } + + public void update() { + this.gantry.update(); + this.lift.update(); + 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 8ee0d15..1754fb2 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 @@ -24,22 +24,29 @@ public class RobotConstants { public static int SLIDE_UP = 100; // Arm - public static double PICKUP_ARM_MIN = 0.19; - public static double PICKUP_ARM_MAX = 0.75; + public static double PICKUP_ARM_MIN = 0.17; + public static double PICKUP_ARM_MAX = 0.76; public static double CLAW_MIN = 0.9; public static double CLAW_MAX = 0.6; - public static double CLAW_ARM_DELTA = 0.005; + public static double CLAW_ARM_DELTA = 0.008; // 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 double GANTRY_ARM_MIN = 0.43; + public static double GANTRY_ARM_MAX = 0.95; 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_SCREW_DELTA = 0.01; + public static double GANTRY_ARM_KP = 0.03; + public static double GANTRY_ARM_KI = 0f; + public static double GANTRY_ARM_KD = 0f; + public static double GANTRY_ARM_DELTA_MAX = 0.0025; // Robot Lift - public static int LIFT_ROTATION_UP = 100; - public static int LIFT_EXTEND_MAX = 100; + 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.1; + public static double LIFT_ARM_KP = 0.038; + 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 34a489b..7735619 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 @@ -1,51 +1,88 @@ package org.firstinspires.ftc.teamcode.hardware; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_ARM_KP; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_EXTEND_MAX; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_RETRACT_PCT; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_ROTATION_DOWN; 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.arcrobotics.ftclib.controller.PController; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.Telemetry; public class RobotLift { - private DcMotor rotation; + private Servo rotation; private DcMotor lift; + PController armController = new PController(LIFT_ARM_KP); + private double armControllerTarget; + private Telemetry telemetry; public RobotLift(HardwareMap hardwareMap) { this.init(hardwareMap); } + + public RobotLift(HardwareMap hardwareMap, Telemetry telemetry) { + this(hardwareMap); + this.telemetry = telemetry; + } + 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.rotation = hardwareMap.get(Servo.class, ROBOT_LIFT_ROTATION_NAME); this.lift = hardwareMap.get(DcMotor.class, ROBOT_LIFT_LIFT_NAME); + this.lift.setTargetPosition(0); this.lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); this.lift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); } - public void raise() { - this.rotation.setTargetPosition(LIFT_ROTATION_UP); + public void up() { + this.armControllerTarget = LIFT_ROTATION_UP; + } + + public void extend() { + this.armControllerTarget = 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); + public void retract() { + this.armControllerTarget = LIFT_ROTATION_UP; + int liftTarget = (int)(LIFT_EXTEND_MAX * (1 - LIFT_RETRACT_PCT)); + int target = this.lift.getCurrentPosition() < liftTarget ? 0 : liftTarget; + this.lift.setTargetPosition(target); this.lift.setPower(1); - this.rotation.setPower(1); } - public void reset() { - this.rotation.setTargetPosition(0); - this.lift.setTargetPosition(0); + public void startReset() { + this.armControllerTarget = LIFT_ROTATION_DOWN; + this.lift.setTargetPosition(-1 * LIFT_EXTEND_MAX); + this.lift.setPower(1); + } - this.lift.setPower(0.25); - this.rotation.setPower(0.25); + public void stopReset() { + this.armControllerTarget = LIFT_ROTATION_DOWN; + this.lift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + this.lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.lift.setPower(0); + } + + public void update() { + this.armController.setP(LIFT_ARM_KP); + 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() { + return this.armControllerTarget == LIFT_ROTATION_UP; } } \ No newline at end of file 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 index 458e930..4734d7d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slide.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slide.java @@ -21,12 +21,12 @@ public class Slide { } public void setTarget(int target) { - this.left.setMode(DcMotor.RunMode.RUN_TO_POSITION); this.left.setTargetPosition(target); + this.left.setMode(DcMotor.RunMode.RUN_TO_POSITION); this.left.setPower(1); - this.right.setMode(DcMotor.RunMode.RUN_TO_POSITION); this.right.setTargetPosition(target); + this.right.setMode(DcMotor.RunMode.RUN_TO_POSITION); this.right.setPower(1); } diff --git a/build.common.gradle b/build.common.gradle index 12e6acb..e004d35 100644 --- a/build.common.gradle +++ b/build.common.gradle @@ -57,6 +57,7 @@ android { minSdkVersion 24 //noinspection ExpiredTargetSdkVersion targetSdkVersion 28 + multiDexEnabled true /** * We keep the versionCode and versionName of robot controller applications in sync with @@ -93,14 +94,14 @@ android { signingConfig signingConfigs.release ndk { - abiFilters "armeabi-v7a", "arm64-v8a" + abiFilters "armeabi-v7a" } } debug { debuggable true jniDebuggable true ndk { - abiFilters "armeabi-v7a", "arm64-v8a" + abiFilters "armeabi-v7a" } } }