用c语言写一段控制舵机方向的pid程序

由于不知道你使用的是哪种型号的舵机和控制器，下面是一个通用的舵机PID控制器的例子，你可以根据实际需要进行修改。

#include <stdio.h>
#include <stdlib.h>
#include <wiringPi.h>   // 树莓派GPIO库头文件

// 宏定义，用于设置舵机角度范围和初始位置
#define MIN_ANGLE   10
#define MAX_ANGLE   170
#define INIT_ANGLE  90

// PID控制参数
double Kp = 1.0;    // 比例系数
double Ki = 0.0;    // 积分系数
double Kd = 0.0;    // 微分系数

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

// PID控制器中的变量
double last_error = 0.0;
double integral = 0.0;

// 初始化舵机和PID控制器
void init()
{
    wiringPiSetup();    // 初始化wiringPi库
    pinMode(1, PWM_OUTPUT);    // 将GPIO1设置为PWM输出
    pwmSetClock(375);   // 设置PWM时钟频率为60KHz
    pwmSetRange(1024);  // 设置PWM范围为0~1024
    pwmWrite(1, INIT_ANGLE);    // 将舵机转至INIT_ANGLE角度
}

// 获取当前角度
double getAngle()
{
    // 根据实际舵机型号进行读取角度的操作
    // 这里假设已经获取到了当前角度值并保存在变量currentAngle中
    return currentAngle;
}

// 设置舵机角度
void setAngle(double angle)
{
    // 根据实际舵机型号进行舵机控制的操作
    // 这里假设已经将舵机转动到了指定角度
    pwmWrite(1, (int)angle);
}

// PID控制器
void PID_control()
{
    double error = targetAngle - currentAngle;
    double derivative = error - last_error;

    integral += error;

    double output = Kp*error + Ki*integral + Kd*derivative;

    // 限制输出范围为舵机角度范围
    if (output < MIN_ANGLE)
    {
        output = MIN_ANGLE;
        integral -= error;
    }
    else if (output > MAX_ANGLE)
    {
        output = MAX_ANGLE;
        integral -= error;
    }

    setAngle(output);

    last_error = error;
}

int main()
{
    init();     // 初始化舵机和PID控制器

    // 循环运行PID控制器
    while (1)
    {
        currentAngle = getAngle();  // 获取当前角度
        PID_control();              // 进行PID控制
        delay(20);                  // 延时20ms，等待舵机稳定
    }

    return 0;
}