CurrentAutonCode

package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import java.util.List;
import org.firstinspires.ftc.robotcore.external.ClassFactory;
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection;
import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import com.qualcomm.hardware.bosch.BNO055IMU;
// use "import com.qualcomm.robotcore.hardware.Bot;" later, for robot.gyro.position or whatever but
// not now
// calibrate robot when it is on the ground


//fellow comrades, please note that the encoder ticks below are just for reference, since they
//must be calculated later on
//this is for crater

@Autonomous(name = "Julia is the Best78999012")

//@Disabled
public class TensorFlowIterative extends OpMode {

    //Declare OpMode members
    private DcMotor upperLeftDrive = null;
    private DcMotor lowerLeftDrive = null;
    private DcMotor upperRightDrive = null;
    private DcMotor lowerRightDrive = null;
    private DcMotor hook = null;
    private Servo teamMarker = null;
    private DcMotor intakeArm = null;
    private BNO055IMU imu;
    private AngleUnit unit = AngleUnit.DEGREES;


    //these are example ratios and circumferences for the drive train,
    //change accordingly, btw for 2:1 it is 2, 80 tooth to 40 tooth
    //for gyro, i am assuming that the head of the robot is at 0 initially, and goes clockwise

    private double DriveGearRatio = 1 / 2;
    private double DriveCircumference = (1.97 * 2) * Math.PI;

    //not for the actual motor, but for the final gear
    private int ticksPerRotation = (int) (DriveGearRatio * 1120);

    private static final String TFOD_MODEL_ASSET = "RoverRuckus.tflite";
    private static final String LABEL_GOLD_MINERAL = "Gold Mineral";
    private static final String LABEL_SILVER_MINERAL = "Silver Mineral";
    private static final String VUFORIA_KEY = "AbZUuPf/////AAAAGUmS0Chan00iu7rnRhzu63+JgDtPo889M6dNtjvv+WKxiMJ8w2DgSJdM2/zEI+a759I7DlPj++D2Ryr5sEHAg4k1bGKdo3BKtkSeh8hCy78w0SIwoOACschF/ImuyP/V259ytjiFtEF6TX4teE8zYpQZiVkCQy0CmHI9Ymoa7NEvFEqfb3S4P6SicguAtQ2NSLJUX+Fdn49SEJKvpSyhwyjbrinJbak7GWqBHcp7fGh7TNFcfPFMacXg28XxlvVpQaVNgkvuqolN7wkTiR9ZMg6Fnm0zN4Xjr5lRtDHeE51Y0bZoBUbyLWSA+ts3SyDjDPPUU7GMI+Ed/ifb0csVpM12aOiNr8d+HsfF2Frnzrj2";
    private VuforiaLocalizer vuforia;
    private TFObjectDetector tfod;
    private String mineralPosition = "";
    private int step = 1;
    private int nextstep = 0;
    private int targetPosition = 0;
    private int position = 1;
    private boolean fl;
    private boolean fr;
    private boolean dl;
    private boolean dr;


    @Override
    public void init() {
        // The TFObjectDetector uses the camera frames from the VuforiaLocalizer, so we create that
        // first.

        telemetry.addData("Status", "Initialized");
        telemetry.update();

        upperLeftDrive = hardwareMap.get(DcMotor.class, "front left");
        upperRightDrive = hardwareMap.get(DcMotor.class, "front right");
        lowerLeftDrive = hardwareMap.get(DcMotor.class, "back left");
        lowerRightDrive = hardwareMap.get(DcMotor.class, "back right");
        teamMarker = hardwareMap.get(Servo.class, "team marker");
        hook = hardwareMap.get(DcMotor.class, "hook");
        intakeArm = hardwareMap.get(DcMotor.class, "intake arm");
        imu = hardwareMap.get(BNO055IMU.class, "imu");

        //initialization
        upperLeftDrive.setDirection(DcMotor.Direction.FORWARD);
        upperRightDrive.setDirection(DcMotor.Direction.REVERSE);
        lowerLeftDrive.setDirection(DcMotor.Direction.FORWARD);
        lowerRightDrive.setDirection(DcMotor.Direction.REVERSE);

    }

    @Override
    public void init_loop() {
    }

    @Override
    public void start() {
        initVuforia();
    }

    //if opMode is active

    //assumes there's a bug in the code because there is no communication between the robot and the phones
    //state machine, upon finishing a state, one can go to another state
    //similar to a flow chart, its linear
    //make one variable in case 0, but in each case, make it different things, different ways to refer to the same object
    //do hook and vuforia
    //break makes it exit the switch statement
    //make a variable that is 8400
    @Override
    public void loop() {

        switch(step) {
            case -1:
                fl = upperLeftDrive.getCurrentPosition() >= position - 20;
                fr = upperRightDrive.getCurrentPosition() >= position - 20;
                dl = lowerLeftDrive.getCurrentPosition() >= position - 20;
                dr = lowerRightDrive.getCurrentPosition() >= position - 20;

                if (fl && fr && dl && dr) {
                    //update to see if they are still under the target position
                    step = nextstep;
                }
                break;

            case 0:
                if (hook.getCurrentPosition() >= 8400 - 20) {
                    step = nextstep;
                }
                    break;
            case 1:
                DrivePosition(8400);
                nextstep = 2;
                step = 0;
                break;
            case 2:
                //here, position is 0
                position = DriveTrain(20, "backwards");
                nextstep = 3;
                step = -1;
                break;
            case 3:
                break;
        }

        telemetry.addLine("" + hook.getCurrentPosition());
        telemetry.addLine(upperLeftDrive.getCurrentPosition() + "");
        telemetry.addLine(upperRightDrive.getCurrentPosition() + "");
        telemetry.addLine(lowerLeftDrive.getCurrentPosition() + "");
        telemetry.addLine(lowerRightDrive.getCurrentPosition() + "");
        telemetry.addLine(step + "");
        telemetry.addLine(position + "");
        telemetry.update();



        /*

        //move the hook
        DrivePosition(8400);
        if (hook.getCurrentPosition() <= 8400 - 20) {
            telemetry.addLine(hook.getCurrentPosition() + "");
            telemetry.update();
        } else if (hook.getCurrentPosition() >= 8400 - 20) {
            hook.setPower(0);
        } else {
            hook.setPower(0);
        }

        //drive off
        DriveTrain(5, "backwards");

        try {
            Thread.sleep(10000);
        }
        catch (Exception e) {
            telemetry.addLine("Error, weeb");
            telemetry.update();
        }

        long startTime = System.currentTimeMillis(); //log when you started to turn

        //point the phone to the minerals
        while (true) {
            gyroCorrect(270, 5, getGyroRotation(unit), .1, .4);

            long currentTime = System.currentTimeMillis(); // get current time
            if (currentTime - startTime > 5000) { // if more than 5 seconds has passed, stop turning and procede
                break; //exit loop
            }
        }

        //if tfod can be initiated
        if (ClassFactory.getInstance().canCreateTFObjectDetector()) {
            initTfod();
        } else {
            telemetry.addData("Sorry!", "This device is not compatible with TFOD");
        }

        telemetry.update();

            // Activate Tensor Flow Object Detection.
            if (tfod != null) {
                tfod.activate();

                //allocate time for tfod to work
                try {
                    Thread.sleep(1000);
                }
                catch (Exception e) {
                    telemetry.addLine("Sorry, error with the code.");
                }


                // getUpdatedRecognitions() will return null if no new information is available since
                // the last time that call was made.
                List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
                if (updatedRecognitions != null) {
                    telemetry.addData("# Object Detected", updatedRecognitions.size());
                    if (updatedRecognitions != null) {
                        for (int i = 0; i < updatedRecognitions.size(); i++) {
                            if (updatedRecognitions.get(i).getWidth() * updatedRecognitions.get(i).getHeight() < 10000 ||
                                    updatedRecognitions.get(i).getWidth() - updatedRecognitions.get(i).getHeight() > 50) {
                                //If the difference between height and width is larger than 50 it is unlikely to be a mineral
                                updatedRecognitions.remove(i); // remove the recognition
                                i--;
                            }
                        }

                        //there should be three minerals left
                        if (updatedRecognitions.size() == 3) {

                            //these initializations are just to check that, later on, the values
                            //are different and thus minerals have been recognized
                            int goldMineralX = -1;
                            int silverMineral1X = -1;
                            int silverMineral2X = -1;

                            //a for each loop to run through each recognition to check for
                            //the gold mineral

                            for (Recognition recognition : updatedRecognitions) {

                                //get the label of the recognition according to the neural
                                //network

                                //save the x coordinate of each identification to subsequent
                                //mineral variable
                                if (recognition.getLabel().equals(LABEL_GOLD_MINERAL)) {
                                    goldMineralX = (int) recognition.getLeft();

                                } else if (silverMineral1X == -1) {
                                    silverMineral1X = (int) recognition.getLeft();

                                } else {
                                    silverMineral2X = (int) recognition.getLeft();
                                }
                            }

                            //if all three minerals were identified
                            if (goldMineralX != -1 && silverMineral1X != -1 && silverMineral2X != -1) {
                                if (goldMineralX < silverMineral1X && goldMineralX < silverMineral2X) {
                                    telemetry.addData("Gold Mineral Position", "Left");
                                    mineralPosition = "left";

                                } else if (goldMineralX > silverMineral1X && goldMineralX > silverMineral2X) {
                                    telemetry.addData("Gold Mineral Position", "Right");
                                    mineralPosition = "right";

                                } else {
                                    telemetry.addData("Gold Mineral Position", "Center");
                                    mineralPosition = "center";

                                }

                            }

                        }

                        //strafe to go closer to the minerals
                        strafeleft(24);

                        //depending on each mineral location,

                        if (mineralPosition.equals("left")) { //if the gold mineral is all the way on the left (from the perspective
                            //of the camera)

                            //go backwards towards the leftmost mineral
                            DriveTrain(30, "backwards");

                            //strafe to hit the mineral
                            strafeleft(12);

                            //strafe back out
                            straferight(12);

                        } else if (mineralPosition.equals("right")) {

                            //go backwards towards the rightmost mineral
                            DriveTrain(30, "forwards");

                            //strafe to hit the mineral
                            strafeleft(12);

                            //strafe back out
                            straferight(12);

                            //go back to the left to get ready for the weird turn
                            DriveTrain(60, "backwards");

                        } else { //most likely, the mineral will be in the center if the above
                            //if statements fail , but in case tfod doesn't
                            //detect any minerals, this gives us a fail-safe

                            //strafe to hit the mineral
                            strafeleft(12);

                            //strafe back out
                            straferight(12);

                            DriveTrain(30, "backwards");
                        }


                        //do the weird turn
                        weirdTurn(24);

                        //strafe closer to hit the wall
                        strafeleft(28);

                        //go towards the depot square
                        DriveTrain(36, "backwards");

                        //turns towards the depot square
                        startTime = System.currentTimeMillis(); //log when you started to turn

                        while (true) {
                            gyroCorrect(-90, 5, getGyroRotation(unit), .1, .4);
                            //correct to the angle of 90
                            long currentTime = System.currentTimeMillis(); // get current time
                            if (currentTime - startTime > 5000) { // if more than 5 seconds has passed (more time than should be needed for a turn), stop turning and procede
                                break; //exit loop
                            }
                        }

                        //deploy the teamMarker
                        teamMarker.setPosition(0);

                        //strafe towards the crater
                        straferight(36);

                        //turn towards the crater
                        startTime = System.currentTimeMillis(); //log when you started to turn

                        //point the phone to the minerals
                        while (true) {
                            gyroCorrect(90, 5, getGyroRotation(unit), .1, .4);
                            //correct to the angle of 90
                            long currentTime = System.currentTimeMillis(); // get current time
                            if (currentTime - startTime > 5000) { // if more than 5 seconds has passed (more time than should be needed for a turn), stop turning and procede
                                break; //exit loop
                            }
                        }

                        //move the arm to the crater
                        DrivePositionArm(-360);
                        telemetry.update();
                    }
                }
            }

            */
    }

    @Override
    public void stop() {
        if (tfod != null) {
            tfod.shutdown();
        }
        telemetry.addLine("Long live the russian regime! (aka Sasha)");
        telemetry.update();
    }

    private void initVuforia() {
        //Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
        //config the vuforia with parameters and variables

        VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();

        parameters.vuforiaLicenseKey = VUFORIA_KEY;
        parameters.cameraDirection = CameraDirection.FRONT;

        //  Instantiate the Vuforia engine
        vuforia = ClassFactory.getInstance().createVuforia(parameters);

        // Loading trackables is not necessary for the Tensor Flow Object Detection engine.
    }

    /**
     * Initialize the Tensor Flow Object Detection engine.
     */
    private void initTfod() {
        //stores an id for where to later draw the image, a window per se
        int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
                "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());

        TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);

        //implements the parameters, and passes the image to tfod
        tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);

        /* professor Wen states that this is a neural network, so each parameter is linked
        to a specific identification system and we are simply giving names to those final
        identifications to reference them later on */
        tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_GOLD_MINERAL, LABEL_SILVER_MINERAL);
    }

    public void DrivePosition(int position) {
        //apparently using negative or positive power here doesn't matter
        hook.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

        hook.setTargetPosition(position);

        hook.setMode(DcMotor.RunMode.RUN_TO_POSITION);

        hook.setPower(1);
    }

    public void DrivePositionArm(int position) {
        intakeArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

        intakeArm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

        intakeArm.setTargetPosition(position);

        intakeArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);

        intakeArm.setPower(1);
    }

    //for forwards, power = -1, and for backwards, power = 1
    public int DriveTrain(double distance, String direction) {
        //example gear ratio, 2:1
        //example circumference
        //distance is in inches

        double circumferenceTraveled = distance / DriveCircumference;

        int position = (int) (ticksPerRotation * circumferenceTraveled);

        //if it is forwards or backwards, change sign of position
        if (direction.equalsIgnoreCase("forwards")) {
            position = -position;
        }
        else if (direction.equalsIgnoreCase("backwards")) {
            //position is equal to itself
        }

        double power = powerScaleDistance(distance);

        upperLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        upperLeftDrive.setTargetPosition(position);
        upperLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);

        lowerLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        lowerLeftDrive.setTargetPosition(position);
        lowerLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);

        upperRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        upperRightDrive.setTargetPosition(position);
        upperRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);

        lowerRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        lowerRightDrive.setTargetPosition(position);
        lowerRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);

        upperRightDrive.setPower(power);
        lowerLeftDrive.setPower(power);
        upperRightDrive.setPower(power);
        lowerRightDrive.setPower(power);

        return position;
    }

    //power scale according to distance
    public double powerScaleDistance(double distance) {
        double DrivePower = 0;
        if (distance <= 24) {
            DrivePower = 0.5;
        }
        else if (distance > 24) {
            DrivePower = 1;
        }
        return DrivePower;
    }

    public void strafeleft(double distance) {
        //guess and check, then check formulas later on
        //assuming a similar relationship from distance to ticks in the drive train, probably going to
        //be much less

        //how much the wheel actually needs to turn
        double circumferenceTraveled = distance / DriveCircumference;

        //here position must be positive
        int position = (int) (ticksPerRotation * circumferenceTraveled);

        double power = powerScaleDistance(distance);

        upperLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        upperLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        upperLeftDrive.setTargetPosition(position);
        upperLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        upperLeftDrive.setPower(powerScaleDistance(power));

        upperRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        upperRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        upperRightDrive.setTargetPosition(-position);
        upperRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        upperRightDrive.setPower(powerScaleDistance(power));

        lowerLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        lowerLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        lowerLeftDrive.setTargetPosition(-position);
        lowerLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        lowerLeftDrive.setPower(powerScaleDistance(power));

        lowerRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        lowerRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        lowerRightDrive.setTargetPosition(position);
        lowerRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        lowerRightDrive.setPower(powerScaleDistance(power));
    }

    public void straferight(double distance) {
        //guess and check, then check formulas later on
        //assuming a similar relationship from distance to ticks in the drive train, probably going to
        //be much less

        double circumferenceTraveled = distance / DriveCircumference;

        int position = (int) (ticksPerRotation * circumferenceTraveled);

        double power = powerScaleDistance(distance);

        upperLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        upperLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        upperLeftDrive.setTargetPosition(-position);
        upperLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        upperLeftDrive.setPower(powerScaleDistance(power));

        upperRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        upperRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        upperRightDrive.setTargetPosition(position);
        upperRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        upperRightDrive.setPower(powerScaleDistance(power));

        lowerLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        lowerLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        lowerLeftDrive.setTargetPosition(position);
        lowerLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        lowerLeftDrive.setPower(powerScaleDistance(power));

        lowerRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        lowerRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        lowerRightDrive.setTargetPosition(-position);
        lowerRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        lowerRightDrive.setPower(powerScaleDistance(power));
    }

    public void gyroCorrect(double gyroTarget, double gyroRange, double gyroActual, double minSpeed,
                            double addSpeed) {

        while ((gyroTarget - getGyroRotation(unit) + 360.0) % 360.0 < 0.5) {

            //this makes it an actual angle between 0 and 360 anyways, so it doesn't matter if the target
            //is weird for the gyro or actually between 0 and 360
            double delta = (gyroTarget - gyroActual + 360.0) % 360.0; // in case it is negative

            if (delta > 180.0) {
                delta -= 360.0; // delta becomes between -180 and 180
                //because the range is from 0-> 180 and -180-> 0 instead of 0-> 360
            }

            if (Math.abs(delta) > gyroRange) {
                double gyroMod = delta / 45.0; // if delta is less than 45 and bigger than -45, this will make a scale from
                // -1 to 1

                if (Math.abs(gyroMod) > 1.0) {
                    gyroMod = Math.signum(gyroMod); //makes gyroMod -1 or 1 if error is more than 45
                    // or less than -45 degrees
                }

                //if the error is more than 180, then the power is positive, and it turns to the left
                //if the error is less than 180, the power in the turn in negative, and it turns to the
                //right
                //if the error is larger, faster speed
                this.turn(minSpeed * Math.signum(gyroMod) + addSpeed * gyroMod);
            } else {
                turn(0.0);
            }
        }
    }

    public void turn(double power) { //this is opposite to the varsity because our gear train makes
        //forwards negative and backwards positive
        upperLeftDrive.setPower(power);
        upperRightDrive.setPower(-power);
        lowerLeftDrive.setPower(power);
        lowerRightDrive.setPower(-power);
    }

    //gets current angle position
    public float getGyroRotation(AngleUnit unit) {
        return imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, unit).firstAngle;
    }

    //this is a weird turn because only the left half of the motors move
    public void weirdTurn(double distance) {
        //example gear ratio, 2:1
        //example circumference
        //distance is in inches

        double circumferenceTraveled = distance / DriveCircumference;

        int position = (int) (ticksPerRotation * circumferenceTraveled);

        //if it is forwards or backwards, change sign of position
        double power = powerScaleDistance(distance);

        //position is positive because it will make the motors go backwards
        upperLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        upperLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        upperLeftDrive.setTargetPosition(position);
        upperLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        upperLeftDrive.setPower(powerScaleDistance(power));

        lowerLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        lowerLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        lowerLeftDrive.setTargetPosition(position);
        lowerLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        lowerLeftDrive.setPower(power);
    }
}