Proto Threading 2 functions

Question

I would like to proto thread two functions to run pseudo-simultaneously. One function takes the input of a force sensor and moves a servo accordingly, and the other takes the input of a gyroscope (MPU650) and moves a different sensor accordingly. Below is my code where I attempt to use timers to handle this.

#include <Wire.h>
#include <Servo.h> 
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Servo.h"

int FSR_Pin = A0; 
bool isClosed = false;
unsigned long lastMillis = 0;
unsigned long lastMillis2 = 0;

MPU6050 mpu;

int16_t ax, ay, az;
int16_t gx, gy, gz;


Servo handServo;  

Servo wristServo;

int pos = 0;    
int val;
int prevVal;

void setup(){
  Serial.begin(9600);
  handServo.attach(9);

  Wire.begin();
  Serial.begin(38400);

  Serial.println("Initialize MPU");
  mpu.initialize();
  Serial.println(mpu.testConnection() ? "Connected" : "Connection failed");
  wristServo.attach(8);
}


void handleFingers () {
  int FSRReading = analogRead(FSR_Pin); 
  Serial.println(FSRReading);               

if (FSRReading > 9) {
 if (pos < 90) {
     handServo.write(pos);
      pos ++;
 }
  isClosed = true;

} else if (isClosed == true && FSRReading < 10) {
  pos = 0;
  handServo.write(pos);
  isClosed = false;
}
}

void handleWrist() {
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    val = map(ay, -17000, 17000, 0, 179);
    if (val != prevVal)
    {
        wristServo.write(val);
        prevVal = val;
    }

}

void loop(){
  if (millis() - lastMillis > 1000) {
    handleFingers ();
  }
   lastMillis = millis();

 if (millis() - lastMillis2 > 1000) {
    handleWrist();
  }
   lastMillis2 = millis();

}

When I run the code above, the servos move a few degrees each and then do nothing at all. It appears that none of the servos are responsive to the sensor feedback. Something is wrong with my timing or my method of proto threading. I would really appreciate your help, thanks.

Answer 1

Jot provided the answer in a comment: you are updating lastMillis in the wrong place. The correct way is:

if (millis() - lastMillis >= 1000) {
    handleFingers();
    lastMillis = millis();
}

Optionally, you can use lastMillis += 1000 instead of lastMillis = millis() if you want to precisely control the average (rather than minimum) update period.

Just to elaborate a bit, the idiom I like for doing this kind of stuff is as follows:

const unsigned long FINGER_UPDATE_PERIOD = 1000;
const unsigned long WRIST_UPDATE_PERIOD  = 1000;

void loop() {
    unsigned long now = millis();

    // Handle the fingers.
    static unsigned long last_finger_update;
    if (now - last_finger_update >= FINGER_UPDATE_PERIOD) {
        last_finger_update = now;
        handleFingers();
    }

    // Handle the wrist.
    static unsigned long last_wrist_update;
    if (now - last_wrist_update >= WRIST_UPDATE_PERIOD) {
        last_wrist_update = now;
        handleWrist();
    }
}

Note that:

• The update periods are constants defined at the top of the program.
• millis() only needs to be called once per loop iteration: if the loop takes many milliseconds to run you would be doing something wrong anyway.
• last_finger_update and last_wrist_update stand for lastMillis and lastMillis2, which are very poor naming choices.
• These two variables are local to loop(), as they are not needed anywhere else.
• These variables are also declared static, which means they are implicitly initialized to 0 and they preserve their values from call to call.
• I update the timing variables before doing the actual work. This helps keeping all the timing code together, which makes the code clearer, especially if the actual work takes many lines of code (e.g. you do the work inline instead of having it in separate functions).

Some people prefer using libraries for this kind of proto-threading. If you use the ArduinoThread library, the same could be written as:

const unsigned long FINGER_UPDATE_PERIOD = 1000;
const unsigned long WRIST_UPDATE_PERIOD  = 1000;

Thread updateFingers(handleFingers, FINGER_UPDATE_PERIOD);
Thread updateWrist(handleWrist, WRIST_UPDATE_PERIOD);
StaticThreadController<2> scheduler(&updateFingers, &updateWrist);

void loop() {
    scheduler.run();
}