2

I'm trying to use protothreading to control two (and eventually four) servo motors over I2C using a PCA9685 PWM board.

To start, I am simply trying to sweep the two motors back and fourth at the same time.

Here is the code I am using:

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#include <protothreads.h>

#define SERVOMIN  150 
#define SERVOMAX  600 
#define USMIN  600 
#define USMAX  2400 
#define SERVO_FREQ 50 

int servo1 = 0;
int servo2 = 4;

////////////////////////     T H R E A D    1     /////////////////////////

pt ptServo1;
int Servo1Thread(struct pt* pt){
  PT_BEGIN(pt);
  for(;;){
    for (uint16_t microsec = USMIN; microsec < USMAX; microsec++) {
      pwm.writeMicroseconds(servo1, microsec);
    }
    PT_SLEEP(pt, 500);
    for (uint16_t microsec = USMAX; microsec > USMIN; microsec--) {
      pwm.writeMicroseconds(servo1, microsec);
    }
    PT_SLEEP(pt, 500);
  }
  PT_END(pt);
}

////////////////////////     T H R E A D    2     /////////////////////////

pt ptServo2;
int Servo2Thread(struct pt* pt){
  PT_BEGIN(pt);
  for(;;){
    for (uint16_t microsec = USMIN; microsec < USMAX; microsec++) {
      pwm.writeMicroseconds(servo2, microsec);
    }
    PT_SLEEP(pt, 500);
    for (uint16_t microsec = USMAX; microsec > USMIN; microsec--) {
      pwm.writeMicroseconds(servo2, microsec);
    }
    PT_SLEEP(pt, 500);
  }
  PT_END(pt);
}


////////////////////////////////////////////////////////////////////////////

void setup() {
  Serial.begin(9600);
  PT_INIT(&ptServo1);
  PT_INIT(&ptServo2);
  pwm.begin();
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates
}


void loop() {
  PT_SCHEDULE(Servo1Thread(&ptServo1));
  PT_SCHEDULE(Servo2Thread(&ptServo2));
}

I am noticing that Servo 1 turns to -90 and stops. While it has stopped, Servo 2 turns to -90 and then stops....then Servo 1 turns to +90 and stops and so on. They are not doing the sweeping at the same time.

Any help would be greatly appreciated!

Thank you,

H

1 Answer 1

1

Without knowing Protothread in detail: It is most likely handling the threadchange during PT_SLEEP(), which you are only executing after one "sweep" (one of the inner for loops), not giving any opportunity to change to the other thread.

Additionally this way you don't have any delay in the inner for loops, thus they get executed very fast. Faster than the motor really can follow. This means, that the servo just moves to the end position as fast as it can and the for loop is essentially useless. You need a delay inside the inner for loops for slowing the signal down.

Please try something like this:

pt ptServo1;
int Servo1Thread(struct pt* pt){
  PT_BEGIN(pt);
  for(;;){
    for (uint16_t microsec = USMIN; microsec < USMAX; microsec++) {
      pwm.writeMicroseconds(servo1, microsec);
      PT_SLEEP(pt, 50);
    }
    PT_SLEEP(pt, 500);
    for (uint16_t microsec = USMAX; microsec > USMIN; microsec--) {
      pwm.writeMicroseconds(servo1, microsec);
      PT_SLEEP(pt, 50);
    }
    PT_SLEEP(pt, 500);
  }
  PT_END(pt);
}

Change the sleep time in the inner loop according to your wanted sweep speed.

2
  • Thank you for your answer! Curiously, once I add PT_SLEEP(pt,50) inside the for loop as you suggested, nothing happens. The motors do not move at all...is it possible that protothreading does not work with I2c perhaps?
    – H. M
    Sep 2, 2021 at 15:19
  • You might have to make the sleep time significantly lower (havent registered before that you write the microsecond range from 600 to 2400 with step size of 1) or increase the step size from 1 to a higher number. With the current setting it might just take too long to notice the movement
    – chrisl
    Sep 5, 2021 at 7:50

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.