Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- package org.firstinspires.ftc.teamcode;
- import com.qualcomm.hardware.bosch.BNO055IMU;
- import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
- 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.DcMotorSimple;
- import com.qualcomm.robotcore.hardware.DistanceSensor;
- import com.qualcomm.robotcore.hardware.Servo;
- import com.qualcomm.robotcore.util.ElapsedTime;
- 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.DistanceUnit;
- import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
- import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
- import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
- import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
- import java.util.List;
- @Autonomous(name="Autonoma_Bila_Crater", group="Autonoma_Final")
- public class Autonoma_Bila_Crater extends LinearOpMode
- {
- //Timp
- private ElapsedTime runtime = new ElapsedTime();
- //Motoare
- DcMotor motorDF;
- DcMotor motorDS;
- DcMotor motorSF;
- DcMotor motorSS;
- DcMotor motorTija;
- DcMotor motorExten_fata;
- DcMotor motorExten_spate;
- DcMotor motorSwitch;
- //Senzori
- BNO055IMU imu;
- DistanceSensor senzorTija;
- DistanceSensor senzorFata;
- ColorSensor senzorCuloare;
- //Servo
- Servo servo_colect;
- Servo servo_lander;
- //Variabile pentru detectare
- 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 = "Ad94F5T/////AAABmXHK2XiLFk+MoiPfQGybpl8fbSSDPMTEpMn4lwDMSxox2HG3QIgFb3MFGa/wO+tOLca7Tw/dvKNqD2IaQMLInqAnRPZPozTL/NE7fvbpOviIVdjNcRP3bsFzS4umkgbgtTdMahlIRW+KPSbFWnSZZ1diFpfqhIPLdMiUGBDNCU/Cizx7E1y8RlwQYH7n5nPfYskKSomRW4ZpFxTRkrDgxNbY6g+6xF/QAPEXj72MWYqjDfHi6UMaP8pfXPYqwotNlf3BDZbHiHLEBLY93jsr4Y/aXUo2bt6qWZEk/kvxiCoFTRAJFXPJwbFqWvwO+ajBbabdbTCrdd/IwtXu4CjxuHJB5veBRBIIwcQlU7+PocgD";
- private VuforiaLocalizer vuforia;
- private TFObjectDetector tfod;
- @Override
- public void runOpMode()
- {
- initMotoare();
- initSenzori();
- detect();
- waitForStart();
- if (opModeIsActive())
- {
- coborare(17200, 8.6, 8);
- Movement_Lateral_Timp(0.6, 0.7);
- Movement_Fata_Culoare(0.4, 1.5);
- angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
- unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
- sleep(100);
- Punctare();
- sleep(100);
- gyroTurn_left(-70 ,-0.3, -0.8);
- sleep(100);
- Miscare_Timp(0.8, 1);
- sleep(100);
- gyroTurn_left(-35, -0.8, -0.8 );
- sleep(100);
- Movement_Lateral_Timp(-0.8, 2);
- sleep(100);
- Miscare_Distanta(0.8, 15, 2);
- sleep(100);
- gyroTurn_left(-170, -0.8, -0.8);
- sleep(100);
- Miscare_Distanta(0.8, 30, 4);
- }
- }
- public void ExtindereBrat_Team_Marker()
- {
- }
- public void ExtindereBrat_Sampling()
- {
- }
- int a;
- int slide_fata = 0;
- public void Miscare_Slide(double viteza, int tickuri, double timeout)
- {
- /*
- Daca e inchis la apelare se va deschide si reciproc
- */
- runtime.reset();
- if(slide_fata == 0)
- {
- motor.Exten_fata.setPower(viteza);
- while(motor.Exten_fata.getCurrentPosition() <= tickuri && runtime.seconds <= timeout);
- motor.Exten_fata.setPower(0);
- slide_fata = 1;
- {
- else
- {
- motor.Exten_fata.setPower(- viteza);
- while(motor.Exten_fata.getCurrentPosition() >= 10 && runtime.seconds <= timeout);
- motor.Exten_fata.setPower(0);
- slide_fata = 0;
- }
- }
- int sistem_colectare = -1;
- public void Coborare_Sistem_Colectare(double viteza, int tickuri, double timeout)
- {
- runtime.reset();
- motorSwitch.setPower(viteza);
- while(motor.Swtich.getCurrentPosition() <= tickuri && runtime.seconds <=timeout);
- motorSwitch.setPower(0);
- sistem_colectare = 1;
- }
- public void Miscare_Sistem colectare(double viteza, int tickuri_coborare, double timeout)
- {
- runtime.reset();
- if(sistem_colectare == 0)
- {
- motorSwitch.setPower(viteza);
- while(motorSwitch.getCurrentPosition() <= tickuri && runtime.seconds <=timeout);
- motorSwitch.setPower(0);
- sistem_colectare = 1;
- }
- else if(sistem_colectare == 1)
- {
- motorSwitch.setPower(- viteza)
- while(motorSwitch.getCurrentPosition() >= 0 && runtime.seconds <= timeout);
- motorSwitch.setPower(0);
- sistem_colectare = 0;
- }
- }
- double distantaIntitiala;
- public void Miscare_Timp(double viteza, double timeout)
- {
- runtime.reset();
- motorDS.setPower(viteza);
- motorDF.setPower(viteza);
- motorSS.setPower(viteza);
- motorSF.setPower(viteza);
- while (runtime.seconds() <= timeout);
- motorDS.setPower(0);
- motorDF.setPower(0);
- motorSS.setPower(0);
- motorSF.setPower(0);
- }
- public void Miscare_Distanta(double distanta, double viteza, double timeout)
- {
- runtime.reset();
- distantaIntitiala = senzorFata.getDistance(DistanceUnit.CM);
- motorDS.setPower(viteza);
- motorDF.setPower(viteza);
- motorSS.setPower(viteza);
- motorSF.setPower(viteza);
- while (senzorFata.getDistance(DistanceUnit.CM) >= distantaIntitiala - distanta && runtime.seconds() <= timeout )
- {
- telemetry.addData("DISTANTA", senzorFata.getDistance(DistanceUnit.CM));
- telemetry.addData("MAXIM", distantaIntitiala - distanta);
- telemetry.update();
- }
- motorDS.setPower(0);
- motorDF.setPower(0);
- motorSS.setPower(0);
- motorSF.setPower(0);
- }
- Orientation angles;
- double unghiCurent, unghiFinal;
- public void gyroTurn_right(double angle, double viteza_left, double viteza_right) // unghi si viteza pozitive
- {
- runtime.reset();
- angle = -angle;
- if (unghiCurent + angle < -180)
- {
- unghiFinal = 360 + (unghiCurent + angle);
- motorDF.setPower(-viteza_right);
- motorDS.setPower(-viteza_right);
- motorSF.setPower(viteza_left);
- motorSS.setPower(viteza_left);
- angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
- unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
- while((unghiCurent < 0 || unghiCurent > unghiFinal) && runtime.seconds() < 5)
- {
- angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
- unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
- telemetry.addData("Unghi_final", unghiFinal);
- telemetry.addData("Curent", unghiCurent);
- telemetry.update();
- }
- motorDF.setPower(0);
- motorDS.setPower(0);
- motorSF.setPower(0);
- motorSS.setPower(0);
- telemetry.update();
- }
- else
- {
- unghiFinal = unghiCurent + angle;
- motorDF.setPower(-viteza_right);
- motorDS.setPower(-viteza_right);
- motorSF.setPower(viteza_left);
- motorSS.setPower(viteza_left);
- angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
- unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
- while(unghiFinal < unghiCurent && runtime.seconds() < 5)
- {
- angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
- unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
- telemetry.addData("Unghi_final", unghiFinal);
- telemetry.addData("Curent", unghiCurent);
- telemetry.update();
- }
- motorDF.setPower(0);
- motorDS.setPower(0);
- motorSF.setPower(0);
- motorSS.setPower(0);
- telemetry.update();
- }
- unghiCurent = unghiFinal;
- }
- public void gyroTurn_left(double angle, double viteza_left, double viteza_right)
- {
- runtime.reset();
- angle = -angle;
- if (unghiCurent + angle > 180)
- {
- unghiFinal = -360 + (unghiCurent + angle);
- motorDF.setPower(-viteza_right);
- motorDS.setPower(-viteza_right);
- motorSF.setPower(viteza_left);
- motorSS.setPower(viteza_left);
- angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
- unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
- while((unghiCurent > 0 || unghiCurent < unghiFinal) && runtime.seconds() < 5)
- {
- angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
- unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
- telemetry.addData("Unghi_final", unghiFinal);
- telemetry.addData("Curent", unghiCurent);
- telemetry.update();
- }
- motorDF.setPower(0);
- motorDS.setPower(0);
- motorSF.setPower(0);
- motorSS.setPower(0);
- telemetry.update();
- }
- else
- {
- unghiFinal = unghiCurent + angle;
- motorDF.setPower(-viteza_right);
- motorDS.setPower(-viteza_right);
- motorSF.setPower(viteza_left);
- motorSS.setPower(viteza_left);
- angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
- unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
- while(unghiFinal > unghiCurent && runtime.seconds() < 5)
- {
- angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
- unghiCurent = formatAngle(angles.angleUnit, angles.firstAngle);
- telemetry.addData("Unghi_final", unghiFinal);
- telemetry.addData("Curent", unghiCurent);
- telemetry.update();
- }
- motorDF.setPower(0);
- motorDS.setPower(0);
- motorSF.setPower(0);
- motorSS.setPower(0);
- telemetry.update();
- }
- unghiCurent = unghiFinal;
- }
- double formatAngle(AngleUnit angleUnit, double angle) {return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));}
- double formatDegrees(double degrees){return AngleUnit.DEGREES.normalize(degrees); }
- int pozitieCurenta;
- double distantaCurenta;
- public void Movement_Lateral_Timp(double viteza, double timeout)
- {
- runtime.reset();
- motorSF.setPower(-viteza);
- motorDF.setPower( viteza);
- motorDS.setPower(-viteza);
- motorSS.setPower( viteza);
- while (runtime.seconds() <= timeout);
- motorSF.setPower(0);
- motorDF.setPower(0);
- motorDS.setPower(0);
- motorSS.setPower(0);
- }
- int rosu, verde, albastru;
- public void Movement_Fata_Culoare(double viteza, double timeout)
- {
- runtime.reset();
- telemetry.addData("Red ", senzorCuloare.red());
- telemetry.addData("Green", senzorCuloare.green());
- telemetry.addData("Blue ", senzorCuloare.blue());
- rosu = senzorCuloare.red();
- verde = senzorCuloare.green();
- albastru = senzorCuloare.blue();
- while (senzorCuloare.red() <= rosu + 10 && senzorCuloare.blue() <= albastru + 10 && runtime.seconds() <= timeout)
- {
- motorSF.setPower( viteza);
- motorDF.setPower( viteza);
- motorDS.setPower( viteza);
- motorSS.setPower( viteza);
- telemetry.addData("Red ", senzorCuloare.red());
- telemetry.addData("Green", senzorCuloare.green());
- telemetry.addData("Blue ", senzorCuloare.blue());
- telemetry.update();
- }
- motorSF.setPower(0);
- motorDF.setPower(0);
- motorDS.setPower(0);
- motorSS.setPower(0);
- }
- public void coborare(int tickuri_encoder,double distanta, double timeout )
- {
- runtime.reset();
- pozitieCurenta = motorTija.getCurrentPosition();
- distantaCurenta = senzorTija.getDistance(DistanceUnit.CM);
- telemetry.addData("Distanta", distantaCurenta);
- telemetry.addData("Encoder", pozitieCurenta);
- motorTija.setPower(1);
- while (runtime.seconds() <= timeout && (pozitieCurenta <= tickuri_encoder || distantaCurenta >= distanta + 0.1))
- {
- pozitieCurenta = motorTija.getCurrentPosition();
- distantaCurenta = senzorTija.getDistance(DistanceUnit.CM);
- telemetry.addData("Distanta", distantaCurenta);
- telemetry.addData("Encoder", pozitieCurenta);
- telemetry.update();
- }
- motorTija.setPower(0);
- }
- public void Punctare()
- {
- if (poz == 2)
- {
- gyroTurn_right(10, 0.4, 0.4);
- //
- sleep(100);
- gyroTurn_left(-10, -0.4, -0.4);
- }
- else if (poz == 1)
- {
- gyroTurn_right(40, 0.4, 0.4);
- //
- sleep(100);
- gyroTurn_left(-40, -0.4, -0.4);
- }
- else
- {
- gyroTurn_left(-25, -0.4, -0.4);
- //
- sleep(100);
- gyroTurn_right(10, 0.4, 0.4);
- }
- }
- int poz = 0;
- public void detect ()
- {
- initVuforia();
- if (ClassFactory.getInstance().canCreateTFObjectDetector()) {
- initTfod();
- } else {
- telemetry.addData("Sorry!", "This device is not compatible with TFOD");
- }
- if (tfod != null) {
- tfod.activate();
- }
- while(!opModeIsActive())
- {
- if (tfod != null)
- {
- List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
- if (updatedRecognitions != null)
- {
- telemetry.addData("# Object Detected", updatedRecognitions.size());
- if (updatedRecognitions.size() >= 1) {
- int goldMineralX = -1;
- for (Recognition recognition : updatedRecognitions)
- {
- if (recognition.getLabel().equals(LABEL_GOLD_MINERAL))
- {
- goldMineralX = (int) recognition.getTop();
- }
- }
- telemetry.addData("GOLD", goldMineralX);
- if(goldMineralX != -1)
- {
- if (goldMineralX <= 300)
- {
- poz = 1;
- } else if (goldMineralX <= 500)
- {
- poz = 2;
- } else
- {
- poz = 3;
- }
- }
- }
- telemetry.update();
- }
- }
- }
- tfod.shutdown();
- }
- private void initVuforia() {
- VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
- parameters.vuforiaLicenseKey = VUFORIA_KEY;
- parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
- vuforia = ClassFactory.getInstance().createVuforia(parameters);
- }
- private void initTfod() {
- int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
- "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
- TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
- tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
- tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_GOLD_MINERAL, LABEL_SILVER_MINERAL);
- }
- public void initMotoare()
- {
- motorTija = hardwareMap.get(DcMotor.class, "motorTija");
- motorTija.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
- motorTija.setDirection(DcMotorSimple.Direction.REVERSE);
- motorTija.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
- motorExten_fata = hardwareMap.get(DcMotor.class, "motorExten_fata");
- motorExten_fata.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
- motorExten_fata.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
- motorExten_spate = hardwareMap.get(DcMotor.class, "motorExte_spate");
- motorExten_spate.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
- motorExten_spate.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
- motorSwitch = hardwareMap.get(DcMotor.class, "motorSwitch");
- motorSwitch.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
- motorSwitch.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
- motorDS = hardwareMap.get(DcMotor.class, "motorDS");
- motorDS.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
- motorSS = hardwareMap.get(DcMotor.class, "motorSS");
- motorSS.setDirection(DcMotor.Direction.REVERSE);
- motorSS.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
- motorDF = hardwareMap.get(DcMotor.class, "motorDF");
- motorSS.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
- motorSF = hardwareMap.get(DcMotor.class, "motorSF");
- motorSF.setDirection(DcMotor.Direction.REVERSE);
- motorSF.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
- servo_colect = hardwareMap.get(Servo.class, "servo_colect");
- servo_lander = hardwareMap.get(Servo.class, "servo_lander");
- }
- public void initSenzori()
- {
- BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
- parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
- parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
- parameters.calibrationDataFile = "BNO055IMUCalibration.json";
- parameters.loggingEnabled = true;
- parameters.loggingTag = "IMU";
- parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
- imu = hardwareMap.get(BNO055IMU.class, "imu");
- imu.initialize(parameters);
- senzorTija = hardwareMap.get(DistanceSensor.class, "senzorTija");
- senzorFata = hardwareMap.get(DistanceSensor.class, "senzorFata");
- senzorCuloare = hardwareMap.get(ColorSensor.class, "senzorCuloare");
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement