Improve gyro delay and implement ultrasonic sensor with Protothreading

Hi there,

Since Nybble stutters a lot when the gyro is activated, I implemented the protothreading.h library.

Instead of the delay() action, PT_SLEEP() can be used, without stopping the microcontroller. This makes the movement more fluent and speeds up Nybble in gyro mode.

I also implemented the ultrasonic sensor as a protothread, so the distance can be calculated without delay. Please review the code (certainly not perfect yet) and share your experience.

To use protothreads, you have to install the protothread library in the Arduino-IDE (Tools --> Manage Libraries).

Ultrasonic Sensor:

#include "protothreads.h"

/* Protothreads:    Thanks to Scott Fitzgerald, Arturo Guadalupi, 
                    Colby Newman and Ben Artin */


pt ptUltra;
int ultraThread(struct pt* pt) {
    static int duration = 0;
    static float distance = 0.0;
    static float farTime =  LONGEST_DISTANCE*2/0.034;

        
    PT_BEGIN(pt);
    
    // Loop forever
    for(;;) {
        digitalWrite(TRIGGER, LOW);   // turn the LED on (HIGH is the voltage level)
        PT_SLEEP(pt, 2e-3);
        digitalWrite(TRIGGER, HIGH);    // turn the LED off by making the voltage LOW
        PT_SLEEP(pt, 10e-3);
        digitalWrite(TRIGGER, LOW);

        // Reads the echoPin, returns the sound wave travel time in microseconds
        duration = pulseIn(ECHO, HIGH, farTime);  

        // Calculating the distance
        distance = duration * 0.034 / 2; // 10^-6 * 34000 cm/s
        PTL(distance);
        PT_SLEEP(pt, 500);
    }

    PT_END(pt);
}

BodyMotion Thread

pt ptBodyMotion;
int checkBodyMotionThread(struct pt* pt)  {
  static byte hold = 0;
  PT_BEGIN(pt);

  // Loop forever
  for(;;) {
      //if (!dmpReady) return;
      getYPR();
      // --
      //deal with accidents
      if (fabs(ypr[1]) > LARGE_PITCH || fabs(ypr[2]) > LARGE_ROLL) {//wait until stable
        if (!hold) {
            getYPR();
            PT_SLEEP(pt, 10);
        }
        if (fabs(ypr[1]) > LARGE_PITCH || fabs(ypr[2]) > LARGE_ROLL) {//check again
          if (!hold) {
            token = 'k';
            if (fabs(ypr[2]) > LARGE_ROLL) {
              strcpy(newCmd, "rc");
              newCmdIdx = 4;
            }
            else {
              strcpy(newCmd, ypr[1] < LARGE_PITCH ? "lifted" : "dropped");
              newCmdIdx = 1;
            }
          }
          hold = 10;
        }
      }

      // recover
      else if (hold) {
        if (hold == 1) {
          token = 'k';
          strcpy(newCmd, "balance");
          newCmdIdx = 1;
        }
        hold --;
        if (!hold) {
          char temp[CMD_LEN];
          strcpy(temp, newCmd);
          strcpy(newCmd, lastCmd);
          strcpy(lastCmd, temp);
          newCmdIdx = 1;
          meow();
        }
      }
      //calculate deviation
      for (byte i = 0; i < 2; i++) {
        RollPitchDeviation[i] = ypr[2 - i] - motion.expectedRollPitch[i]; //all in degrees
        //PTL(RollPitchDeviation[i]);
        RollPitchDeviation[i] = sign(ypr[2 - i]) * max(fabs(RollPitchDeviation[i]) - levelTolerance[i], 0);//filter out small angles
      }
  PT_SLEEP(pt, 5);
  }
  //PTL(jointIdx);
  PT_END(pt);
}

Don't forget to add those lines in setup()

  PT_INIT(&ptUltra);
  PT_INIT(&ptBodyMotion);

And to call the threads regularly in the main loop()

    PT_SCHEDULE(ultraThread(&ptUltra));
    PT_SCHEDULE(checkBodyMotionThread(&ptBodyMotion));