Announcement

Collapse
No announcement yet.

Odd problem with Core Device Interface (seems to impact both Color Sensor and ODS)

Collapse
This topic is closed.
X
X
 
  • Filter
  • Time
  • Show
Clear All
new posts

  • #16
    mikets, if you have an ODS handy, can you run the same code with just it mounted? I'm suspicious that what might be happening is that for all devices actually declared in the setup, it's polling all of them each time. If the ODS by itself is quick, but then placing a gyro, declaring it, but never accessing it causes the times to increase, then this would imply a "get everything" approach. I wish someone on the SDK team could comment. I sent Tom a PM asking him to take a look here. Philbot also seems to have some inside knowledge, or perhaps he's just dug into it a lot.
    Technical Coach, Newton Busters FTC 10138, FLL 3077
    Chicago 'burbs, IL

    Comment


    • #17
      Originally posted by mikets View Post
      I am glad you are looking so deep into this issue. That made me curious too so I did a little experiment today with the I2C gyro and it surprised me. We always had a hard time tuning our PID constants for turns. So I logged some gyro data to see what's going on and I found that while the robot is turning, the gyro heading changes its value at an average interval of 300 ms even though our loop time is about 10 ms!!! It means the heading doesn't change for approx. 30 loops. Since our robot has 4 I2C sensors, I unplugged all other I2C sensors leaving only the gyro so to eliminate the I2C bus congestion issue but the 300 ms interval remained the same. Now this preliminary data is not very representative because I was using the competition code which still attempted to read the unplugged I2C devices. I am going to write a test opmode tomorrow with only gyro in it and test it again.
      Well now that's interesting, as it seemed to me that our BNO055's heading was always lagging a bit.

      I'm going to have to run some timing tests as well...

      Comment


      • #18
        mikets, just some added info. The testing we're doing is also on the kids competition robot. It has a gyro, plus two additional color sensors for redundancy when testing the beacon colors (they look at both sides to ensure they have the correct color). So if the added I2C sensors impact the refresh time (especially for an analog sensor), and despite their not being used in this portion of code, that will be important.

        I did receive a reply from support at Modern Robotics. They are looking into my questions on the hardware side. Tom also replied to my PM with some experimental data. I've asked if he could post it here. Again, with a simple robot and only one sensor (ODS), his refresh rate looks pretty fast. I haven't had time to dig into the numbers yet (day job).
        Technical Coach, Newton Busters FTC 10138, FLL 3077
        Chicago 'burbs, IL

        Comment


        • #19
          Our team made some changes to the robot's drive train that requires retuning of all PIDs of the drive train. We retuned all other PIDs fine but for turn PID, we have a hard time making it turn accurately. Looking at the trace log, we can see the gyro sampling interval played a big part on that. Knowing that I think it is still possible to tune it to be acceptably accurate but it may take time. We have our next competition in a week. So we need to make a quick decision on whether we can tune it accurately or we will undo the changes. Tonight, we will focus on trying to tune the turning PID. If we can't then we need to undo. So it may take me a while to get back to the experiment. However, I do want to do another quick experiment. That is to try our own Modern Robotics Gyro driver. When we add I2C device support to our library, as a test, we wrote a Modern Robotics I2C driver to make sure it does everything the FTC SDK's built-in gyro driver does. But of course, the internal implementation of our driver may be different from the SDK's. This will be a quick test. If it doesn't change anything, then there is no conclusion. But if it improves the timing, then it means the I2C access implementation plays a role.

          Comment


          • #20
            I tried some testing with ODS. When running with just an ODS, it looks like it gets updated about every 5 milliseconds. When I add a color sensor, it looks like the ODS value is updated about every 15 to 20 milliseconds.
            How many devices do you have connected? My understanding is that it does poll but I thought it was able to complete its polling in under 50 milliseconds. If I get my gyro code working then I will add it to the test.

            This is my code to collect the data (I commented out the telemetry to make sure it didn't impact the timing):

            Code:
            package org.firstinspires.ftc.teamcode;
            
            import com.qualcomm.robotcore.eventloop.opmode.OpMode;
            import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
            import com.qualcomm.robotcore.hardware.ColorSensor;
            import com.qualcomm.robotcore.hardware.OpticalDistanceSensor;
            import com.qualcomm.robotcore.util.ElapsedTime;
            
            import java.io.BufferedWriter;
            import java.io.File;
            import java.io.FileWriter;
            import java.io.IOException;
            import java.io.Writer;
            
            /**
             * Created by KSHA on 11/30/2016.
             */
            @TeleOp(name = "Test: ODS", group = "Test")
            public class OdsTester extends OpMode {
                private ElapsedTime runtime = new ElapsedTime();
                OpticalDistanceSensor odsSensor;  // Hardware Device Object
                private Writer fileWriter;
                private String outString;
             //   ColorSensor colorSensor;    // Hardware Device Object
            
                @Override
                public void init() {
                    odsSensor = hardwareMap.opticalDistanceSensor.get("sensor_ods");
                    // bLedOn represents the state of the LED.
                    boolean bLedOn = true;
            
                    // get a reference to our ColorSensor object.
             //       colorSensor = hardwareMap.colorSensor.get("color");
            
                    // Set the LED in the beginning
             //       colorSensor.enableLed(bLedOn);
            
                    telemetry.addData("Status", "Initialized");
                    try {
                        FileWriter writer = new FileWriter(this.hardwareMap.appContext.getExternalFilesDir(null) + File.separator + "out.csv");
                        fileWriter = new BufferedWriter(writer);
                    } catch (IOException e) {
                        throw new RuntimeException("Cannot write to file", e);
                    }
                }
            
                /*
                   * Code to run when the op mode is first enabled goes here
                   * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start()
                   */
                @Override
                public void init_loop() {
                }
            
                /*
                 * This method will be called ONCE when start is pressed
                 * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop()
                 */
                @Override
                public void start() {
                    runtime.reset();
                }
            
                /*
                 * This method will be called repeatedly in a loop
                 * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop()
                 */
                @Override
                public void loop() {
            /*
                    telemetry.addData("Raw",    odsSensor.getRawLightDetected());
                    telemetry.addData("Normal", odsSensor.getLightDetected());
                    telemetry.addData("Status", "Run Time: " + runtime.toString());
                    telemetry.addData("Clear", colorSensor.alpha());
                    telemetry.addData("Red  ", colorSensor.red());
                    telemetry.addData("Green", colorSensor.green());
                    telemetry.addData("Blue ", colorSensor.blue());
                    telemetry.update();
            */
                    outString = Double.toString(runtime.seconds()) + "," + Double.toString(odsSensor.getLightDetected()) +
                            "," + Double.toString(odsSensor.getRawLightDetected()) + 'n';
            //                "," + Double.toString(colorSensor.alpha()) +
            //                "," + Double.toString(colorSensor.red()) +
            //                "," + Double.toString(colorSensor.green()) +
            //                "," + Double.toString(colorSensor.blue()) + "n";
                    writeToFile(outString);
                }
            
                @Override
                public void stop() {
                    try {
                        fileWriter.close();
                    } catch (IOException e) {
                        throw new RuntimeException("Cannot close file", e);
                    }
                }
            
                private void writeToFile(String data) {
                    try {
                        fileWriter.write(data);
                    } catch (IOException e) {
                        throw new RuntimeException("Cannot write to file", e);
                    }
                }
            }

            Comment


            • #21
              Originally posted by KHoyt View Post
              I tried some testing with ODS. When running with just an ODS, it looks like it gets updated about every 5 milliseconds. When I add a color sensor, it looks like the ODS value is updated about every 15 to 20 milliseconds.
              How many devices do you have connected? My understanding is that it does poll but I thought it was able to complete its polling in under 50 milliseconds. If I get my gyro code working then I will add it to the test.

              This is my code to collect the data (I commented out the telemetry to make sure it didn't impact the timing):

              Code:
              package org.firstinspires.ftc.teamcode;
              
              import com.qualcomm.robotcore.eventloop.opmode.OpMode;
              import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
              import com.qualcomm.robotcore.hardware.ColorSensor;
              import com.qualcomm.robotcore.hardware.OpticalDistanceSensor;
              import com.qualcomm.robotcore.util.ElapsedTime;
              
              import java.io.BufferedWriter;
              import java.io.File;
              import java.io.FileWriter;
              import java.io.IOException;
              import java.io.Writer;
              
              /**
               * Created by KSHA on 11/30/2016.
               */
              @TeleOp(name = "Test: ODS", group = "Test")
              public class OdsTester extends OpMode {
                  private ElapsedTime runtime = new ElapsedTime();
                  OpticalDistanceSensor odsSensor;  // Hardware Device Object
                  private Writer fileWriter;
                  private String outString;
               //   ColorSensor colorSensor;    // Hardware Device Object
              
                  @Override
                  public void init() {
                      odsSensor = hardwareMap.opticalDistanceSensor.get("sensor_ods");
                      // bLedOn represents the state of the LED.
                      boolean bLedOn = true;
              
                      // get a reference to our ColorSensor object.
               //       colorSensor = hardwareMap.colorSensor.get("color");
              
                      // Set the LED in the beginning
               //       colorSensor.enableLed(bLedOn);
              
                      telemetry.addData("Status", "Initialized");
                      try {
                          FileWriter writer = new FileWriter(this.hardwareMap.appContext.getExternalFilesDir(null) + File.separator + "out.csv");
                          fileWriter = new BufferedWriter(writer);
                      } catch (IOException e) {
                          throw new RuntimeException("Cannot write to file", e);
                      }
                  }
              
                  /*
                     * Code to run when the op mode is first enabled goes here
                     * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start()
                     */
                  @Override
                  public void init_loop() {
                  }
              
                  /*
                   * This method will be called ONCE when start is pressed
                   * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop()
                   */
                  @Override
                  public void start() {
                      runtime.reset();
                  }
              
                  /*
                   * This method will be called repeatedly in a loop
                   * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop()
                   */
                  @Override
                  public void loop() {
              /*
                      telemetry.addData("Raw",    odsSensor.getRawLightDetected());
                      telemetry.addData("Normal", odsSensor.getLightDetected());
                      telemetry.addData("Status", "Run Time: " + runtime.toString());
                      telemetry.addData("Clear", colorSensor.alpha());
                      telemetry.addData("Red  ", colorSensor.red());
                      telemetry.addData("Green", colorSensor.green());
                      telemetry.addData("Blue ", colorSensor.blue());
                      telemetry.update();
              */
                      outString = Double.toString(runtime.seconds()) + "," + Double.toString(odsSensor.getLightDetected()) +
                              "," + Double.toString(odsSensor.getRawLightDetected()) + 'n';
              //                "," + Double.toString(colorSensor.alpha()) +
              //                "," + Double.toString(colorSensor.red()) +
              //                "," + Double.toString(colorSensor.green()) +
              //                "," + Double.toString(colorSensor.blue()) + "n";
                      writeToFile(outString);
                  }
              
                  @Override
                  public void stop() {
                      try {
                          fileWriter.close();
                      } catch (IOException e) {
                          throw new RuntimeException("Cannot close file", e);
                      }
                  }
              
                  private void writeToFile(String data) {
                      try {
                          fileWriter.write(data);
                      } catch (IOException e) {
                          throw new RuntimeException("Cannot write to file", e);
                      }
                  }
              }
              You should do the test using LinearOpMode. In the loop method of the regular OpMode, you return to the FTC SDK and it runs a lot of things before calling you again. So to have complete control, do it in the while (opModeIsActive()) loop of the LinearOpMode.

              Comment


              • #22
                KHoyt, your code only accessed the ODS. There is no code to access the color sensor. So are you saying you just physically plugged in a color sensor to the I2C bus and not even adding any code to access the color sensor and the ODS access interval becomes longer? Did you add the color sensor to the Robot Config? If not, I would be very surprise.

                Comment


                • #23
                  Our next league round is this weekend so we haven't been able to do any more testing. I also haven't had time to look more since in addition to an FTC event on Saturday, our FLL team has their regional on Sunday. Busy weekend. Still hoping to see more data from other teams on this to determine if the issue is ours alone (but mikets's data with 300ms gyro persistence seems to indicate it's more widespread). It seems that with only a single ODS sensor fast updates are possible. But, the question is what happens as a more autonomous rich sensor environment is in place. I mean, the CDI has all those ports, it can't mean that populating them will slow everything down to un-usability
                  Technical Coach, Newton Busters FTC 10138, FLL 3077
                  Chicago 'burbs, IL

                  Comment


                  • #24
                    Originally posted by mikets View Post
                    Our team made some changes to the robot's drive train that requires retuning of all PIDs of the drive train. We retuned all other PIDs fine but for turn PID, we have a hard time making it turn accurately. Looking at the trace log, we can see the gyro sampling interval played a big part on that. Knowing that I think it is still possible to tune it to be acceptably accurate but it may take time. We have our next competition in a week. So we need to make a quick decision on whether we can tune it accurately or we will undo the changes. Tonight, we will focus on trying to tune the turning PID. If we can't then we need to undo. So it may take me a while to get back to the experiment.
                    We did some more tuning last night. It improved but still not with the accuracy we'd like. Our robot was turning consistently during last competition. Then we made the drive train change and also lots of software changes. We are going to restore the software back to the version of last competition and try it again. If still no go, then we will undo the drive train changes.

                    Comment


                    • #25
                      I ran 10 different configurations, 5 linear opmode, 5 iterative opmode. 1 ODS Only, 2 ODS with Color Sensor object instantiated but not used, 3 ODS with Colors Sensor and Color Sensor Read, 4. ODS, Color Sensor Used, IMU instantiated but not used, and 5. ODS, Color Sensor, and IMU read.

                      All of them were set to run as fast as possible.

                      Hold Time Std Dev Min Max Sample Std Dev Min Max
                      ODS Only Iterative 6.14 1.80 0.44 24.46 5.20 1.78 0.35 16.12
                      ODS Only Linear 4.65 2.28 0.17 31.71 0.14 0.37 0.08 25.04
                      ODS Color Iterative 12.47 3.68 1.41 36.64 5.27 1.76 0.39 20.49
                      ODS Color Linear 11.73 3.94 2.91 44.09 0.13 0.38 0.08 28.52
                      ODS Color Iterative Use 12.40 3.65 3.76 46.20 5.03 2.00 0.46 14.41
                      ODS Color Linear Use 12.13 4.29 2.88 30.00 0.16 0.36 0.10 19.30
                      ODS Color IMU Iterative 19.44 5.70 5.52 50.08 5.16 1.82 0.35 16.32
                      ODS Color IMU Linear 16.01 6.19 2.21 49.84 0.17 0.39 0.10 27.02
                      ODS Color IMU Iterative Use 18.98 5.98 2.66 61.06 3.11 2.46 0.53 16.05
                      ODS Color IMU Linear Use 15.87 5.48 1.12 44.49 0.23 0.40 0.13 20.93

                      In summary, Independent of OpMode, adding a device increased the duration at which the ODS "held" its value. ODS Only 5 milliseconds, ODS + Color 12 Milliseconds, ODS + Color + IMU 18 Milliseconds. Also the worst case (max) hold time increased as sensors were added.

                      Just adding a sensor had the same effect as adding and reading the sensor to how long ODS held a constant value.

                      In general, the Iterative OpMode ran at about 200 Hz (5 milliseconds) independent of adding functionality. The Linear OpMode started at about 7000 Hz and slowed to about 4000 Hz as functionality was added.

                      Although not shown, the IMU would update its fused heading data at about 50 Hz (every 21.2 milliseconds). It was programmed to run at 50 Hz.

                      Sorry the table is not formatting well in the forum. The editing of it has nice neat columns but it looks like the extra white space to align to columns gets removed when posted.

                      Comment


                      • #26
                        Originally posted by KHoyt View Post
                        I ran 10 different configurations, 5 linear opmode, 5 iterative opmode. 1 ODS Only, 2 ODS with Color Sensor object instantiated but not used, 3 ODS with Colors Sensor and Color Sensor Read, 4. ODS, Color Sensor Used, IMU instantiated but not used, and 5. ODS, Color Sensor, and IMU read.

                        All of them were set to run as fast as possible.

                        Hold Time Std Dev Min Max Sample Std Dev Min Max
                        ODS Only Iterative 6.14 1.80 0.44 24.46 5.20 1.78 0.35 16.12
                        ODS Only Linear 4.65 2.28 0.17 31.71 0.14 0.37 0.08 25.04
                        ODS Color Iterative 12.47 3.68 1.41 36.64 5.27 1.76 0.39 20.49
                        ODS Color Linear 11.73 3.94 2.91 44.09 0.13 0.38 0.08 28.52
                        ODS Color Iterative Use 12.40 3.65 3.76 46.20 5.03 2.00 0.46 14.41
                        ODS Color Linear Use 12.13 4.29 2.88 30.00 0.16 0.36 0.10 19.30
                        ODS Color IMU Iterative 19.44 5.70 5.52 50.08 5.16 1.82 0.35 16.32
                        ODS Color IMU Linear 16.01 6.19 2.21 49.84 0.17 0.39 0.10 27.02
                        ODS Color IMU Iterative Use 18.98 5.98 2.66 61.06 3.11 2.46 0.53 16.05
                        ODS Color IMU Linear Use 15.87 5.48 1.12 44.49 0.23 0.40 0.13 20.93

                        In summary, Independent of OpMode, adding a device increased the duration at which the ODS "held" its value. ODS Only 5 milliseconds, ODS + Color 12 Milliseconds, ODS + Color + IMU 18 Milliseconds. Also the worst case (max) hold time increased as sensors were added.

                        Just adding a sensor had the same effect as adding and reading the sensor to how long ODS held a constant value.

                        In general, the Iterative OpMode ran at about 200 Hz (5 milliseconds) independent of adding functionality. The Linear OpMode started at about 7000 Hz and slowed to about 4000 Hz as functionality was added.

                        Although not shown, the IMU would update its fused heading data at about 50 Hz (every 21.2 milliseconds). It was programmed to run at 50 Hz.

                        Sorry the table is not formatting well in the forum. The editing of it has nice neat columns but it looks like the extra white space to align to columns gets removed when posted.
                        It makes sense that adding devices to the config file will decrease the update rate, regardless of whether or not they are actually used. The devices in the hardware map are instantiated based on the config file regardless of whether or not you actually take them out of the hardware map, and most of the original I2C drivers were implemented as repeatedly reading a set of registers and then allowing you to fetch the last read value. This means that simply having constructed the sensor object results in it repeatedly using the I2C bus. When you have several of these devices, their traffic will needlessly hog the DIM's I2C bus for a substantial amount of time each hardware cycle, making every other device return values more slowly. For this reason, in our I2C drivers, the user must manually indicate that they want to start issuing reads. It's a trade-off between having a (possibly quite stale) value always available and getting updated values frequently, and I think FIRST decided it would make it easier for newcomers if the drivers always issued reads in the background. New drivers, like those built for the compass and range sensors, use I2cDeviceSynch, so I am not sure whether they also suffer from this problem. However, the color sensor, gyro, and IR seeker sensor are all based off I2cControllerPortDeviceImpl and explicitly issue continual reads. I don't believe the analog sensors indicate to the DIM that they would like to read data; every analog input is just read each hardware cycle regardless.

                        Comment


                        • #27
                          Okay, we did some more tests tonight and it is conclusive that the number of I2C devices does affect the sampling time of the sensors. First, we configured the robot with only one gyro sensor and its sampling time is about 50 msec. Then we went the other end to have 4 I2C sensors (1 gyro, 2 color sensors and one range sensor). This config made the gyro sampling time to become 300 msec. Then I took the range sensor off, then the gyro sampling time went to 240 msec. Then in addition, I took one of the color sensors off. The gyro sampling time became 150 msec. So in general, the addition of each sensor adds between 50 to 100 msec. Wow, this is very bad. There is no way to do close loop control with this kind of sampling time. We need to have FIRST look into the cause of this delay whether there is an architecture issue with the I2C device support. The 100 kbps I2C bus is slow but it alone can't explain the fast degradation of the sampling time. Rough math says 100 kbps yielded about 10 kByte per second (assuming rounding to 10 bits for byte). If a sensor sampling time is 100 msec, it means it could have communicated 1 kbyte of data within this time. Even with 50 msec sampling time, it would have communicated half a kbyte (assuming the actual data acquisition on the hardware take insignificant time). So there is major overhead somewhere.
                          In the meantime, our team will try to minimize the number of I2C sensors on the bus just to make the gyro reading acceptable. I know two sensors are fine and 3 is stretching it. Four is definitely not acceptable.

                          Comment


                          • #28
                            Originally posted by GearTicks View Post
                            For this reason, in our I2C drivers, the user must manually indicate that they want to start issuing reads. It's a trade-off between having a (possibly quite stale) value always available and getting updated values frequently, and I think FIRST decided it would make it easier for newcomers if the drivers always issued reads in the background.
                            I am also looking into using our own I2C drivers in place of the ones provided by the FTC SDK. We wrote our own drivers (e.g. Gyro and color sensors) just to test the I2C device support in our library. These drivers used to have a feature to enable/disable it at will. So for example, for the line detection color sensor, there is no reason to enable it until I am looking for a line. And it is unlikely that I need to look for a line when turning with gyro. So it will hopefully cut down the gyro sampling time. Unfortunately, FIRST has changed the SDK this year with new I2C support. We had to modify our I2C device infrastructure in the library correspondingly and apparently, we are now using their sync implementation which means it is no longer "raw". When we did it "raw", we can control whether we read the data or not. But now, we gave up that option. I will look into the possibility of restore that feature. Does anybody know if the "raw" access of I2C is still supported? I may resurrect the old driver and try to make it work.

                            Comment


                            • #29
                              GearTicks, yes I agree the numbers make sense. And the measured values of 5 to 6 milliseconds per sensor are close to PhilBots estimation of 8 to 10 milliseconds per sensor. But Mikets and gof are seeing 50 milliseconds and greater. Up to a few hundred milliseconds. These numbers seem excessive as if something else is causing it. my test does not include motor controllers nor servo controllers.

                              Comment


                              • #30
                                Originally posted by KHoyt View Post
                                GearTicks, yes I agree the numbers make sense. And the measured values of 5 to 6 milliseconds per sensor are close to PhilBots estimation of 8 to 10 milliseconds per sensor. But Mikets and gof are seeing 50 milliseconds and greater. Up to a few hundred milliseconds. These numbers seem excessive as if something else is causing it. my test does not include motor controllers nor servo controllers.
                                Bear in mind that we are using the FTC SDK I2C support layers, like I2CDeviceSynch and features like window block read. I believe once the I2C device is declared in the robot config, the I2C device support will probably start reading a block of registers from the device repeatedly in anticipation of somebody requesting data from the device. The device data will be kept in its cache. So when you try to "getData" from the device, it's not from the hardware. It's from the cache. So it really depends on how often the cache is "refreshed". It could contain stale data. That's the "sampling interval" we are looking at. It would have been nice if FTC SDK provides a method to "start/stop" the device reader because there is no reason for the color sensor to start until I am at the beacon. So if most of the I2C devices are "stopped", then there is more bandwidth for the other devices. I assume FTC SDK is supporting some sort of serialized request queue for the I2C bus. So if only one or two devices are started, the request queue will only contain those requests and they will be serviced more frequently.

                                Comment

                                Working...
                                X