PID算法测试

PID算法测试

/*************************************************
              PID算法C语言测试

接线说明：
   PA6 PA7 PB0 PB1 电机端口
	 PC1~PC5 五路传感器端口
**************************************************/

#include <stdlib.h>
#include <stdio.h>
/*****************************************
              PID算法调速
******************************************/

//反馈速度
 int right_speed;
 int left_speed;

//参数设置
float      Kp = 10, Ki = 0.5, Kd = 0.1;                      //pid弯道参数  调试
float      error = 0, P = 0, I = 0, D = 0, PID_value = 0;  //pid直道参数
float      previous_error = 0;                             //先前误差值
int         sensor[5] = {0, 0, 0, 0, 0};                   	  //5个传感器数值的数组
int         initial_motor_speed = 50;                  //初始速度

/*************************************************
                   函数声明
*************************************************/
void  read_sensor_values(void);                     	 //读取初值
void  calc_pid(void);                               	 //计算pid
void  motor_control(void);
void  operation(void);                               //电机控制
/*********************************************************************************************************
* 名    称 ：main
* 功能描述 : 实现TIM3四个通道同时输出相同的信号
* 输入参数 : none
* 返回参数 : none.
*********************************************************************************************************/
void line(void)
{
     printf("*************************************");
}
int main(void)
{
    int i , j , count;
    printf("请输入测试次数：\n");
        scanf("%d",&count);

     for(i=0;i<count;i++)
	 {
	      line();
	      line();
          printf("\n\n(%d)请输入五路传感器数值\n",i+1);
          for(j=0;j<5;j++)
               scanf("%d",&sensor[j]);

          printf("\n输入成功！\n");
           read_sensor_values();         //读取数值 此处由输入代替
           calc_pid();                            //PID计算
           motor_control();                 //电机速度控制
            operation();                       //输出结果
            line();
	 }
}
/************************************************************
               第一部分 检测、反馈   获取误差值
*************************************************************/
void read_sensor_values()
{
       sensor[5]=1;
         if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1))
				 {
                      error = 0;//十字路口 1 1 1 1 1   直行
                 }
			else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0))
				{
					     error = 0;//十字路口 0 1 1 1 0   直行
				}
			else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1))
				{
					     error = 3;//90度路口 0 0 1 1 1    右转90度
				}
			else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0))
				{
				     	error = 2;//90度路口 0 0 1 1 0    右转90度
				}
				else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0))
				{
                     error = -3;//90度路口 1 1 1 0 0    左转90度
               }
 				else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0))
			  {
                      error = -2;//90度路口 0 1 1 0 0    左转90度
               }

               		else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0))
			  {
                      error = 2;//90度路口 0 0 1 1 0    右转
               }


         if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 1))
			{
				error = 3;      //          0 0 0 0 1
			}
		else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 0))
			{
                error = 2;			//          0 0 0 1 0
            }
		else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0))
		  {
              error = 0;//          0 0 1 0 0
          }
		else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0))
			{
                 error = -2;//         0 1 0 0 0
            }
	 else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0))
		 {
                 error = -3;//         1 0 0 0 0
         }
	 else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0))
		 {
				if (error == -2)
					{                                 //  0 0 0 0 0
					   error = -3;
				    }
				else
                  {
					   error = 3;
				  }
         }
}
/************************************************************
                 第二部分 计算PID（位置式PID）
*************************************************************/

void calc_pid()
{
      P = error;
      I = I + error;
      D = error - previous_error;
      PID_value = (Kp * P) + (Ki * I) + (Kd * D);
      previous_error = error;

      printf("\n\nerror（误差）=%f\npid_value（pid值）=%f\n\n",error,PID_value);
}

/************************************************************
                     第三部分 电机速度处理
*************************************************************/


void motor_control()
{
   left_speed = initial_motor_speed + PID_value;
   right_speed = initial_motor_speed - PID_value;

      if(left_speed <0)
         {
             left_speed = 0;
          }
              if(left_speed > 100)
                {
                     left_speed = 100;
                }

      if(right_speed < 0)
        {
             right_speed = 0;
        }
              if(right_speed > 100)
                {
                      right_speed = 100;
                }

}
/************************************************************
                         第四部分  输出速度
*************************************************************/

void operation(void)
{
	     if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1))
                {
                           printf("up\nright_speed=%d\nleft_speed=%d\n",right_speed,left_speed);
                }
			else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0))
				{
					      printf("up\nright_speed=%d\nleft_speed=%d\n",right_speed,left_speed);
				}
			else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1))
				{
					      printf("right\nright_speed=%d\nleft_speed=%d\n",right_speed,left_speed);
				}
			else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0))
				{
				         printf("right\nright_speed=%d\nleft_speed=%d\n",right_speed,left_speed);
				}
            else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0))
				{
                         printf("left\nright_speed=%d\nleft_speed=%d\n",right_speed,left_speed);
                }
           else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0))
			   {
                         printf("left\nright_speed=%d\nleft_speed=%d\n",right_speed,left_speed);
               }
             		else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0))
			  {
                      printf("right\nright_speed=%d\nleft_speed=%d\n",right_speed,left_speed);
              }

					if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 1))
						{
							   printf("right\nright_speed=%d\nleft_speed=%d\n",right_speed,left_speed);
						}
					else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 0))
						{
							 printf("right\nright_speed=%d\nleft_speed=%d\n",right_speed,left_speed);
						}
					else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0))
						{
							 printf("up\nright_speed=%d\nleft_speed=%d\n",50,50);
						}
					else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0))
						{
							 printf("left\nright_speed=%d\nleft_speed=%d\n",right_speed,left_speed);
						}
				 else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0))
					 {
						     printf("left\nright_speed=%d\nleft_speed=%d\n",right_speed,left_speed);
					 }
				 else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0))
					 {
							if (error < 0)
								{                                 //  0 0 0 0 0
									printf("left\nright_speed=%d\nleft_speed=%d\n",right_speed,left_speed);
								}
							else if(error > 0)
								{
									printf("right\nright_speed=%d\nleft_speed=%d\n",right_speed,left_speed);
								}
								else
								{
								   printf("stop\nright_speed=%d\nleft_speed=%d\n",right_speed,left_speed);
								}
						}
}