9.4. Autonomie – mers în pătrat

/* Copyright (c) 2017 FIRST. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted (subject to the limitations in the disclaimer below) provided that
 * the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this list
 * of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright notice, this
 * list of conditions and the following disclaimer in the documentation and/or
 * other materials provided with the distribution.
 *
 * Neither the name of FIRST nor the names of its contributors may be used to endorse or
 * promote products derived from this software without specific prior written permission.
 *
 * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
 * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

package org.firstinspires.ftc.teamcode;

import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DistanceSensor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ReadWriteFile;

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.internal.system.AppUtil;

import java.io.File;

@Autonomous(name = "MergeInPatrat", group = "Sensor")
//@Disabled
public class mersInPatrat extends LinearOpMode {

    ColorSensor sensorColor1;
    DistanceSensor sensorDistance1;

    ColorSensor sensorColor2;
    DistanceSensor sensorDistance2;

    DcMotor leftmotor1;
    DcMotor leftmotor2;
    DcMotor rightmotor1;
    DcMotor rightmotor2;

    double unghiGlobal, power = 0.4;

    BNO055IMU imu;
    Orientation lastunghiuri = new Orientation();

    @Override

    public void runOpMode() {

        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 = hardwareMap.get(BNO055IMU.class, "imu1");

        Servo servo1;
        Servo servo2;

        imu.initialize(parameters);

        while (!isStopRequested() && !imu.isGyroCalibrated()) {
            sleep(50);
            idle();
        }

        sensorColor1 = hardwareMap.get(ColorSensor.class, "color1");
        sensorDistance1 = hardwareMap.get(DistanceSensor.class, "color1");
        sensorColor2 = hardwareMap.get(ColorSensor.class, "color2");
        sensorDistance2 = hardwareMap.get(DistanceSensor.class, "color2");

        servo1 = hardwareMap.servo.get("servo1");
        servo2 = hardwareMap.servo.get("servo2");

        leftmotor1 = hardwareMap.dcMotor.get("stanga fata");
        leftmotor2 = hardwareMap.dcMotor.get("stanga spate");
        rightmotor1 = hardwareMap.dcMotor.get("dreapta fata");
        rightmotor2 = hardwareMap.dcMotor.get("dreapta spate");

        rightmotor1.setDirection(DcMotor.Direction.REVERSE);

        waitForStart();

        while (opModeIsActive()) {


            roteste_90();//this function turns the robot 90 degrees to right in our case
            // reseteazaUnghi();//we made an external function that resets the IMU to 0 degrees
        }// we do this proces 4 times to make a square

    }
    public void roteste_90() {

        while (preiaUnghi() < 86) {
            rightmotor1.setPower(-1);
            leftmotor1.setPower(1);
            rightmotor2.setPower(-1);
            leftmotor2.setPower(1);
        }

        leftmotor1.setPower(0);
        leftmotor2.setPower(0);
        rightmotor1.setPower(0);
        rightmotor2.setPower(0);

        sleep(500);

        leftmotor1.setPower(-0.3);
        leftmotor2.setPower(-0.3);
        rightmotor1.setPower(-0.3);
        rightmotor2.setPower(-0.3);

        sleep(500);

        leftmotor1.setPower(0);
        leftmotor2.setPower(0);
        rightmotor1.setPower(0);
        rightmotor2.setPower(0);
    }
    private void reseteazaUnghi()
    {
        lastunghiuri = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
        unghiGlobal = 0;
    }


    private double preiaUnghi() {
        Orientation unghiuri = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);

        double Unghi = unghiuri.firstAngle - lastunghiuri.firstAngle;

        if (Unghi < -180)
            Unghi += 360;
        else if (Unghi > 180)
            Unghi -= 360;
        unghiGlobal += Unghi;
        lastunghiuri = unghiuri;
        return unghiGlobal;
    }
}