Merge branch 'bumblebee_dev' of https://github.com/IronEaglesRobotics/CenterStage into bumblebee_dev

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;
+            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) {
+        if (gantryRight) {
-//            this.robot.getGantry().setX(this.robot.getGantry().getX() + GANTRY_X_DELTA);
+            this.robot.getGantry().setX(X_MIN);
-//        } else if (gantryRight) {
+        } else if (gantryLeft) {
-//            this.robot.getGantry().setX(this.robot.getGantry().getX() - GANTRY_X_DELTA);
+            this.robot.getGantry().setX(X_MAX);
-//        }
+        } else {
+            this.robot.getGantry().center();
+        }
 
         // Robot Lift
 

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

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) {
+    public void setSlideTarget(int target) {
-        this.xServo.setPosition(x);
+        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())));
+            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() + output);
+            this.armServo.setPosition(this.armServo.getPosition() + armOutput);
         }
 
-        this.telemetry.addData("Arm P Controller", output);
+        double xOutput = 0;
-        this.telemetry.addData("Arm P Setpoint", this.armControllerTarget);
+        if (!this.xController.atSetPoint()) {
-        this.telemetry.addData("Arm In", this.isIn());
+            xOutput = this.xController.calculate(this.xServo.getPosition());
+            this.xServo.setPosition(this.xServo.getPosition() + xOutput);
+        }
     }
 }

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.update();
-//        this.telemetry.addData("x", pose.getX());
-//        this.telemetry.addData("y", pose.getY());
     }
 }

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 X_KP = 0.1;
-    public static double GANTRY_ARM_KD = 0f;
+    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;
 

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() {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slide.java

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

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

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_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_Y = -4.409449; // Y is the strafe direction
 
-    public static double PERPENDICULAR_X = -0.6299213;
+    public static double PERPENDICULAR_X = 0.6299213;
-    public static double PERPENDICULAR_Y = 0.1122047;
+    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) {