CurrentAutonCode

package org.firstinspires.ftc.teamcode;

import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;

import org.firstinspires.ftc.robotcore.external.ClassFactory;
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.VuforiaLocalizer;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection;
import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;

import java.util.List;

@Autonomous(name = "Wen'asdasd")

public class TensorFlowAlonePracticePurged extends OpMode {


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


//@Disa

    //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.0 / 2.0;
    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;
    private double gyroTarget;
    private double gyroRange;
    private double minSpeed;
    private double addSpeed;
    private double power;
    private double distance;
    private long startTime;
    private int encoderSign;
    private List<Recognition> alwaysNonNull;


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

        initVuforia();
        initTfod();
        if (tfod != null) {
            tfod.activate();
        }
    }

    @Override
    public void init_loop() {
    }

    @Override
    public void start() {
    }

    //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
    //you can make a varible hold different objects if you want, for example different motors
    public void loop() {

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

        } else {
            alwaysNonNull = updatedRecognitions;
        }
        if (alwaysNonNull != null) {
            telemetry.addData("# Object Detected", alwaysNonNull.size());
            for (int i = 0; i < alwaysNonNull.size(); i++) {
                Recognition recognition = alwaysNonNull.get(i);
                //get the label of the recognition according to the neural
                //network

                telemetry.addData(Integer.toString(i) + recognition.getLabel() + ": ", recognition.getLeft());

                //otherwise strafe to the other one to the right

            }


        } else {
            telemetry.addLine("feels bad man");
        }
    }


    @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.
     */
    public 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) {
        //example gear ratio, 2:1
        //example circumference
        //distance is in inches

        double circumferenceTraveled = distance / DriveCircumference;

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

        position = encoderSign * position;

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

        return position;
    }

    public int 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.setTargetPosition(position);
        upperLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);

        upperLeftDrive.setPower(powerScaleDistance(power));

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


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


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

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

        return position;


    }

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

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

            telemetry.addLine(minSpeed * Math.signum(gyroMod) + addSpeed * gyroMod + "");
            telemetry.update();

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

    //depending on the Direction, the position is either positive or negative for the encoders
    public int setDirection(Direction direction) {
        int positionDirection = 0;
        switch (direction) {
            case FORWARDS:
                positionDirection = 1;
                break;
            case BACKWARDS:
                positionDirection = -1;
                break;
        }
        return positionDirection;
    }
}