Announcement

Collapse
No announcement yet.

Code either stops randomly or it's simply not working

Collapse
X
 
  • Filter
  • Time
  • Show
Clear All
new posts

  • Code either stops randomly or it's simply not working

    Hello, we're having some critical issues regarding our Autonomous, either it stops with an "stuck in stop()" exception or it doesn't work until we restart the app.
    Any thoughts on why?
    Our battery is fully charged (including the phones), and the connection between the REV Expansion Hun and the R.C is secured and stable.

    our code:
    Code:
    package org.firstinspires.ftc.teamcode.BLUE_autonomous;
    
    import android.graphics.Color;
    
    import com.qualcomm.hardware.bosch.BNO055IMU;
    import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
    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.Servo;
    import com.qualcomm.robotcore.util.ElapsedTime;
    
    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.external.navigation.VuforiaLocalizer;
    import com.qualcomm.robotcore.eventloop.opmode.OpMode;
    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.OpMode;
    import com.qualcomm.robotcore.hardware.DcMotor;
    import com.qualcomm.robotcore.hardware.DcMotorSimple;
    
    import org.firstinspires.ftc.robotcore.external.Telemetry;
    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 android.graphics.Color;
    
    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.CRServo;
    import com.qualcomm.robotcore.hardware.ColorSensor;
    import com.qualcomm.robotcore.hardware.DcMotor;
    import com.qualcomm.robotcore.hardware.DcMotorSimple;
    import com.qualcomm.robotcore.hardware.Servo;
    import com.qualcomm.robotcore.hardware.SwitchableLight;
    import com.qualcomm.robotcore.util.ElapsedTime;
    
    import org.firstinspires.ftc.robotcore.external.ClassFactory;
    import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
    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.external.navigation.RelicRecoveryVuMark;
    import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
    import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
    import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
    
    /**
     * Created by user on 31/01/2018.
     */
    @Autonomous(name = "new_blue_left",group = "blue")
    
    public class ab extends LinearOpMode {
        final double SERVO_UP = 0;
        final double SERVO_DOWN = 0.89;
    
    
        final double TOLERANCE_DEGREES = 8.0;
        // motor init
        DcMotor motorLeft = null;
        DcMotor motorRight = null;
        DcMotor Hdrive = null;
        ColorSensor color = null;
        Servo servoColor = null;
        DcMotor output = null;
        DcMotor elvetor = null;
        DcMotor intake = null;
        DcMotor intake1 = null;
        int  vu = 0;
        //vuforia init
    
        int encodr = 0;
        double power = 0;
        double errorP = 0;
        double errorD = 0;
        int wantedPos = 0;
        double kd = 0.1;
        double kp = 0.04;
        double kpspin = 0.001;
        double per_error = 0;
        double speed = 0;
        VuforiaLocalizer vuforia;
    
        double initiate_angle = 0;
    
        BNO055IMU imu;
    
        Orientation angles;
        Orientation init_angles;
    
        @Override
        public void runOpMode() throws InterruptedException {
    
            int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
            VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
            parameters.vuforiaLicenseKey = "AVO1JFr";
            parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
            this.vuforia = ClassFactory.createVuforiaLocalizer(parameters);
            VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark");
            VuforiaTrackable relicTemplate = relicTrackables.get(0);
            relicTemplate.setName("relicVuMarkTemplate"); // can help in debugging; otherwise not necessary
    
    
    
            motorLeft = hardwareMap.dcMotor.get("left");
            motorRight = hardwareMap.dcMotor.get("right");
            Hdrive = hardwareMap.dcMotor.get("hdrive");
            color = hardwareMap.colorSensor.get("color");
            servoColor = hardwareMap.servo.get("servoColor");
            output = hardwareMap.dcMotor.get("ramp");
            intake = hardwareMap.dcMotor.get("intake");
            intake1 = hardwareMap.dcMotor.get("intake1");
    
    
            motorRight.setDirection(DcMotorSimple.Direction.REVERSE);
            output.setDirection(DcMotorSimple.Direction.REVERSE);
    
    
            imu = hardwareMap.get(BNO055IMU.class, "imu");
            motorLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            motorRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            motorRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            motorLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
    
            motorLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            motorRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
    
            BNO055IMU.Parameters parameters_imu = new BNO055IMU.Parameters();
    
            parameters_imu.angleUnit = BNO055IMU.AngleUnit.DEGREES;
            parameters_imu.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
            parameters_imu.calibrationDataFile = "BNO055IMUCalibration.json"; // see the calibration sample opmode
            parameters_imu.loggingEnabled = true;
            parameters_imu.loggingTag = "IMU";
            parameters_imu.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
    
            imu.initialize(parameters_imu);
            init_angles = imu.getAngularOrientation(AxesReference.INTRINSIC,AxesOrder.ZYX,AngleUnit.DEGREES);
            angles = init_angles;
    //color
    
            initiate_angle = angles.firstAngle;
    
            servo(SERVO_UP);
    
            waitForStart();
    // lemhok
            servo(SERVO_DOWN);
    
    
            relicTrackables.activate();
            sleep(800);
            vuReco(relicTemplate);
            relicTrackables.deactivate();
    
    
            //color
            colorreco();
    
            driveWithEncoders(0.4,1650);
            drivegyroback(0.3,-1350);
            //drivegyro(-0.3, -1400);
            //turnRight(90);
            sleep(300);
            driveSpin(0.4, 1160);
            driveWithEncoders(0.4,-500);
            // RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate);
            if (vu== 1) {
                driveSpin(0.4,400);
                output.setPower(-0.6);
                sleep(800);
                output.setPower(0.8);
                sleep(800);
                output.setPower(0);
                driveWithEncoders(0.3,300);
                driveWithEncoders(0.3,-300);
                driveSpin(0.4,-400);
                sleep(500);
    
    
            }
    
            else if (vu == 2) {
                driveSpin(0.4,-400);
                output.setPower(-0.6);
                sleep(800);
                output.setPower(0.8);
                sleep(800);
                output.setPower(0);
                driveWithEncoders(0.3,300);
                driveWithEncoders(0.3,-300);
                driveSpin(0.4,400);
                sleep(500);
    
    
    
            }
            else {
                output.setPower(-0.5);
                sleep(500);
                output.setPower(0.9);
                sleep(500);
                output.setPower(0);
    
            }
    
            //isof box
            intake.setPower(0.5);
            intake1.setPower(-0.5);
            driveWithEncoders(0.5,3500);
            sleep(500);
            intake.setPower(0);
            intake1.setPower(0);
            driveWithEncoders(0.4,-3700);
    
            //plitat box
            //pitograpia(VuforiaTrackable relicTemplate);
            driveWithEncoders(0.3,100);
            output.setPower(-0.5);
            sleep(800);
            output.setPower(0.4);
            driveWithEncoders(0.25,-100);
            output.setPower(0);
            driveWithEncoders(0.7,300);
    
    
    
            motorLeft.setPower(0);
            motorRight.setPower(0);
            motorLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            motorRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    
        }
    
        public void driveWithEncoders(double power, int position) {
    
    
            motorLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            motorRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    
            this.motorLeft.setTargetPosition(-position);
            this.motorRight.setTargetPosition(-position);
    
            this.motorLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            this.motorRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
    
            this.motorLeft.setPower(power);
            this.motorRight.setPower(power);
    
            telemetry.addData("right encoder:", motorRight.getCurrentPosition());
            telemetry.addData("left encoder:", motorLeft.getCurrentPosition());
    
            telemetry.update();
            while (opModeIsActive() && (this.motorLeft.isBusy() || this.motorRight.isBusy())) {
                telemetry.update();
                this.idle();
            }
            telemetry.update();
        }
        public void servo(double pos) {
    
            telemetry.addLine("cccc");
            servoColor.setPosition(pos);
            sleep(1000);
    
            telemetry.update();
        }
    
    
        public void driveSpin(double power, int position) {
            motorLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            motorRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    
            this.motorRight.setTargetPosition(position);
            this.motorLeft.setTargetPosition(-position);
    
    
            this.motorRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            this.motorLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
    
            this.motorRight.setPower(power);
            this.motorLeft.setPower(power);
    
    
            while (opModeIsActive() && (this.motorRight.isBusy() || (this.motorLeft.isBusy()))) {
                telemetry.update();
                this.idle();
            }
            telemetry.update();
        }
    
    
        public void colorreco() {
            //servo(SERVO_DOWN);
            //driveSpin(0.3,100);
            if (color.red() > color.blue()) {
                driveSpin(0.3, 250);
                servo(SERVO_UP);
                driveSpin(0.3, -250);
            } else if (color.red() < color.blue()) {
                driveSpin(0.3, -250);
                servo(SERVO_UP);
                driveSpin(0.3, 250);
            }
            else {
                driveWithEncoders(0.4,-200);
            }
        }
    
    
        public void drivegyroback(double power, int encodr) {
            motorLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            motorRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            motorRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            motorLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            motorLeft.setTargetPosition(encodr);
            motorRight.setTargetPosition(encodr);
            angles.firstAngle=hashCode();
            //drive forward
            while (encodr <= motorRight.getCurrentPosition() & encodr <= motorLeft.getCurrentPosition()) {
                angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
                errorP = angles.firstAngle - initiate_angle;
                errorD = errorP - per_error;
                speed = kp * errorP + kd * errorD;
                per_error = errorP;
                motorLeft.setTargetPosition(encodr);
                motorRight.setTargetPosition(encodr);
    
                if (angles.firstAngle > 1) {
                    motorLeft.setPower(-speed);
    
                }
                if (angles.firstAngle < -1) {
                    motorRight.setPower(-speed);
    
                } else {
                    motorRight.setPower(-power);
                    motorLeft.setPower(-power);
    
    
                }
    
            }
            motorRight.setPower(0);
            motorLeft.setPower(0);
    
        }
    
        public void vuReco(VuforiaTrackable relicTemplate) {
    
            RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate);
            if (vuMark == RelicRecoveryVuMark.RIGHT) {
                vu=1;
                telemetry.addData("vu",vu);
            }
    
    
            else if (vuMark == RelicRecoveryVuMark.LEFT) {
                vu=2;
                telemetry.addData("vu",vu);
            }
            telemetry.update();
    
    
        }
    
    }

  • #2
    Several things...

    1)

    Change
    public void runOpMode() throws InterruptedException { to public void runOpMode() { The InterruptedException is not desired any more.

    2)
    Your gyro code has a bad while loop in it...
    while (encodr <= motorRight.getCurrentPosition() & encodr <= motorLeft.getCurrentPosition()) { does not have a test for isOpmodeActive() in it, so this may be causing the Stuck In Loop error. 3)
    To make your telemetry more useful, I would put the telemetry.update() call right after you set the message, rather than waiting till after the sleep:
    So, instead of:
    Code:
     
             telemetry.addLine("cccc");         servoColor.setPosition(pos);         sleep(1000);         telemetry.update();
    do:
    Code:
      
             telemetry.addLine("cccc");         telemetry.update();         servoColor.setPosition(pos);         sleep(1000);
    4) If you want to see what your motors are doing, you need to put the telemetry inside the loop" Like this:
    Code:
              while (opModeIsActive() && (this.motorLeft.isBusy() || this.motorRight.isBusy())) {             telemetry.addData("right encoder:", motorRight.getCurrentPosition());              telemetry.addData("left encoder:", motorLeft.getCurrentPosition());              telemetry.update();             this.idle();         }
    5) drivegyroback()
    If you are in run to position mode, the requested speed/power should be a positive number. So your addition of a - sign is redundant. The direction is determined by the current position and target position. If you want to go backwards, you need to put in a negative target position. I would display your speed numbers to make sure they are reasonable. In fact, it looks like you are trying to control the speed, AND use run to position. This can work sometimes, but you code is very confusing. You may want to use RUN_USING_ENCODERS instead, then you can set the speed and direction.

    Comment


    • #3
      Thanks Philbot, sorry for the confusing code, this is our first year programming with Android Studio + Java.

      Comment

      Working...
      X