✨Feature/redundant imu

[HakopZ]

…od imu collection and bad one, allowing for imus to switch between the two. Also added deconstructor. Needs testing from jess for memory leaks, and for threshold tuning (constant defined in util). Big change so matters a lot with teams.

Summary and Motivation:

These changes needed to be to allow for redundant imu support, switching the focused imu to good imus while bad imus disconnect or become invalid. I did clean up the ptr to the bad imus once proven always bad. Also switching from good the bad was handled with time and pros::millis(), which should be precise enough. Later on a change to unique and shared ptrs, would probably be needed but this is a quick, but sound fix for now. Also added inputting for scaling.

References (optional):

Test Plan:

Test for memory leaks on deconstructor and bad imus or fully disconnected imus, and see if the switch works. Tests the scaling make sure that works, and the mapping does too. I made map private, but it might be better to make public. Not sure. Test the scaling, memory leeks, switching back and forth multiple times, and maybe even log the switching.

• test item

Download the template for this pull request:

Note

This is auto generated from Add Template to Pull Request

• via manual download: [email protected]+f97d33.zip
• via PROS Integrated Terminal:

curl -o [email protected]+f97d33.zip https://nightly.link/EZ-Robotics/EZ-Template/actions/artifacts/2405833253.zip;
pros c fetch [email protected]+f97d33.zip;
pros c apply [email protected]+f97d33;
rm [email protected]+f97d33.zip;

[ssejrog]

Ran some example autos with 1 IMU and code still works as expected

[ssejrog]

Unplugging port 20 while looking at the current pose on the blank page does successfully switch angle over to the second IMU in port 21. There is a slight lag where the IMU doesn't update anything for a second, then the orange "Inertial Sensor Disconnected" vexos warning comes up, and once that is dismissed angle updates as expected.

But after it switches, x and y are nan instead of something sane. I'm guessing this is something with the lag in switching between IMUs.

[ssejrog]

I tested unplugging port 20 and plugging it in very quickly, I did it within a timeframe that the orange warning screen did not appear. Once it was plugged in the IMU was drifting considerably. I went into the devices menu with this program still running and confirmed port 20 is drifting and port 21 is stable, but the code is using port 20 as the "good" IMU.

Disconnects aren't being found quick enough right now

[ssejrog]

I ran this code to figure out what can be done to detect disconnects.

pros::Imu testimu1(20);
pros::Imu testimu2(21);
std::vector<pros::Imu> testimu = {testimu1, testimu2};
/**
 * Runs the operator control code. This function will be started in its own task
 * with the default priority and stack size whenever the robot is enabled via
 * the Field Management System or the VEX Competition Switch in the operator
 * control mode.
 *
 * If no competition control is connected, this function will run immediately
 * following initialize().
 *
 * If the robot is disabled or communications is lost, the
 * operator control task will be stopped. Re-enabling the robot will restart the
 * task, not resume it from where it left off.
 */
void opcontrol() {
  // This is preference to what you like to drive on
  chassis.drive_brake_set(MOTOR_BRAKE_COAST);

  while (true) {
    // Gives you some extras to make EZ-Template ezier
    ez_template_extras();

    printf("Status: {");
    for (int i = 0; i < testimu.size(); i++) {
      if (i != 0) printf(" - ");
      printf("%i", testimu[i].get_status());
    }
    printf("}   ");

    printf("Value: {");
    for (int i = 0; i < testimu.size(); i++) {
      if (i != 0) printf(" - ");
      printf("%.2f", testimu[i].get_rotation());
    }
    printf("}\n");

    chassis.opcontrol_tank();  // Tank control
    // chassis.opcontrol_arcade_standard(ez::SPLIT);   // Standard split arcade
    // chassis.opcontrol_arcade_standard(ez::SINGLE);  // Standard single arcade
    // chassis.opcontrol_arcade_flipped(ez::SPLIT);    // Flipped split arcade
    // chassis.opcontrol_arcade_flipped(ez::SINGLE);   // Flipped single arcade

    // . . .
    // Put more user control code here!
    // . . .

    pros::delay(ez::util::DELAY_TIME);  // This is used for timer calculations!  Keep this ez::util::DELAY_TIME
  }
}

With this code I did a few tests.

This test has me unplugging the IMU in and never plugging it back in. You can see it disconnects where the value is 34.34 but it takes a significant amount of time before the status catches up to this. In both of these tests, the orange "Inertial Disconnected" screen appeared.
Test with unplugging port 20 - https://pastebin.com/dEXGj49R
Same test with port 21 as a sanity check - https://pastebin.com/g5qQs0FC

In this test, I unplugged the IMU and plugged it back in maybe half a second later. The length of time it was unplugged for was not long enough for the disconnected screen to appear. In both tests, the IMU comes back and gives data again but it drifts a considerable amount after reconnecting, and it sets its state to calibrate.
Test with port 20 being unplugged and replugged - https://pastebin.com/XiPU3tLp
Same test with port 21 as a sanity check - https://pastebin.com/w2kK2nN6

---

I think something in the data above should be used to determine if the IMU is disconnected or not.

With how IMUs can silently come back and pretend like nothing is wrong, I think every IMU needs to be checked every iteration, intsead of just the current IMU like it is now.