diff --git a/Autonomous.java b/Autonomous.java index c44071b..f984f16 100644 --- a/Autonomous.java +++ b/Autonomous.java @@ -52,20 +52,22 @@ public void runOpMode() throws InterruptedException { intakeWheel = hardwareMap.get(DcMotor.class, "intakeWheel"); // firstPixel = hardwareMap.get(Servo.class, "FirstPixel"); // secondPixel = hardwareMap.get(Servo.class, "SecondPixel"); - randomizationWebcam = new PlaceDetectionWebcam(); - randomizationWebcam.init(hardwareMap, "Blue.tflite"); - int loc = 0; - while (!opModeIsActive()){ - if (isStopRequested()){ - break; - } - loc = detectTape(randomizationWebcam); +// randomizationWebcam = new PlaceDetectionWebcam(); +// randomizationWebcam.init(hardwareMap, "Blue.tflite"); +// int loc = 0; +// while (opModeInInit()){ +// loc = detectTape(randomizationWebcam); +// } +// randomizationWebcam.stop(); + navigation = new Navigation(new double[]{12, 60}, 0, this, hardwareMap); + while(opModeInInit()){ + TelemetryWrapper.setLine(7, "x: " + navigation.getCurrentPos()[0] + " y: " +navigation.getCurrentPos()[1]); + TelemetryWrapper.setLine(8, "Gyro bearing: " + navigation.getGyroBearing()); } - randomizationWebcam.stop(); - navigation = new Navigation(new double[]{12, 50}, 270, this, hardwareMap); + navigation = new Navigation(new double[]{12, 60}, 270, this, hardwareMap); navigation.MoveToPosDirect(new double[]{12, 36}); - navigation.MoveToPosDirect(new double[]{12, 12}); + navigation.MoveToPosDirect(new double[]{36, 36}); // // Turn around diff --git a/TeleOp.java b/TeleOp.java index 75f41cd..db9b30f 100644 --- a/TeleOp.java +++ b/TeleOp.java @@ -97,7 +97,7 @@ public void runOpMode() { intakeWheel.setPower((gamepad2.right_trigger - gamepad2.left_trigger) * 0.8); if (Math.abs(gamepad2.left_stick_y) > 0.001) { - outputSlide.startMoveToRelativePos((int) -gamepad2.left_stick_y * 100); + outputSlide.startMoveToRelativePos((int) -gamepad2.left_stick_y * 500); } if (gp2.pressing(ButtonHelper.b)) { outputSlide.startMoveToPos(1430); diff --git a/modules/Navigation.java b/modules/Navigation.java index 1c87ccc..9d43e61 100644 --- a/modules/Navigation.java +++ b/modules/Navigation.java @@ -6,6 +6,14 @@ import org.firstinspires.ftc.teamcode.modules.Webcams.AprilTagWebcam; import org.firstinspires.ftc.teamcode.util.TelemetryWrapper; +import com.qualcomm.hardware.bosch.BNO055IMU; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.Velocity; + public class Navigation { private double[] currentPos; private double currentBearing; @@ -20,14 +28,30 @@ public class Navigation { private AprilTagWebcam tagCam; - private double lastAprilTagBearing; + BNO055IMU imu; + private double initGyroReading; public Navigation (double[] initPos, double initBearing, LinearOpMode opMode, HardwareMap map){ currentPos = initPos; + lastAprilTagPos = initPos; + lastDriveTrainPos = initPos; + lastDriveTrainBearing = initBearing; currentBearing = initBearing; driveTrain = new DriveTrain(opMode); driveTrain.init(map); tagCam = new AprilTagWebcam(); tagCam.init(map); + BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); + parameters.mode = BNO055IMU.SensorMode.IMU; + parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; + parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC; + parameters.loggingEnabled = false; + imu = map.get(BNO055IMU.class, "imu"); + imu.initialize(parameters); + while (opMode.opModeInInit() && !imu.isGyroCalibrated()){ + TelemetryWrapper.setLine(2, "Calibrating gyro..."); + } + TelemetryWrapper.setLine(2, "Gyro calibrated"); + initGyroReading = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES).firstAngle - initBearing; } public double[] getCurrentPosition(){ @@ -38,6 +62,10 @@ public double getCurrentBearing() { return currentBearing; } + public double getGyroBearing(){ + return ((imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES).firstAngle - initGyroReading%360)+360)%360; + } + public double magnitude(double[] vector){ double sum = 0; for (double component:vector){ @@ -45,32 +73,78 @@ public double magnitude(double[] vector){ } return Math.sqrt(sum); } + public double[] getCurrentPos(){ + lastAprilTagPos = tagCam.detectIter(currentBearing); + if (tagCam.isDetecting){ + return lastAprilTagPos; + } + return lastDriveTrainPos; + } + + public void setBearing(double targetBearing){ + targetBearing = (targetBearing%360)+360; + double dir =((targetBearing-currentBearing < 180)? -1 : 1); + driveTrain.move(0, 0, 0.1*dir, 1); + while (Math.abs(targetBearing - currentBearing) > 0.5){ + TelemetryWrapper.setLine(6, "Target Bearing: " + targetBearing + " | " + "Current Bearing: " + currentBearing); + currentBearing = getGyroBearing(); + } + driveTrain.stopStayInPlace(); + } + + public void MoveToPosDirect(double[] targetPos){ double targetBearing = Math.toDegrees(Math.atan2(targetPos[1]-currentPos[1], targetPos[0]-currentPos[0])); - targetBearing = (360+targetBearing)%360; - TelemetryWrapper.setLine(6, "Target Bearing: " + targetBearing + " | " + "Delta Bearing: " + -(targetBearing - currentBearing)); - driveTrain.translate(0.2, 0,0, -(targetBearing-currentBearing), 10); - currentBearing = targetBearing; + setBearing(targetBearing); double[] lastEncoderPos = driveTrain.getEncPos(); double[] displacement = new double[]{targetPos[0]- currentPos[0], targetPos[1] - currentPos[1]}; - driveTrain.move(0,0.2, 0, 1); - while (magnitude(displacement) > 0.5){ + driveTrain.move(0,0.7, 0, 1); + while (magnitude(displacement) > 2){ TelemetryWrapper.setLine(5, "x: "+ currentPos[0] + "| y: " + currentPos[1] + "| theta: " + currentBearing); -// if (tagCam.isDetecting){ -// currentPos = tagCam.detectIter(); -// TelemetryWrapper.setLine(7, "CAM"); -// }else { + double[] currentEncPos = driveTrain.getEncPos(); + currentBearing = getGyroBearing(); + lastAprilTagPos = tagCam.detectIter(currentBearing); + if (tagCam.isDetecting){ + currentPos = lastAprilTagPos; + TelemetryWrapper.setLine(7, "CAM"); + }else { TelemetryWrapper.setLine(7, "ENC"); double delta = 0; for (int i = 0; i < lastEncoderPos.length; i++) { - delta += Math.abs(driveTrain.getEncPos()[i] - lastEncoderPos[i]) / driveTrain.COUNTS_PER_INCH; + delta += Math.abs(currentEncPos[i] - lastEncoderPos[i]) / driveTrain.COUNTS_PER_INCH; } delta /= lastEncoderPos.length; currentPos = new double[]{currentPos[0] + Math.cos(Math.toRadians(currentBearing)) * delta, currentPos[1] + Math.sin(Math.toRadians(currentBearing)) * delta}; -// } + } displacement = new double[]{targetPos[0]- currentPos[0], targetPos[1] - currentPos[1]}; - lastEncoderPos = driveTrain.getEncPos(); + lastEncoderPos = currentEncPos; + } + driveTrain.stopStayInPlace(); + strafeToPos(targetPos); + } + + public void strafeToPos(double[] targetPos){ + double[] displacement = new double[]{targetPos[0]- currentPos[0], targetPos[1] - currentPos[1]}; + double dx = Math.cos(Math.toRadians(currentBearing-90))*(displacement[0]) - Math.sin(Math.toRadians(currentBearing-90))*(displacement[1]); + double dy = Math.sin(Math.toRadians(currentBearing-90))*(displacement[0]) + Math.cos(Math.toRadians(currentBearing-90))*(displacement[1]); + + driveTrain.translate(0.4, dx, 0, 0, 10); + driveTrain.translate(0.4, 0, dy, 0, 10); + lastAprilTagPos = tagCam.detectIter(currentBearing); + if(tagCam.isDetecting){ + currentPos = lastAprilTagPos; + TelemetryWrapper.setLine(7, "CAM"); + }else{ + currentPos = targetPos; + TelemetryWrapper.setLine(7, "ENC"); } - driveTrain.stop(); + driveTrain.stopStayInPlace(); } + + public void MoveToPosAtAngle(double[] targetPos, double angle){ + double directAngle = Math.toDegrees(Math.atan2(targetPos[1]-currentPos[1], targetPos[0]-currentPos[0])); + directAngle = (360+directAngle)%360; + + } + } diff --git a/modules/Webcams/AprilTagWebcam.java b/modules/Webcams/AprilTagWebcam.java index 39bbf6d..2feaa15 100644 --- a/modules/Webcams/AprilTagWebcam.java +++ b/modules/Webcams/AprilTagWebcam.java @@ -64,6 +64,8 @@ public class AprilTagWebcam { */ public boolean isDetecting = false; + private final double[] tagCamOffset = new double[] {0, -8}; + /** * Last seen position */ @@ -144,12 +146,12 @@ public void toggle(){ /** * Add telemetry about AprilTag detections. + * @param bearing the bearing of the webcam in euler angles (x, y, z) */ - public double[] detectIter() { + public double[] detectIter(double bearing) { List currentDetections = aprilTag.getDetections(); - double avg_z = 0; double avg_y = 0; double avg_x = 0; isDetecting = false; @@ -157,17 +159,17 @@ public double[] detectIter() { for (AprilTagDetection detection : currentDetections) { if (detection.metadata != null) { isDetecting = true; - avg_z += detection.metadata.fieldPosition.get(2) - detection.ftcPose.z; - avg_y += detection.metadata.fieldPosition.get(1) - detection.ftcPose.y; - avg_x += detection.metadata.fieldPosition.get(0) - detection.ftcPose.x; + avg_y += detection.metadata.fieldPosition.get(1)-(Math.sin(Math.toRadians(bearing-90))*(detection.ftcPose.x-tagCamOffset[0])+ Math.cos(Math.toRadians(bearing-90))*(detection.ftcPose.y-tagCamOffset[1])); + avg_x += detection.metadata.fieldPosition.get(0)-(Math.cos(Math.toRadians(bearing-90))*(detection.ftcPose.x-tagCamOffset[0]) - Math.sin(Math.toRadians(bearing-90))*(detection.ftcPose.y-tagCamOffset[1])); } } // end for() loop - avg_z /= currentDetections.size(); avg_y /= currentDetections.size(); avg_x /= currentDetections.size(); + + if (isDetecting) { - lastPosition = new double[]{avg_x, avg_y, avg_z}; + lastPosition = new double[]{avg_x, avg_y}; } return lastPosition; diff --git a/modules/output/LinearSlide.java b/modules/output/LinearSlide.java index 1509899..69134dc 100644 --- a/modules/output/LinearSlide.java +++ b/modules/output/LinearSlide.java @@ -85,7 +85,7 @@ public void startRetraction() { */ @Override public void tick() { - if (elevatorBtnLeft.isPressed()){ + if (elevatorBtnLeft.isPressed() && elevatorLeft.isBusy()){ int targetPos = elevatorLeft.getTargetPosition(); double power = elevatorLeft.getPower(); elevatorLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); @@ -93,7 +93,7 @@ public void tick() { elevatorLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION); elevatorLeft.setPower(power); } - if (elevatorBtnRight.isPressed()){ + if (elevatorBtnRight.isPressed() && elevatorRight.isBusy()){ int targetPos = elevatorRight.getTargetPosition(); double power = elevatorRight.getPower(); elevatorRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);