转一个PID控制电机的小程序， 被PID困扰好多天了， 知道它的原理但是一直不明白如何将它运用到电机调速中间去， 看了这个程序之后感觉茅塞顿开。原来也并不难^-^
转载地址：呃，刚刚不小心把网页关掉了(大写的尴尬)。。。。



#include<reg52.h>
#include<stdio.h>
#define uchar unsigned char 
#define uint unsigned int
#define THC0 0xf8
#define TLC0 0xcc   //2ms
unsigned char code Duan[]={0x3F,0x06,0x5B,0x4F,0x66,0x6D,0x7D,0x07,0x7F,0x6F};//共阴极数码管，0-9段码表
unsigned char Data_Buffer[8]={0,0,0,0,0,0,0,0};
unsigned char Data[4]={0,0,0,0};
unsigned char Arry[4]={0,0,0,0};
bit flag1=0;
bit flag0=0;
uchar i=0;
sbit AddSpeed=P1^1;
sbit SubSpeed=P1^2;
sbit PWM_FC=P1^0;
int e=0,e1=0,e2=0;			//pid 偏差
float uk=0,uk1=0.0,duk=0.0;	//pid输出值
float Kp=5,Ki=1.5,Kd=0.9;	//pid控制系数	
//float Kp=10;
int out=0;
uint SpeedSet=1000;
uint cnt=0;
uint Inpluse=0,num=0;		//脉冲计数
uint PWMTime=0;				//脉冲宽度
unsigned char arry[];
void SendString(uint ch);
void PIDControl();
void SystemInit();
void delay(uchar x);
void PWMOUT();
void SetSpeed();
void SegRefre();
/**************主函数************/
void main()
{
	SystemInit();//系统初始化
	while(1)
	{
		SetSpeed();//设置速度
		SegRefre();//更新数码管显示
		PWMOUT();  //PWM输出
		if(flag0==1)
		{
			flag0=0;
			ES=0;//关串口中断，避免TI引起串口中断
			TI=1;//printf前TI置1
			printf("%f",(float)(num>>8));//脉冲计数
			printf("%f",(float)num);	 //脉冲计数
			while(!TI);
			TI=0;	   //TI软件清除
			ES=1;	   //允许串口中断
		}
	}
}


//PID偏差控制计算
void PIDControl()        //pid偏差计算
{
	e=SpeedSet-num;//设置速度-实际速度，两者的差值 
	//对应于增量式PID的公式Δuk=uk-u（k-1）
//	duk=(Kp*(e-e1))/100;//只调节P
	duk=(Kp*(e-e1)+Ki*e)/100;//只调节PI
//	duk=(Kp*(e-e1)+Ki*e+Kd*(e-2*e1+e2))/100;//调节PID
	uk=uk1+duk;//uk=u(k-1)+Δuk
	out=(int)uk;//取整后输出
	if(out>250)	//设置最大限制
		out=250;
	else if(out<0)//设置最小限制
		out=0;
	uk1=uk;		  //为下一次增量做准备
	e2=e1;
	e1=e;
	PWMTime=out;  //out对应于PWM高电平的时间
}


//较短的延时。注意delay的值不要超过255
void delay(uchar x)
{
	uchar i,j;
	for(i=x;i>0;i--)
		for(j=50;j>0;j--);//接近500us
}


//PWM输出
void PWMOUT()
{
	if(cnt<PWMTime)//若小于PWM的设定时间，则输出高电平
		PWM_FC=1;
	else		   //否则输出低电平
		PWM_FC=0;
	if(cnt>250)    //超过限制清零
		cnt=0;
}


//系统初始化
void SystemInit()
{
	TMOD=0X21;    //T1用于串口的波特率发生器 T0用于定时
	TH0=THC0;	  //2ms定时，晶振频率是11.0592MHz，事先算好装入THC0中
	TL0=TLC0;	  //2ms定时，晶振频率是11.0592MHz，实现算好装入TLCO中
	ET0=1;		  //允许定时器0中断
	TR0=1;		  //启动定时器0
	EX0=1;		  //允许外部中断
	IT0=1;		  //中断方式设置为下降沿
	//用于串口调试时用
	PCON=0x00;	 //SMOD不加倍
	SCON=0x50;	 //串口工作方式1，允许接收
	TH1=0xfd;	 //波特率9600
	TL1=0xfd;	 //波特率9600
	TR1=1;		 //启动定时器1
	ES=1;		 //开串口中断
	EA=1;		 //开总中断
	e =0;		 //PID的差值初值均为0
	e1=0;		 
	e2=0;
}


//设置转速
void SetSpeed()
{
	if(AddSpeed==0)//按键
	{
		delay(200);//消抖
		if(AddSpeed==0)//再次判断按键
		{
			SpeedSet+=100;//速度增加
			if(SpeedSet>9999)//超限
				SpeedSet=9999;//设置为最大9999
		}
	}
	if(SubSpeed==0)//按键
	{
		delay(200);//消抖
		if(SubSpeed==0)//按键
		{
			SpeedSet-=100;//速度减
			if(SpeedSet<0)//低于速度的最小值 
				SpeedSet=0;//速度置零
		}
	}
}


//数码管显示更新
void SegRefre()		  //显示刷新
{
	Data_Buffer[0]=SpeedSet/1000;
	Data_Buffer[1]=SpeedSet%1000/100;
	Data_Buffer[2]=SpeedSet%100/10;
	Data_Buffer[3]=SpeedSet%10;
	Data_Buffer[4]=num/1000;
	Data_Buffer[5]=num%1000/100;
	Data_Buffer[6]=num%100/10;
	Data_Buffer[7]=num%10;
}


//外部中断，统计脉冲数目，实际上是统计电机的转速
//在实际中，电机没有一根线引出来可以表示他的转速，只能通过传感器如霍尔传感器的方式测量转速
//脉冲一次下降沿对应于一次自增
void int0() interrupt 0
{
	Inpluse++;
}


//定时器T0操作
void t0() interrupt 1
{
	static unsigned char Bit=0;//静态变量，退出程序值保留
	static unsigned int time=0;
	static unsigned int aa=0;
	TH0=THC0;//重新赋初值
	TL0=TLC0;
	aa++;
	if(aa==50)//每100ms
	{
		aa=0;
		flag0=1;
	}
	cnt++;	//pid 周期
	Bit++;
	time++;  //转速测量周期
	if(Bit>8) Bit=0;//数码管总共只有8位，超过8位则在程序上进行清零
	//数码管显示部分
	P0=0xff;
	P2=Duan[Data_Buffer[Bit]];
	switch(Bit)
	{
		case 0:P0=0X7F;break;
		case 1:P0=0XBF;break;
		case 2:P0=0XDF;break;
		case 3:P0=0XEF;break;
		case 4:P0=0XF7;break;
		case 5:P0=0XFB;break;
		case 6:P0=0XFD;break;
		case 7:P0=0XFE;break;
	}
	//数码管显示部分
	if(time>500)//每1s处理一次脉冲
	{
		time=0;
		num=Inpluse*15;//实际转速，*15是由电机决定的，电机的一个脉冲对应着电机转过了15转
		Inpluse=0;	   //清零，为下一次计数做准备
		PIDControl();  //调用PID控制程序
	}
}


//串口中断，用于对串口的数据进行处理
void u_int(void) interrupt 4
{
	ES=0;//关串口中断，避免在数据处理的过程中造成影响
	if(RI)//若有RI==1，由RI产生中断
	{
		RI=0;//RI标志位必须通过软件进行清零
		arry[i]=SBUF;//将字符赋到arry中
		i++;		 //下一个数
		if(i>3)		 //接收完了指令，进行数据处理，一共有arry[0],arry[1],arry[2],arry[3]四个数据
		{
			flag1=1;//将标志置1
			i=0;	//i清零，重新计数
		}
		if(flag1)	//若flag1==1
		{
			flag1=0;//flag1清零
			SpeedSet=(arry[0]-'0')+(arry[1]-'0')*10+(arry[2]-'0')*100+(arry[3]-'0')*1000;//获得的速度值
			//由于串口发送的是字符，所以要减去'0'
		}
	}
	else//对应的由TI产生中断
		TI=0;//TI由软件进行清零	
	ES=1;//处理完后再开中断
}





#include<reg52.h>

#include<stdio.h>

#define uchar unsigned char 

#define uint unsigned int

#define THC0 0xf8

#define TLC0 0xcc   //2ms

unsigned char code Duan[]={0x3F,0x06,0x5B,0x4F,0x66,0x6D,0x7D,0x07,0x7F,0x6F};//共阴极数码管，0-9段码表

unsigned char Data_Buffer[8]={0,0,0,0,0,0,0,0};

unsigned char Data[4]={0,0,0,0};

unsigned char Arry[4]={0,0,0,0};

bit flag1=0;

bit flag0=0;

uchar i=0;

sbit AddSpeed=P1^1;

sbit SubSpeed=P1^2;

sbit PWM_FC=P1^0;

int e=0,e1=0,e2=0; //pid 偏差

float uk=0,uk1=0.0,duk=0.0; //pid输出值

float Kp=5,Ki=1.5,Kd=0.9; //pid控制系数


//float Kp=10;

int out=0;

uint SpeedSet=1000;

uint cnt=0;

uint Inpluse=0,num=0; //脉冲计数

uint PWMTime=0; //脉冲宽度

unsigned char arry[];

void SendString(uint ch);

void PIDControl();

void SystemInit();

void delay(uchar x);

void PWMOUT();

void SetSpeed();

void SegRefre();

/**************主函数************/

void main()

{
SystemInit();//系统初始化
while(1)
{
SetSpeed();//设置速度
SegRefre();//更新数码管显示
PWMOUT();  //PWM输出
if(flag0==1)
{
flag0=0;
ES=0;//关串口中断，避免TI引起串口中断
TI=1;//printf前TI置1
printf("%f",(float)(num>>8));//脉冲计数
printf("%f",(float)num);
//脉冲计数
while(!TI);
TI=0;    //TI软件清除
ES=1;    //允许串口中断
}
}

}



//PID偏差控制计算

void PIDControl()        //pid偏差计算

{
e=SpeedSet-num;//设置速度-实际速度，两者的差值 
//对应于增量式PID的公式Δuk=uk-u（k-1）

// duk=(Kp*(e-e1))/100;//只调节P
duk=(Kp*(e-e1)+Ki*e)/100;//只调节PI

// duk=(Kp*(e-e1)+Ki*e+Kd*(e-2*e1+e2))/100;//调节PID
uk=uk1+duk;//uk=u(k-1)+Δuk
out=(int)uk;//取整后输出
if(out>250) 
//设置最大限制
out=250;
else if(out<0)//设置最小限制
out=0;
uk1=uk; 
  //为下一次增量做准备
e2=e1;
e1=e;
PWMTime=out;  //out对应于PWM高电平的时间

}



//较短的延时。注意delay的值不要超过255

void delay(uchar x)

{
uchar i,j;
for(i=x;i>0;i--)
for(j=50;j>0;j--);//接近500us

}



//PWM输出

void PWMOUT()

{
if(cnt<PWMTime)//若小于PWM的设定时间，则输出高电平
PWM_FC=1;
else    //否则输出低电平
PWM_FC=0;
if(cnt>250)    //超过限制清零
cnt=0;

}



//系统初始化

void SystemInit()

{
TMOD=0X21;    //T1用于串口的波特率发生器 T0用于定时
TH0=THC0; 
  //2ms定时，晶振频率是11.0592MHz，事先算好装入THC0中
TL0=TLC0; 
  //2ms定时，晶振频率是11.0592MHz，实现算好装入TLCO中
ET0=1; 
  //允许定时器0中断
TR0=1; 
  //启动定时器0
EX0=1; 
  //允许外部中断
IT0=1; 
  //中断方式设置为下降沿
//用于串口调试时用
PCON=0x00; 
//SMOD不加倍
SCON=0x50; 
//串口工作方式1，允许接收
TH1=0xfd; 
//波特率9600
TL1=0xfd; 
//波特率9600
TR1=1; 
//启动定时器1
ES=1; //开串口中断
EA=1; //开总中断
e =0; //PID的差值初值均为0
e1=0;  
e2=0;

}



//设置转速

void SetSpeed()

{
if(AddSpeed==0)//按键
{
delay(200);//消抖
if(AddSpeed==0)//再次判断按键
{
SpeedSet+=100;//速度增加
if(SpeedSet>9999)//超限
SpeedSet=9999;//设置为最大9999
}
}
if(SubSpeed==0)//按键
{
delay(200);//消抖
if(SubSpeed==0)//按键
{
SpeedSet-=100;//速度减
if(SpeedSet<0)//低于速度的最小值 
SpeedSet=0;//速度置零
}
}

}



//数码管显示更新

void SegRefre()   //显示刷新

{
Data_Buffer[0]=SpeedSet/1000;
Data_Buffer[1]=SpeedSet%1000/100;
Data_Buffer[2]=SpeedSet%100/10;
Data_Buffer[3]=SpeedSet%10;
Data_Buffer[4]=num/1000;
Data_Buffer[5]=num%1000/100;
Data_Buffer[6]=num%100/10;
Data_Buffer[7]=num%10;

}



//外部中断，统计脉冲数目，实际上是统计电机的转速

//在实际中，电机没有一根线引出来可以表示他的转速，只能通过传感器如霍尔传感器的方式测量转速

//脉冲一次下降沿对应于一次自增

void int0() interrupt 0

{
Inpluse++;

}



//定时器T0操作

void t0() interrupt 1

{
static unsigned char Bit=0;//静态变量，退出程序值保留
static unsigned int time=0;
static unsigned int aa=0;
TH0=THC0;//重新赋初值
TL0=TLC0;
aa++;
if(aa==50)//每100ms
{
aa=0;
flag0=1;
}
cnt++; 
//pid 周期
Bit++;
time++;  //转速测量周期
if(Bit>8) Bit=0;//数码管总共只有8位，超过8位则在程序上进行清零
//数码管显示部分
P0=0xff;
P2=Duan[Data_Buffer[Bit]];
switch(Bit)
{
case 0:P0=0X7F;break;
case 1:P0=0XBF;break;
case 2:P0=0XDF;break;
case 3:P0=0XEF;break;
case 4:P0=0XF7;break;
case 5:P0=0XFB;break;
case 6:P0=0XFD;break;
case 7:P0=0XFE;break;
}
//数码管显示部分
if(time>500)//每1s处理一次脉冲
{
time=0;
num=Inpluse*15;//实际转速，*15是由电机决定的，电机的一个脉冲对应着电机转过了15转
Inpluse=0; 
   //清零，为下一次计数做准备
PIDControl();  //调用PID控制程序
}

}



//串口中断，用于对串口的数据进行处理

void u_int(void) interrupt 4

{
ES=0;//关串口中断，避免在数据处理的过程中造成影响
if(RI)//若有RI==1，由RI产生中断
{
RI=0;//RI标志位必须通过软件进行清零
arry[i]=SBUF;//将字符赋到arry中
i++; //下一个数
if(i>3) 
//接收完了指令，进行数据处理，一共有arry[0],arry[1],arry[2],arry[3]四个数据
{
flag1=1;//将标志置1
i=0; //i清零，重新计数
}
if(flag1) 
//若flag1==1
{
flag1=0;//flag1清零
SpeedSet=(arry[0]-'0')+(arry[1]-'0')*10+(arry[2]-'0')*100+(arry[3]-'0')*1000;//获得的速度值
//由于串口发送的是字符，所以要减去'0'
}
}
else//对应的由TI产生中断
TI=0;//TI由软件进行清零

ES=1;//处理完后再开中断

}

​在很多学科竞赛中，我们时常会使用到电机。那么本篇文章我们就来说说如何基于STM32F4xx来驱动电机。

首先本篇文章的内容主要有以下几点：
一.我们需要控制什么样的电机。

二.控制这个电机所需要学习的知识。

三.如何对电机的速度进行控制。
一. 我们需要控制什么样的电机


我们所要控制的是带有编码器的电机，编码器的工作原理下文会讲到，这里将他理解为可以读出当前电机速的一个模块就好。
接下来我们对上图电机的接线简单的说明一下：由上至下依次标号1 ~ 6

1，6号线：电机电源输入。
2，5号线：电机编码器供电。
3，4号线双向编码器读值线路。
二、控制这个电机所需要学习的知识。
1.初始化相关GPIO口。
示例：
 GPIO_InitTypeDef  GPIO_InitStructure;
 RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);   //使能PB,PE端口时钟
​
 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;         //LED0-->PB.5 端口配置
 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;      //推挽输出
 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;     //IO口速度为50MHz
 GPIO_Init(GPIOB, &GPIO_InitStructure);           //根据设定参数初始化GPIOB.5
 GPIO_SetBits(GPIOB,GPIO_Pin_5);             //PB.5 输出高电平

初始化GPIO口几点注意事项：
1.开启时钟的总线需要准确。(查找总线翻阅参考手册即可)
2.配置的输出模式需要根据使用的用途进行调整。
​2. TIM定时器使用。
TIM定时器使用之定时器：顾名思义就是将TIM定时器作为定时器然后使其产生中断，然后我们对中断服务函数进行操作。

这部分最重要的就是了解定时器定时时间的计算：
例：寄存器配置如下：
	TIM_TimeBaseInitStruct.TIM_Period        = (10000 - 1);   //重装载值
    TIM_TimeBaseInitStruct.TIM_Prescaler     = (7200 - 1);    //预分频系数

在默认时钟为72M时计算如下:

预分频系数为7199，那么定时器时钟频率为72M / 7200 = 10kHz
周期为0.1ms，定时器计数达到重装值时候，产生定时器中断
则定时器中断时间为10000 * 0.1ms = 1s
TIM定时器使用之PWM输出：即使用TIM的PWM模式使对应IO口输出PWM方波。部分配置主要代码：

  TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //选择定时器模式:TIM脉冲宽度调制模式1      设置为模式一即小于CCRX为有效
  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
  TIM_OCInitStructure.TIM_Pulse =250;        //CCRx
  TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //输出极性:TIM输出的有效极性  低
  TIM_OC3Init(TIM8, &TIM_OCInitStructure);  //根据T指定的参数初始化外设TIM8 通道3
 
  TIM_OC3PreloadConfig(TIM8, TIM_OCPreload_Enable);  //使能TIM8在CCR1上的预装载寄存器
  TIM_ARRPreloadConfig(TIM8,ENABLE);//ARPE使能 //这个周期生效或下个周起生效
  TIM_Cmd(TIM8, ENABLE);  //使能TIM8
  
  TIM_CtrlPWMOutputs(TIM8, ENABLE);     //高级定时器需要这句才能输出PWM

TIM定时器使用之编码器读值：即使用定时器的编码器模式，对电机速度进行读值。部分配置主要代码：
// 定时器编码器模式初始化
  //配置时基单元
  TIM_DeInit(TIM3);                //复位TIM3相关寄存器
  TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);      //初始化
  TIM_TimeBaseStructure.TIM_Prescaler = 0;            //分频值
  TIM_TimeBaseStructure.TIM_Period = 65535;            //自动重装载
  TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;      //
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Down; // 这里可以自己设置，或使用默认值
  TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
  //此处：TIM_ICPolarity_Rising 意思是不反相
  TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising ,TIM_ICPolarity_Rising); // 这里配置了编码器模式
  //配置编码器
  TIM_ICStructInit(&TIM_ICInitStructure);  //该结构中按缺省值（默认）填入
  TIM_ICInitStructure.TIM_Channel=TIM_Channel_1 |TIM_Channel_2;    //通道
  TIM_ICInitStructure.TIM_ICPolarity=TIM_ICPolarity_Rising;
  TIM_ICInitStructure.TIM_ICSelection =TIM_ICSelection_DirectTI;  //设置
  TIM_ICInitStructure.TIM_ICFilter = 0;  //配置滤波器值
  TIM_ICInit(TIM3, &TIM_ICInitStructure);  //设置通道
  TIM_SetCounter(TIM3, 0);
  TIM_Cmd(TIM3, ENABLE);            //定时器总开关

这里使用的是TIM3定时器的通道1和2。
3.PID算法相关知识：
这部分学习可参考：https://www.bilibili.com/video/BV1Ds411t7x4?from=search&seid=17723158423363342532
三.如何对电机的速度进行控制
学习完上面这部分，我们对于控制电机所需要用到的部分已经清楚了。
接下来我们在只要将上面的配置结合起来就能做到对电机的控制。
结合思路如下：

初始化GPIO口。
设定一个定时器为编码器模式，并初始化它。
设置一个定时器，负责每隔一段时间将编码器的计数值读出来。
设置PWM输出，相关寄存器配置。
结合编码器读出的数值编写PID算法。
根据PID算法所返回的值，改变PWM占空比。

PID参考链接



PID增量控制 

#include"Pid_Init.h"
#include"main.h"
extern u16 out;
extern u16 CurSpeed;
extern u16 SpeedSet;
extern u16 PWMTime;

int error=0;	//当前偏差
int error1=0;		//上次偏差
int error2=0;		//上上次偏差

struct Pid_Struct{
	
	float kp;
	float ki;
	float kd;	//pid控制系数
	
	float uk;		//当前增量输出
	float uk1;	//上次增量输出
	float uk2; //上上次增量输出
	

};

struct Pid_Struct PID;

//PID参数初始化
void PID_Init(){
	
	PID.kp=15;
	PID.ki=10;
	PID.kd=3;
	
	PID.uk=0;
	PID.uk1=0;
	PID.uk2=0;

}
//PID增量算法控制转速
void PID_Control(){
	
	error=SpeedSet-CurSpeed;//
	
	PID.uk2=(PID.kp*(error-error1) \
	
			+PID.ki*error \
	
			+PID.kd*(error-2*error1+error2))/50;
			/*增量PID算法*/
	PID.uk=PID.uk1+PID.uk2;
	
	out=(int)PID.uk;
	
	if(out>1000)
	{
		out=1000;
	}
	else if(out<0)
	{
		out=0;
	}
	
	PID.uk1=PID.uk;
	error2=error1;
	error1=error;
	PWMTime=out;

}

工程代码：

链接：https://pan.baidu.com/s/1LbPOF4FrE9WGQI-GvVADcQ 
提取码：maha

 

傅里叶变换与不确定性  卓晴
速度采样频率  卓晴
在线拟合工具

新概念：积分限幅。在物理量达到期望值之前防止误差积分越来越大使得没有稳定的反而作用相反了。而有了积分限幅之后在积分误差达到幅值的时候将会保持不变，其目的是让PID控制函数不会输出一个很大的值，这样就能让物理量稳定下来。

从机
uint8_t start_flag = 0;
uint8_t finish_flag = 0;
uint8_t cnt = 0;
uint8_t data_to_receive[2];
void UART4_IRQHandler(void)
{
 //接收数据帧
 if(start_flag == 1 && finish_flag == 0) {
  if (cnt < 2)
   data_to_receive[cnt++] = USART_ReceiveData(UART4);
  if (cnt == 2)
   finish_flag = 1;
 }
 
 //检测是否为帧头
 if (start_flag == 0) {
  uint8_t start_byte = USART_ReceiveData(UART4);
  if (start_byte == 0xAA)
   start_flag = 1;
 }
 
 //检测帧尾
 if (finish_flag == 1) {
  uint8_t finish_byte = USART_ReceiveData(UART4);
  if (finish_byte == 0x11 && cnt == 2) {
   finish_flag = 0;
   start_flag = 0;
   cnt = 0;
   
   int16_t data = *((int16_t *)data_to_receive);
   int pwm = (int)((data + 36.705882) / 2.111569);
   MotorRun(data, data);
  }
 }
 
}


主机

增加了速度方向的显示。
检测标志位来判断方向不稳定，会有毛刺出现。

通过一个现实不可能达到的阈值来检测方向，效果不错。

开环测试PWM占空比与转速的关系
初始图像



提取的数据




x（非占空量）
y（rps * 100）




100
168


125
225


150
273


175
331


200
388


225
437


250
495


275
552


300
600


325
658


350
706


375
760


400
811


425
860


450
911


475
965


500
1005


进行线性拟合



函数方程表达式为：$F(x) = 2.11156862745098*x-36.70588235294107$

拟合误差：$0.9995420363644586$
PID调节（速度采样和控制间隔均为20ms）
kp = 0.80, ki = 2.50, kd = 0.00, SetSpeed = 6.0