两个舵机同时执行？

rh902999

希望两个电机同时运行，

现在情况是电机1只运行一个角度，电机2正常运行，而且是轮流运行。


******************************************************************
代码区
#include <Arduino_FreeRTOS.h>
#include <MemoryFree.h>
#include <Wire.h>                     //添加头文件用于I2C通信
#include <Adafruit_PWMServoDriver.h>  //添加16路舵机控制板头文件
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();  //分配默认地址0x40
void TaskPrint1(void *param); //声明打印任务1
void TaskPrint2(void *param); //声明打印任务2
void TaskPrint3(void *param); //声明打印任务3
void TaskPrint4(void *param); //声明打印任务3
void setup() {
  Serial.begin(9600);
   Serial.println("16 channel Servo test!");
   pwm.begin();         // 开始调制PWM信号
   pwm.setPWMFreq(50);  // 设定PWM信号频率为50Hz
  
  while (!Serial);//等待串口连接后执行
  xTaskCreate(TaskPrint1, "Print1", 128, NULL, 1, NULL); //创建任务1
  xTaskCreate(TaskPrint2, "Print2", 128, NULL, 2, NULL); //创建任务2
  xTaskCreate(TaskPrint3, "Print3", 128, NULL, 3, NULL); //创建任务3
  xTaskCreate(TaskPrint4, "Print4", 128, NULL, 4, NULL); //创建任务4
  vTaskStartScheduler(); //启动任务调度
}

void loop() {
}
//------------------------------------
void setServoAngle(uint8_t n, double angle){    //该函数将高电平时间的函数简化为角度的函数
    uint16_t pulselen = angle*(512/225)+(512/5);
    pwm.setPWM(n,0, pulselen);
}

void Motor_a(int ax0)
{   
  
    delay(1000);
    for(double angle = 0; angle < 45; angle++) {
    delay(20);
    setServoAngle(ax0,angle);
   }
   delay(1000);
    for(double angle = 45; angle < 90; angle++) {
   delay(20);
    setServoAngle(ax0,angle);
   }
   delay(1000);
    for(double angle =90; angle < 135; angle++) {
   delay(20);
    setServoAngle(ax0,angle);
   }
   delay(1000);
    for(double angle = 135; angle < 185; angle++) {
   delay(20);
    setServoAngle(ax0,angle);
   }
   delay(1000);
    for(double angle = 185; angle >= -5; angle--) {  //电机角度误差补偿
   delay(20);                                        //电机速度
    setServoAngle(ax0,angle);
   }
   
  
}

void Motor_b(int ax0)
{   
  
    delay(1000);
    for(double angle = 0; angle <195; angle++) {
    delay(20);
    setServoAngle(ax0,angle);
   }
   delay(1000);
    for(double angle = 195; angle >=0; angle--) {  //电机角度误差补偿
   delay(20);                                        //电机速度
    setServoAngle(ax0,angle);
   }
   
  
}

void TaskPrint1(void *param)
{
  while (1)
  {
   Serial.println("TaskPrint1...");
    Motor_a(0);  //电机1
   vTaskDelay(1000 / portTICK_PERIOD_MS ); // 等待1秒
  }
}

void TaskPrint2(void *param)
{
  while (1)
  {
    Serial.println("TaskPrint2...");
    Motor_b(1);  //电机2
    vTaskDelay(1000 / portTICK_PERIOD_MS ); // 等待1秒
  }
}
void TaskPrint3(void *param)
{
  while (1)
  {
    Serial.println("TaskPrint3...");
    Serial.print("内存剩余(字节)=");
    Serial.println(freeMemory());
    vTaskDelay(10000 / portTICK_PERIOD_MS ); // 等待2秒
  }
}
void TaskPrint4(void *param)
{
  while (1)
  {
   Serial.println("TaskPrint4...");
   //Motor_a(0);  //电机1
   vTaskDelay(5000 / portTICK_PERIOD_MS ); // 等待2秒
  }
}