Drive still needs work

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
 }

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 robotLiftRotation = gamepad2.right_trigger > 0.05; // RT
-        boolean liftRobot = gamepad2.right_bumper; // RB
+        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) {
+        if (screwDeposit) {
-            this.currentScrewPosition += GANTRY_SCREW_DELTA;
+            this.robot.getGantry().deposit();
-            this.robot.getGantry().setScrew(currentScrewPosition);
+        } else if (screwIntake) {
-        } else if (!previousScrewReset && screwReset) {
+            this.robot.getGantry().intake();
-            this.robot.getGantry().resetScrew();
+        } 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);
         }
+//        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();
+        if (robotLiftRotation || this.liftArmShouldBeUp) {
-        } else if (liftRobot) {
+            this.liftArmShouldBeUp = true;
-            this.robot.getLift().lift();
+            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.previousRobotLiftReset = robotLiftReset;
-        this.previousScrewReset = screwReset;
+        this.previousRobotLiftExtend = robotLiftExtend;
-
     }
 }

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);

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 final CRServo screwServo;
-    private int currentScrewIndex = 0;
+    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 = hardwareMap.get(CRServo.class, GANTRY_SCREW_NAME);
-        this.screwServo.setPosition(1);
+        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() {
+    public void intake() {
-        this.screwServo.setPosition(0);
+        this.screwServo.setPower(1);
+        this.screwServo.setDirection(DcMotorSimple.Direction.FORWARD);
     }
 
-    public void setScrew(double target) {
+    public void deposit() {
-        double clamped = Math.min(1, Math.max(0, target));
+        this.screwServo.setPower(1);
-        this.screwServo.setPosition(clamped);
+        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());
+    }
 }

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();
     }
 }

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_MIN = 0.17;
-    public static double PICKUP_ARM_MAX = 0.75;
+    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_MIN = 0.43;
-    public static double GANTRY_ARM_MAX = 0;
+    public static double GANTRY_ARM_MAX = 0.95;
-    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_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 double LIFT_ROTATION_UP = 0.4;
-    public static int LIFT_EXTEND_MAX = 100;
+    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;
+
 }

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 = hardwareMap.get(Servo.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.setTargetPosition(0);
         this.lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
         this.lift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
     }
 
-    public void raise() {
+    public void up() {
-        this.rotation.setTargetPosition(LIFT_ROTATION_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() {
+    public void retract() {
-        this.rotation.setTargetPosition(LIFT_ROTATION_UP);
+        this.armControllerTarget = LIFT_ROTATION_UP;
-        this.lift.setTargetPosition(0);
+        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() {
+    public void startReset() {
-        this.rotation.setTargetPosition(0);
+        this.armControllerTarget = LIFT_ROTATION_DOWN;
-        this.lift.setTargetPosition(0);
+        this.lift.setTargetPosition(-1 * LIFT_EXTEND_MAX);
+        this.lift.setPower(1);
+    }
 
-        this.lift.setPower(0.25);
+    public void stopReset() {
-        this.rotation.setPower(0.25);
+        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;
     }
 }

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);
     }
 

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"
             }
         }
     }