舵机pid控制代码 C语言在线程里

以下是一个在C语言多线程中运行的舵机PID控制代码示例：

#include <stdio.h>
#include <stdlib.h>
#include <pthread.h>
#include <wiringPi.h>
#include <softPwm.h>

// 舵机参数
#define SERVO 1
#define PWM_RANGE 2000
#define PWM_CLOCK 384

// PID参数
double Kp = 1.0;
double Ki = 0.0;
double Kd = 0.0;

// 目标角度和当前角度
double targetAngle = 90.0;
double currentAngle = 0.0;

// PID变量
double error = 0.0;
double lastError = 0.0;
double integral = 0.0;
double derivative = 0.0;

// PID计算结果
double output = 0.0;

// 时间间隔和调节周期
unsigned long lastTime = 0;
unsigned long currentTime = 0;
double timeInterval = 0.0;
double sampleTime = 10.0; // 调节周期为10毫秒

// 线程锁和条件变量
pthread_mutex_t mutex;
pthread_cond_t cond;

void* servoControl(void* arg) {
  // 初始化wiringPi库和舵机
  wiringPiSetup();
  softPwmCreate(SERVO, 0, PWM_RANGE, PWM_CLOCK);

  while (1) {
    // 等待条件变量触发
    pthread_mutex_lock(&mutex);
    pthread_cond_wait(&cond, &mutex);
    pthread_mutex_unlock(&mutex);

    // 计算时间间隔
    currentTime = millis();
    timeInterval = (double)(currentTime - lastTime) / 1000.0;

    // 如果时间间隔大于等于调节周期，则进行PID计算
    if (timeInterval >= sampleTime) {
      // 计算误差
      error = targetAngle - currentAngle;

      // 计算积分项
      integral += error * timeInterval;

      // 计算微分项
      derivative = (error - lastError) / timeInterval;

      // 计算PID输出
      output = Kp * error + Ki * integral + Kd * derivative;

      // 保存误差值
      lastError = error;

      // 保存当前时间
      lastTime = currentTime;

      // 限制输出范围
      if (output > 180.0) {
        output = 180.0;
      }
      else if (output < 0.0) {
        output = 0.0;
      }

      // 更新舵机角度
      softPwmWrite(SERVO, (int)(100.0 * output / 180.0) + 5);

      // 更新当前角度
      currentAngle = output;
    }
  }
}

int main() {
  // 初始化线程锁和条件变量
  pthread_mutex_init(&mutex, NULL);
  pthread_cond_init(&cond, NULL);

  // 创建线程
  pthread_t thread;
  pthread_create(&thread, NULL, servoControl, NULL);

  while (1) {
    // 读取用户输入的目标角度
    double angle;
    printf("请输入目标角度（0-180）：");
    scanf("%lf", &angle);

    // 设置目标角度
    targetAngle = angle;

    // 触发条件变量，开始PID计算
    pthread_mutex_lock(&mutex);
    pthread_cond_signal(&cond);
    pthread_mutex_unlock(&mutex);
  }

  return 0;
}

在这个代码中，我们使用了一个多线程的方式来控制舵机的角度。我们创建了一个线程，用于运行PID控制器，同时在主线程中等待用户输入目标角度，并通过条件变量触发PID计算。在PID计算过程中，我们使用了wiringPi库和软件PWM来控制舵机的转动。