Библиотека Protothread останавливается на одном потоке

Я пытаюсь использовать библиотеку protothreading, найденную здесь

http://harteware.blogspot.com/2010/11/protothread-powerfull-library.html

для выполнения protothreading с моим Arduino, чтобы я мог «одновременно» считывать данные со входа чипа Bluetooth HC-05 для получения информации с главного компьютера, к которому я подключен по Bluetooth, а также обрабатывать и передавать данные с датчиков по Bluetooth.

Мой код выглядит так

/*
Этот модуль использует чип L293D для модуляции и питания
два двигателя постоянного тока, управляющие колесами FryeBot
*/


#include <Time-1.5.0\Time.h>
#include <WiShield\pt.h>

/*L293D INPUT 7*/
const int YELLOW_GEAR_MOTOR_BACK = 8;

/*L293D INPUT 2*/
const int YELLOW_GEAR_MOTOR_FORWARD = 9;

/*L293D INPUT 10*/
const int RED_GEAR_MOTOR_BACK = 10;

/*L293D INPUT 15*/
const int RED_GEAR_MOTOR_FORWARD = 11;

/*
    Establish ratio os the rate of the faster slower gear
    to the faster gear to scale the duty cycle of the
    faster gear
*/
const float RED_GEAR_SPEED_TO_YELLOW = 0.97;

/*Constant for full duty cycle for analogWrite*/
const int FULL_ANALOG_MOTOR_SPEED = 255;

/* HC05 Setup Data */
const int pingPin = 7;
const int HC05PowerPin = 2;

/* Distance data */
long duration, cm;

/* Values for Bluetooth RX/TX */
char buf[25];

char current;
String messageHolder;
String message;

/* Protothreads */
static struct pt pt1, pt2;

void setup() {
    Serial.begin(9600);
    Serial1.begin(9600);

    /* add setup code here */
    pinMode(YELLOW_GEAR_MOTOR_FORWARD, OUTPUT);
    pinMode(YELLOW_GEAR_MOTOR_BACK, OUTPUT);
    pinMode(RED_GEAR_MOTOR_BACK, OUTPUT);
    pinMode(RED_GEAR_MOTOR_FORWARD, OUTPUT);
    pinMode(HC05PowerPin, OUTPUT);

    PT_INIT(&pt1);  
    PT_INIT(&pt2);

}

static int protothreadCollectAndTransmitData(struct pt* pt, int interval) {
    static unsigned long timestamp = 0;

    PT_BEGIN(pt);

    while (1) {
        collectAndTransmitData();
        PT_WAIT_UNTIL(pt, millis() - timestamp > interval);
    }

    PT_END(pt);
}

void collectAndTransmitData() {
    digitalWrite(HC05PowerPin, HIGH);

    pinMode(pingPin, OUTPUT);
    digitalWrite(pingPin, LOW);
    delayMicroseconds(2);
    digitalWrite(pingPin, HIGH);
    delayMicroseconds(5);
    digitalWrite(pingPin, LOW);

    pinMode(pingPin, INPUT);
    duration = pulseIn(pingPin, HIGH);

    cm = microsecondsToCentimeters(duration);

    Serial.print("TX: ");
    Serial.print(cm);
    Serial.println();

    if (message.indexOf("G") > 0) {

        analogWrite(YELLOW_GEAR_MOTOR_FORWARD, (int)(RED_GEAR_SPEED_TO_YELLOW * (float)FULL_ANALOG_MOTOR_SPEED));
        digitalWrite(YELLOW_GEAR_MOTOR_BACK, LOW);

        digitalWrite(RED_GEAR_MOTOR_BACK, LOW);
        digitalWrite(RED_GEAR_MOTOR_FORWARD, HIGH);
    }
    else if (message.indexOf("S") > 0) {
        digitalWrite(YELLOW_GEAR_MOTOR_FORWARD, 0);
        digitalWrite(YELLOW_GEAR_MOTOR_BACK, LOW);

        digitalWrite(RED_GEAR_MOTOR_BACK, LOW);
        digitalWrite(RED_GEAR_MOTOR_FORWARD, LOW);
    }

    /* Load the distance value into a buffer */
    String sendValue = String(cm);
    sendValue.toCharArray(buf, sizeof(buf));

    /* Write the buffer with a space after for parsing*/
    Serial1.write(buf);
    Serial1.write(' ');
}

long microsecondsToCentimeters(long microseconds) {
    // The speed of sound is 340 m/s or 29 microseconds per centimeter.
    // The ping travels out and back, so to find the distance of the
    // object we take half of the distance travelled.
    return microseconds / 29 / 2;
}

static int protothreadReadAndParseData(struct pt* pt, int interval) {
    static unsigned long timestamp = 0;

    PT_BEGIN(pt);

    while (1) {
        readAndParseData();
        PT_WAIT_UNTIL(pt, millis() - timestamp > interval);
    }

    PT_END(pt);
}

void readAndParseData() {
    Serial.println("HERE");
    current = Serial1.read();
    Serial.print("RX: ");
    Serial.print(current);
    Serial.println();

    if (current == ' ') {
        message = messageHolder;
        messageHolder = "";
        Serial.print("message: ");
        Serial.print(message);
        Serial.println();
    }
    else {
        messageHolder += current;
    }
}


void loop() {
    Serial.println("A");
    protothreadReadAndParseData(&pt2, 20); // by calling them infinitely
    Serial.println("B");
    protothreadCollectAndTransmitData(&pt1, 20); // schedule the two protothreads
}

При выполнении код выводит на консоль следующее.

A
HERE
RX: �
B
TX: 522
TX: 524
TX: 524
TX: 524
TX: 524
TX: 524

Затем продолжается TX: ### навсегда.

Я не уверен, почему код не возвращается к первой функции или хотя бы не запускает ее во второй раз. Я не совсем понимаю, как именно использовать эту библиотеку или кто ее поддерживает. Документация на их странице Github для библиотеки WiShield, частью которой является этот pt.h, не слишком полезна. Если у вас есть опыт работы с другими библиотеками Arduino protothreading, дайте мне знать, чтобы я мог попробовать их. Я не уверен, где я ошибся с этой библиотекой в своей реализации.

Вот репозиторий, содержащий пакет, который я использую

https://github.com/nicolaskruchten/arduino/tree/master/libraries/WiShield