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