IMU initialization time

[FTC12676]

We have two rev hubs and on both IMU initialization takes more than 5 sec and gives stuck in Init error. I increased Init watchdog timer to 10 sec and it works fine after that. This is exactly same behavior on sample code also. Any idea why it take so long? Anything we need to check?

[Philbot]

Hi

It sounds like you are using the standard Opmode (rather than the LinearOpmode) yes?

My experience is that the IMU does not take that long to startup, but even if it does, you should not be waiting in a while loop, inside the any of the opmode methods..
If you want to do this, you should be using linearOpmode

Can you show your startup code?
There's probably a simple way to prevent the stuck in loop (Extending the timeout is not particularly safe, with respect to emergency situations)

[FTC12676]

Phil - we changed our opmode to linear now. Here is how we are initializing IMU in init method of sisBot class -

 
 public void init(HardwareMap _hardwareMap, Telemetry _telemetery, LinearOpMode _opMode) {     this.hwMap = _hardwareMap;     this.telemetry = _telemetery;     this.opMode = _opMode;       // Define and Initialize Motors     leftFront = hwMap.get(DcMotor.class, "leftFront");     leftBack = hwMap.get(DcMotor.class, "leftBack");     rightFront = hwMap.get(DcMotor.class, "rightFront");     rightBack = hwMap.get(DcMotor.class, "rightBack");     // Glyph Arm Motor     glyphArm = hwMap.get(DcMotor.class, "glyphArm");      leftFront.setDirection(DcMotorSimple.Direction.REVERSE);     leftBack.setDirection(DcMotorSimple.Direction.REVERSE);     // Set all motors to zero power     leftFront.setPower(0);     leftBack.setPower(0);     rightFront.setPower(0);     rightBack.setPower(0);     glyphArm.setPower(0);      // Set all motors to reset     this.setMotorMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER, leftFront, leftBack, rightFront, rightBack, glyphArm);     //glyphArm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);     //leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);     //leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);     //rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);     //rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);      // Set motor modes     this.setDriveRunUsingEncoder();     this.setGlyphRunToPosition();      // Initalize servos     // Define and initialize ALL installed servos.     leftClaw = hwMap.get(Servo.class, "leftClaw");     rightClaw = hwMap.get(Servo.class, "rightClaw");     JArm = hwMap.get(Servo.class, "JArm");      leftClaw.setPosition(LEFT_CLAW_HOME);     rightClaw.setPosition(RIGHT_CLAW_HOME);     JArm.setPosition(JArm_HOME);     telemetry.addLine("HW init complete.. starting IMU init");     telemetry.update();       // NEw set of paras     BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();     parameters.mode = BNO055IMU.SensorMode.IMU;     parameters.useExternalCrystal = true;     parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;     parameters.pitchMode = BNO055IMU.PitchMode.WINDOWS;     parameters.loggingEnabled = true;     parameters.loggingTag = "IMU";       // Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port     // on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU",     // and named "imu".     imu = hwMap.get(BNO055IMU.class, "imu");     telemetry.addLine("IMU created...");     telemetry.update();     imu.initialize(parameters);      telemetry.addLine("IMU init complete...");     telemetry.update();      // Coolor Sensor     //revColorDistance = new REVColorDistance();     //revColorDistance.init((hwMap.get(ColorSensor.class, "color"), hwMap.get(DistanceSensor.class, "color"));   }

Opmode calls above init method as below -

 
 public class SisBotTeleop extends LinearOpMode {      // Declare OpMode members.     private SisRobot sisRobot = new SisRobot();     private boolean useFC = false;     private double driveScale = 0.1;      @Override     public void runOpMode() {          // Initialize the hardware variables.         sisRobot.init(hardwareMap, telemetry,this);          // Wait for the game to start (driver presses PLAY)         waitForStart();         telemetry.addData("Gyro Ready?", sisRobot.isGyroCalibrated() ? "YES" : "no.");           // run until the end of the match (driver presses STOP)         while (opModeIsActive()) { // do actions } }

[FTC12676]

in sisBot code where we initialize IMU, it always stuck or take long time executing following statement -

 
 imu.initialize(parameters);

I tried going up in hierarchy and found class BNO055IMUImpl where actual initialization happens. Looking at following initialization code and comment in this class, it looks like this problem is well known.

 
 @Override public boolean internalInitialize(@NonNull Parameters parameters)     {     if (parameters.mode==SensorMode.DISABLED)         return false;      // Remember parameters so they're accessible starting during initialization.     // Disconnect from user parameters so he won't interfere with us later.     Parameters prevParameters = this.parameters;     this.parameters = parameters.clone();      // Configure logging as desired (we no longer log at the I2C level)     // this.deviceClient.setLogging(this.parameters.loggingEnabled);     // this.deviceClient.setLoggingTag(this.parameters.loggingTag);      // Make sure we're talking to the correct I2c address     this.deviceClient.setI2cAddress(parameters.i2cAddr);      // We retry the initialization a few times: it's been reported to fail, intermittently,     // but, so far as we can tell, entirely non-deterministically. Ideally, we'd like that to     // never happen, but in light of our (current) inability to figure out how to prevent that,     // we simply retry the initialization if it seems to fail.      SystemStatus expectedStatus = parameters.mode.isFusionMode() ? SystemStatus.RUNNING_FUSION : SystemStatus.RUNNING_NO_FUSION;      for (int attempt=0; !Thread.currentThread().isInterrupted() && attempt < 5; attempt++)         {         if (internalInitializeOnce(expectedStatus))             {             this.isInitialized = true;             return true;             }          // Hack: try again with more delay next time         delayScale = Math.min(3, delayScale * 1.2f);         log_w("retrying IMU initialization");         }      log_e("IMU initialization failed");     this.parameters = prevParameters;     return false;     }

[FTC12676]

sorry not sure why code is showing in one line. Is there a way to format it properly? I will repost

[FTC12676]

In addition - we tried replacing USB cable between phone and expansion hub, using IMUs from both hubs one at a time.

[clevergreed10]

I'm not totally sure as to why it's taking you 5+ seconds to initialize the imu, but I do know that you shouldn't be initializing the imu inside your hardware init. Our team used to do that until we realized it was causing our robot to crash with the error "Stuck in init()". Moving the initialization of the imu into your SisBotTeleop class before the waitForStart() call could fix your problem, it worked for us.

[FTC12676]

It is calling initialization before waitForStart() -
public void runOpMode() { // Initialize the hardware variables. sisRobot.init(hardwareMap, telemetry,this); // Wait for the game to start (driver presses PLAY) waitForStart(); However is it is initializing in sisRobot.init() but it is exactly same code. how will it matter if it is part of hardware class vs inline in teleop?

[clevergreed10]

I don't know why it's happening,I saw a forum post where a team was getting the stuck in init() problem and it was because they were trying to wait for their gyro to calibrate before starting their opMode. I can't find the post though.

All I can say is that you should (1) try initializing the imu inside of your opMode but outside of any sort of init() function, and (2) don't use any loops trying to pause your code before the imu been fully set up. Our Autonomous is a LinearOpMode and all we're doing (in terms of imu) is initialize, waitForStart(), run commands and we haven't gotten any problems.

[FTC12676]

Thanks. I will try your suggestion tomorrow and post observations