Как управлять несколькими серводвигателями одновременно с помощью протопотока с помощью платы ШИМ I2C PCA9685?

Question

Как управлять несколькими серводвигателями одновременно с помощью протопотока с помощью платы ШИМ I2C PCA9685?

Я пытаюсь использовать протопоточность для управления двумя (и, в конечном счете, четырьмя) серводвигателями по I2C с помощью платы PCA9685 PWM.

Для начала я просто пытаюсь запустить два двигателя назад и четвертый одновременно.

Вот код, который я использую:

#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));
}

Я замечаю, что сервопривод 1 поворачивается на -90 и останавливается. Когда он остановился, сервопривод 2 поворачивается до -90, а затем останавливается....затем сервопривод 1 поворачивается на +90 и останавливается, и так далее. Они не делают уборку одновременно.

Любая помощь будет очень признательна!

Спасибо тебе,

H

@H. M, 👍2

1 ответ

@*chrisl* · Answer 1 · 2021-09-02 07:03-00:00

Не зная подробно Protothread: скорее всего, он обрабатывает обмен потоками во время PT_SLEEP (), который вы выполняете только после одной "развертки" (одного из внутренних циклов for), не давая никакой возможности переключиться на другой поток.

Кроме того, таким образом, у вас нет никаких задержек во внутренних циклах for, поэтому они выполняются очень быстро. Быстрее, чем двигатель действительно может следовать. Это означает, что сервопривод просто перемещается в конечное положение так быстро, как только может, а цикл for практически бесполезен. Вам нужна задержка внутри внутренних петель for для замедления сигнала.

Пожалуйста, попробуйте что-нибудь вроде этого:

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);
}

Измените время сна во внутреннем цикле в соответствии с требуемой скоростью развертки.