自平衡小车控制代码
两轮自平衡小车双闭环PID控制设计
两轮⾃平衡⼩车双闭环PID控制设计两轮⾃平衡⼩车的研究意义1.1两轮平衡车的研究意义两轮平衡车是⼀种能够感知环境,并且能够进⾏分析判断然后进⾏⾏为控制的多功能的系统,是移动机器⼈的⼀种。
在运动控制领域中,为了研究控制算法,建⽴两轮平衡车去验证控制算法也是⾮常有⽤的,这使得在研究⾃动控制领域理论时,两轮平衡车也被作为课题,被⼴泛研究。
对于两轮平衡车模型的建⽴、分析以及控制算法的研究是课题的研究重点和难点。
设计的两轮平衡车实现前进、后退、转弯等功能是系统研究的⽬的,之后要对车⼦是否能够爬坡、越野等功能进⾏测试。
⼀个⾼度不稳定,其动⼒学模型呈现多变量、系统参数耦合、时变、不确定的⾮线性是两轮平衡车两轮车研究内容的难点,其运动学中的⾮完整性约束要求其控制任务的多重性,也就是说要在平衡状态下完成指定的控制任务,如在复杂路况环境下实现移动跟踪任务,这给系统设计带来了极⼤的挑战。
因此可以说两路平衡车是⼀个相对⽐较复杂的控制系统,这给控制⽅法提出了很⾼的要求,对控制理论⽅法提出来很⼤的挑战,是控制⽅法实现的典型平台,得到该领域专家的极⼤重视,成为具有挑战性的控制领域的课题之⼀。
两轮平衡车是⼀个复杂系统的实验装置,其控制算法复杂、参数变化⼤,是理论研究、实验仿真的理想平台。
在平衡车系统中进⾏解賴控制、不确定系统控制、⾃适应控制、⾮线性系统控制等控制⽅法的研究,具有物理意义明显、⽅便观察的特点,并且平衡车从造价来说不是很贵,占地⾯积⼩,是很好的实验⼯具，另外建⽴在此基础上的平衡系统的研究,能够适应复杂环境的导航、巡视等,在⼯业⽣产和社会⽣中具有⾮常⼤的应⽤潜⼒。
两轮平衡车所使⽤的控制⽅法主要有:状态回馈控制、PID控制、最优控制、极点回馈控制等,这些控制⽅法被称为传统控制⽅法。
1.2 本⽂研究内容（1）两轮⾃平衡⼩车的简单控制系统设计。
（2）基于倒⽴摆模型的两轮⾃平衡⼩车的数学建模。
（3）利⽤MATLAB⼯具进⾏两轮⾃平衡⼩车的系统控制⽅法分析。
使用STM32CubeMx搭建平衡小车代码框架
使用STM32CubeMx快速搭建平衡小车代码框架硬件平台：STM32CubeMxHAL代码库：STM32F1xx项目平台:MDK5.17A1.项目总体框架如下:MPU6050的数据读取采用软件模拟IIC,可使用MPU的DMP库直接生成角度值，减轻MCU计算负担;(DMP库资源详见，正点原子MPU6050资料)电机驱动部分采用市面上常见的直流电机驱动，引脚分布如下：PWMA,PWMB,A0,A1,B0,B1；其中PWMA、PWMB为电机驱动信号；A0、A1、B0、B1为电机方向控制信号，其控制电平如下：A0 A1 电机高高制动高低正转低高反转低低停止其中，制动为电机锁死，而停止为电机停转；2.项目搭建：Step1.打开STM32CubeMX，单击“New Project”，选择芯片型号，STM32F103C8Tx。
Step2.配置Debug，根据实际选择Step3.配置外部时钟信号Step4.配置TIM2(PWM发生器)Step5.配置模拟IIC引脚Step6.配置电机控制引脚Step7.配置TIM3(用作微妙延时时钟)，CubeMx生成的代码中不包含微妙延时，此部分用于实现模拟IIC的微妙延时Step8.配置USART1(用于串口调试)Step9.时钟配置注：关于输入时钟一定要按实际晶振频率填写，否则会造成时序混乱；参数配置（10KHz）Step11.配置TIM3(微妙延时定时器)定时器时钟频率的计算：定时器时钟频率:72MHz72MHz/(PSC+1)/ARR=72/(71+1)/1=1Mhz=1us; Step12.配置GPIO口Step13.生成项目配置至此，关于平衡小车的软件框架配置已全部完成，点击项目生成，进入MDK 编写代码：代码片段1：微妙函数的实现#include ""#include ""void Delay_us(uint32_t us){uint16_t counter=us&0xffff;HAL_TIM_Base_Start(&htim3);__HAL_TIM_SetCounter(&htim3,counter);while(counter>1){counter=__HAL_TIM_GetCounter(&htim3);}HAL_TIM_Base_Stop(&htim3);}void Delay_ms(uint32_t ms){Delay_us(1000*ms);}代码片段2 模拟IIC：#define HIGH 1#define LOW 0#define SDA_IN() {GPIOB->CRL&=0x0FFFFFFF;GPIOB->CRL|=0x;}#define SDA_OUT() {GPIOB->CRL&=0x0FFFFFFF;GPIOB->CRL|=0x;}#define IIC_SCL(n) (nHAL_GPIO_WritePin(GPIOB,GPIO_PIN_6,GPIO_PIN_SET):HAL_GPIO_WritePin( GPIOB,GPIO_PIN_6,GPIO_PIN_RESET)) //SCL#define IIC_SDA(n) (nHAL_GPIO_WritePin(GPIOB,GPIO_PIN_7,GPIO_PIN_SET):HAL_GPIO_WritePin( GPIOB,GPIO_PIN_7,GPIO_PIN_RESET)) //SDA#define READ_SDA HAL_GPIO_ReadPin(GPIOB,GPIO_PIN_7)void IIC_Init(void){IIC_SDA(HIGH);IIC_SCL(HIGH);}void IIC_Start(void){SDA_OUT();IIC_SDA(HIGH);IIC_SCL(HIGH);Delay_us(4);IIC_SDA(LOW);Delay_us(4);IIC_SCL(LOW);}void IIC_Stop(void){SDA_OUT();IIC_SCL(LOW);IIC_SDA(LOW);Delay_us(4);IIC_SCL(HIGH);IIC_SDA(HIGH);Delay_us(4);}uint8_t IIC_Wait_Ack(void){uint8_t ucErrTime=0;SDA_IN();IIC_SDA(HIGH);Delay_us(1);IIC_SCL(HIGH);Delay_us(1);while(READ_SDA){ucErrTime++;if(ucErrTime>250){IIC_Stop();return 1;}}IIC_SCL(LOW);return 0;}void IIC_Ack(void){IIC_SCL(LOW);SDA_OUT();IIC_SDA(LOW);Delay_us(2);IIC_SCL(HIGH);Delay_us(2);IIC_SCL(LOW);}void IIC_NAck(void){IIC_SCL(LOW);SDA_OUT();IIC_SDA(HIGH);Delay_us(2);IIC_SCL(HIGH);Delay_us(2);IIC_SCL(LOW);}void IIC_Send_Byte(uint8_t txd) {uint8_t t;SDA_OUT();IIC_SCL(LOW);for(t=0;t<8;t++){IIC_SDA((txd&0x80)>>7);txd<<=1;Delay_us(2);IIC_SCL(HIGH);Delay_us(2);IIC_SCL(LOW);Delay_us(2);}}uint8_t IIC_Read_Byte(uint8_t ack) {uint8_t i,receive=0;SDA_IN();for(i=0;i<8;i++ ){IIC_SCL(LOW);Delay_us(2);IIC_SCL(HIGH);receive<<=1;if(READ_SDA)receive++;Delay_us(1);}if (!ack)IIC_NAck();else IIC_Ack();return receive;}代码片段3 PID控制器//50#define P_DATA//#define I_DATA//#define D_DATA 0//以上三值需根据实际调整参数typedef struct PID{int SetPoint;double Proportion;double Integral;double Derivative;int LastError;int PrevError;}PID;void IncPIDInit(PID* sptr){sptr->LastError=0;sptr->PrevError=0;sptr->Proportion=P_DATA;sptr->Integral=I_DATA;sptr->Derivative=D_DATA;sptr->SetPoint=0;}int IncPIDCalc(PID* sptr,int nextPoint){int iError,iIncpid;iError=sptr->SetPoint-nextPoint;iIncpid=sptr->Proportion*iError-\sptr->Integral*sptr->LastError+\sptr->Derivative*sptr->PrevError;sptr->PrevError=sptr->LastError;sptr->LastError=iError;return iIncpid;}代码片段4 PWM发生器HAL_GPIO_WritePin(Left_Dir0_GPIO_Port,Left_Dir0_Pin,GPIO_PIN_S ET);HAL_GPIO_WritePin(Left_Dir1_GPIO_Port,Left_Dir1_Pin,GPIO_PIN_R ESET);HAL_GPIO_WritePin(Right_Dir0_GPIO_Port,Right_Dir0_Pin,GPIO_PIN _SET);HAL_GPIO_WritePin(Right_Dir1_GPIO_Port,Right_Dir1_Pin,GPIO_PIN _RESET);__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,0);__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,0);HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1);HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_2);注：更改PWM的占空比使用HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,val)函数即可，其占空比的为用户设定的值除以ARR的值，即val/99+1;即val直接等于占空比；后记：关于DMP的代码直接参考正点原子的MPU6050的代码即可；。
平衡小车调试指南(直立环 速度环)
平衡小车调试指南接下来将和大家一起以工程的思想去完成一个平衡小车的调试，包括平衡小车的直立环、速度环、转向环，一般我们是先调试直立环，再调试速度环，最好调试转向环。
另外需要说明的是，因为我们使用的电机性能非常好，对PID参数不敏感，也就是说每个参数的取值范围都很广，这将对我们接下来的调试有很大的帮助。
1.1平衡小车直立控制调试平衡小车直立环使用PD（比例微分）控制器，其实一般的控制系统单纯的P控制或者PI控制就可以了，但是那些对干扰要做出迅速响应的控制过程需要D （微分）控制。
下面是直立PD控制的代码：int balance(float Angle,float Gyro){float Bias,kp=300,kd=1;int balance;Bias=Angle-0;//计算直立偏差balance=kp*Bias+Gyro*kd;//计算直立PWMreturn balance;//返回直立PWM}入口参数是平衡小车倾角和Y轴陀螺仪（这个取决MPU6050的安装），我们的小车前进方向是MPU6050的X轴的正方向，电机轴与Y轴平行。
前面两行是相关变量的定义，第三行是计算角度偏差，第四行通过PD控制器计算直立PWM，最后一行是返回。
调试过程包括确定平衡小车的机械中值、确定kp值的极性（也就是正负号）和大小、kd值的极性和大小等步骤。
在调试直立环的时候，我们要屏蔽速度环和转向环，如下图所示：1.1.1确定平衡小车的机械中值把平衡小车放在地面上，绕电机轴旋转平衡小车，记录能让小车接近平衡的角度，一般都在0°附近的。
我们调试的小车正好是0度，所以就是Bias=Angle-0;1.1.2确定kp值的极性（令kd=0）首先我们估计kp的取值范围。
我们的PWM设置的是7200代表占空比100%，假如我们设定kp值为720，那么平衡小车在±10°的时候就会满转。
根据我们的感性认识，这显然太大了，那我们就可以估计kp值在0~720之间，首先大概我们给一个值kp=-200,我们可以观察到，小车往哪边倒，电机会往那边加速让小车到下，就是一个我们不愿看到的正反馈的效果。
[应用]两轮自平衡小车测试程序
![[应用]两轮自平衡小车测试程序](https://img.taocdn.com/s3/m/28098909640e52ea551810a6f524ccbff121ca31.png)
两轮自平衡小车测试程序/********************************************************** *************// 两轮自平衡车最终版控制程序（6轴MPU6050+互补滤波+PWM电机）// 单片机STC12C5A60S2// 晶振：20M// 日期：2012.11.26 - ？*********************************************************** ************/#include <REG52.H>#include <math.h>#include <stdio.h>#include <INTRINS.H>typedef unsigned char uchar;typedef unsigned short ushort;typedef unsigned int uint;//******功能模块头文件*******#include "DELAY.H" //延时头文件//#include "STC_ISP.H" //程序烧录头文件#include "SET_SERIAL.H" //串口头文件#include "SET_PWM.H" //PWM头文件#include "MOTOR.H" //电机控制头文件#include "MPU6050.H" //MPU6050头文件//******角度参数************float Gyro_y; //Y轴陀螺仪数据暂存float Angle_gy; //由角速度计算的倾斜角度float Accel_x; //X轴加速度值暂存float Angle_ax; //由加速度计算的倾斜角度float Angle; //小车最终倾斜角度uchar value; //角度正负极性标记//******PWM参数*************int speed_mr; //右电机转速int speed_ml; //左电机转速int PWM_R; //右轮PWM值计算int PWM_L; //左轮PWM值计算float PWM; //综合PWM计算float PWMI; //PWM积分值//******电机参数*************float speed_r_l; //电机转速float speed; //电机转速滤波float position; //位移//******蓝牙遥控参数*************uchar remote_char;char turn_need;char speed_need;//*********************************************************//定时器100Hz数据更新中断//********************************************************* void Init_Timer1(void) //10毫秒@20MHz,100Hz刷新频率{ AUXR &= 0xBF; //定时器时钟12T模式TMOD &= 0x0F; //设置定时器模式TMOD |= 0x10; //设置定时器模式TL1 = 0xE5; //设置定时初值TH1 = 0xBE; //设置定时初值TF1 = 0; //清除TF1标志TR1 = 1; //定时器1开始计时}//*********************************************************//中断控制初始化//*********************************************************void Init_Interr(void){ EA = 1; //开总中断EX0 = 1; //开外部中断INT0EX1 = 1; //开外部中断INT1IT0 = 1; //下降沿触发IT1 = 1; //下降沿触发ET1 = 1; //开定时器1中断}//******卡尔曼参数************float code Q_angle=0.001;float code Q_gyro=0.003;float code R_angle=0.5;float code dt=0.01; //dt为kalman滤波器采样时间;char code C_0 = 1;float xdata Q_bias, Angle_err;float xdata PCt_0, PCt_1, E;float xdata K_0, K_1, t_0, t_1;float xdata Pdot[4] ={0,0,0,0};float xdata PP[2][2] = { { 1, 0 },{ 0, 1 } };//*********************************************************// 卡尔曼滤波//*********************************************************//Kalman滤波，20MHz的处理时间约0.77ms；void Kalman_Filter(float Accel,float Gyro){ Angle+=(Gyro - Q_bias) * dt; //先验估计Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分Pdot[1]=- PP[1][1];Pdot[2]=- PP[1][1];Pdot[3]=Q_gyro;PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差PP[1][0] += Pdot[2] * dt;PP[1][1] += Pdot[3] * dt;Angle_err = Accel - Angle; //zk-先验估计PCt_0 = C_0 * PP[0][0];PCt_1 = C_0 * PP[1][0];E = R_angle + C_0 * PCt_0;K_0 = PCt_0 / E;K_1 = PCt_1 / E;t_0 = PCt_0;t_1 = C_0 * PP[0][1];PP[0][0] -= K_0 * t_0; //后验估计误差协方差PP[0][1] -= K_0 * t_1;PP[1][0] -= K_1 * t_0;PP[1][1] -= K_1 * t_1;Angle += K_0 * Angle_err; //后验估计Q_bias += K_1 * Angle_err; //后验估计Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度}//*********************************************************// 倾角计算（卡尔曼融合）//*********************************************************void Angle_Calcu(void){ //------加速度--------------------------//范围为2g时，换算关系：16384 LSB/g//角度较小时，x=sinx得到角度（弧度）, deg = rad*180/3.14//因为x>=sinx,故乘以1.3适当放大Accel_x = GetData(ACCEL_XOUT_H); //读取X轴加速度Angle_ax = (Accel_x - 1100) /16384; //去除零点偏移,计算得到角度（弧度）Angle_ax = Angle_ax*1.2*180/3.14; //弧度转换为度,//-------角速度-------------------------//范围为2000deg/s时，换算关系：16.4 LSB/(deg/s)Gyro_y = GetData(GYRO_YOUT_H); //静止时角速度Y 轴输出为-30左右Gyro_y = -(Gyro_y + 30)/16.4; //去除零点偏移，计算角速度值,负号为方向处理//Angle_gy = Angle_gy + Gyro_y*0.01; //角速度积分得到倾斜角度.//-------卡尔曼滤波融合-----------------------Kalman_Filter(Angle_ax,Gyro_y); //卡尔曼滤波计算倾角/*//-------互补滤波-----------------------//补偿原理是取当前倾角和加速度获得倾角差值进行放大，然后与//陀螺仪角速度叠加后再积分，从而使倾角最跟踪为加速度获得的角度//0.5为放大倍数，可调节补偿度；0.01为系统周期10msAngle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01);*/ }//*********************************************************//电机转速和位移值计算//*********************************************************void Psn_Calcu(void){ speed_r_l =(speed_mr + speed_ml)*0.5;speed *= 0.7; //车轮速度滤波speed += speed_r_l*0.3;position += speed; //积分得到位移position += speed_need;if(position<-6000) position = -6000;if(position> 6000) position = 6000;}static float code Kp = 9.0; //PID参数static float code Kd = 2.6; //PID参数static float code Kpn = 0.01; //PID参数static float code Ksp = 2.0; //PID参数//********************************************************* //电机PWM值计算//*********************************************************void PWM_Calcu(void){ if(Angle<-40||Angle>40) //角度过大，关闭电机{ CCAP0H = 0;CCAP1H = 0;return;}PWM = Kp*Angle + Kd*Gyro_y; //PID：角速度和角度PWM += Kpn*position + Ksp*speed; //PID：速度和位置PWM_R = PWM + turn_need;PWM_L = PWM - turn_need;PWM_Motor(PWM_L,PWM_R);}//*********************************************************//手机蓝牙遥控//*********************************************************void Bluetooth_Remote(void){ remote_char = receive_char(); //接收蓝牙串口数据if(remote_char ==0x02) speed_need = -80; //前进else if(remote_char ==0x01) speed_need = 80; //后退else speed_need = 0; //不动if(remote_char ==0x03) turn_need = 15; //左转else if(remote_char ==0x04) turn_need = -15; //右转else turn_need = 0; //不转}/*========================================================= =======*///*********************************************************//main//*********************************************************void main(){ delaynms(500); //上电延时Init_PWM(); //PWM初始化Init_Timer0(); //初始化定时器0，作为PWM时钟源Init_Timer1(); //初始化定时器1Init_Interr(); //中断初始化Init_Motor(); //电机控制初始化Init_BRT(); //串口初始化（独立波特率）InitMPU6050(); //初始化MPU6050delaynms(500);while(1){ Bluetooth_Remote();}}/*========================================================= ===*///********timer1中断***********************void Timer1_Update(void) interrupt 3{ TL1 = 0xE5; //设置定时初值10MSTH1 = 0xBE;//STC_ISP(); //程序下载Angle_Calcu(); //倾角计算Psn_Calcu(); //电机位移计算PWM_Calcu(); //计算PWM值speed_mr = speed_ml = 0;}//********右电机中断***********************void INT_L(void) interrupt 0{ if(SPDL == 1) { speed_ml++; } //左电机前进else { speed_ml--; } //左电机后退LED = ~LED;}//********左电机中断***********************void INT_R(void) interrupt 2{ if(SPDR == 1) { speed_mr++; } //右电机前进else { speed_mr--; } //右电机后退LED = ~LED;}。
stm32平衡小车控制部分代码解析
买了这个车子好久了，拿到代码，一头雾水。
所谓的什么用户手册真的是，扯。
没考虑到初学者的感受。
但，问题总要解决，所以用这篇文章来分析一下平衡小车之家出的这个车子吧。
尽管有很多的库函数和宏，其实有很多都是厂商提供的，怎么实现的，大多每个代码段都提供了comment，英文好的话，理解应该不存在问题。
而我们做开发，首要关心的并不是每个模块如何实现，而是应该有几个模块，这些模块是怎么工作的，一个平衡小车能够安稳地在平面上静止与运动，都是通过晶振可以提供一定频率的中断，每一段时间都触发执行了中断服务程序，在此程序中可以获得此刻的欧拉角、温度以及对于平衡小车编码器的数据信息，并且通过PID控制来对小车的步进电机进行控制。
控制部分主要由MPU6050的INT引脚触发5ms定时中断，这个中断是由EXTI控制(具体如何配置要参考MPU6050的INT引脚的特性)。
本车的INT引脚挂载到PA12上，那么他相应的中断服务函数就应该是EXTI15_10_IRQHandler。
接下来我们来分析一下它是如何来控制小车的。
Begining~基本的平衡控制。
1、 读取编码器的值。
--------------------Encoder_Left=Read_Encoder(2);Encoder_Right=Read_Encoder(4); 2、 获取欧拉角，即俯仰角，偏航角和横滚角。
------------------Get_Angle(Way_Angle);3、 平衡、速度PID 控制。
Balance_Pwm =balance(Angle_Balance,Gyro_Balance); Velocity_Pwm=velocity(Encoder_Left,Encoder_Right);4、 赋值给PWM 寄存器。
Moto1=Balance_Pwm-Velocity_Pwm-Turn_Pwm;Moto2=Balance_Pwm-Velocity_Pwm+Turn_Pwm;Set_Pwm(Moto1,Moto2);平衡+移动控制刨去超声波，led 灯，按键的读取，新增加了转向环的控制。
基于STM32的两轮自平衡小车控制系统设计
基于STM32的两轮自平衡小车控制系统设计本文主要对两轮自平衡小车的姿态检测算法、PID控制算法两方面进行展开研究。
用加速度传感器和陀螺仪传感器融合而成的姿态传感系统与互补滤波器组合得到自平衡小车准确而稳定的姿态信息，然后PID调节器则利用这些姿态信息输出电机控制信号，控制电机的转动，从而使小车得以平衡。
标签：STM32；自平衡小车；控制系统；控制算法1 研究意义应用意义：两轮平衡车是一种新型的交通工具，它与电动自行车和摩托车车轮前后排列方式不同，而是采用两轮并排固定的方式，就像一种两轮平行的机器人一样。
两轮自平衡控制系统是一种两轮左右平行布置的，像传统的倒立摆一样，本身是一个自然不稳定体，必须施加强有力的控制手段才能使之稳定。
两轮平衡车具有运动灵活、智能控制、操作简单、节省能源、绿色环保、转弯半径为0等优点。
因此它适用于在狭小空间内运行，能够在大型购物中心、国际性会议或展览场所、体育场馆、办公大楼、大型公园及广场、生态旅游风景区、城市中的生活住宅小区等各种室内或室外场合中作为人们的中、短距离代步工具。
具有很大的市场和应用前景。
理论研究意义：车体状态运算主要是将各传感器测量的数据加以融合得出车体倾斜角度值、倾斜角速度值以及行车速度等。
平衡控制运算根据车体状态数据，计算保持平衡需要的行车速度和加速度，或者转弯所需要的左右电机速度变化值，向电机控制驱动模块发送控制指令。
运算模块相当于两轮自平衡电动车的大脑，它主要负责的工作是：控制电机的起停，向控制模块发出加速、减速、电机正反转和制动等速度控制信号，接收电机Hall信号进行车速度计算，并通过RS 一232串口向PC发送车速数据以供存储和分析。
另外，还负责接收车体平衡姿态数据，进行自平衡运算。
现有的自平衡车结构种类繁多，但车体都归根于由三层的基本结构组成，从上到下依次是电池层、主控层、电机驱动层。
电池层用于放置给整个系统供电的6V锂电池，主控层由主控芯片系统和传感器模块组成，电机驱动层接受单片机信号，并控制电机。
stm32的自平衡小车设计
stm32的自平衡小车设计STM32自平衡小车设计是一个将许多功能组合在一起的有趣项目。
它不仅需要控制技术，还需要实时处理图像，以便识别障碍物。
自平衡小车使用STM32单片机来控制，这是一款微控制器，具有16位或32位内外存储器、高速Cortex-M4 MCU和多种集成的外设。
STM32单片机的内部集成了多种传感器，如角度传感器、编码器、温度传感器和光学传感器等，可以测量周围环境的变化并作出相应的反应，使小车保持平衡。
它还有两个电机驱动的轮子，电机可以控制小车的前进和后退，而角度传感器可以测量小车的角度，从而判断小车是否已完成平衡，从而调整小车的动作来使其保持平衡。
此外，STM32开发板还具有I2C通信接口，可让开发者使用I2C总线通信，与外部设备交换数据，如摄像头等。
摄像头的主要功能是对周围环境的跟踪，可以帮助小车避开障碍物，准确地定位和预测小车的行驶路径。
为了使小车实现自主运动，还需要一块用于实现运动控制的FPGA芯片，可以用于处理传感器发来的控制信号，根据预设的算法以及图像处理结果，向STM32发出运动控制指令，使小车实现自动行驶。
FPGA芯片可以提供更高的运算速度，以满足实时性要求，这是实现智能小车自动行驶的重要前提。
最后，将所有的控制程序和程序连接在一起，并与SOC系统进行连接，形成一个完整的系统，以实现智能小车的自动行驶。
当实现了自动行驶的功能之后，可以根据需要添加更多的功能，比如跟踪、识别物体、定位、自动充电等，这些功能可以帮助小车自主行驶时更加“聪明”，也可以使小车更好地适应环境调整，实现自主运动。
总而言之，设计一台智能自平衡小车，其基本设计思路是：首先使用STM32单片机作为主控核心，集成传感器用于控制小车保持平衡，而两个电机驱动的轮子可以控制小车前后行驶；其次，使用I2C总线通信的图像传感器可以测量小车的方向，以避开前方的障碍物；最后，使用FPGA芯片实现小车的运动控制，实现智能小车的自动行驶。
利用PID控制算法控制自平衡车
近两年来，在公共场合常常能见到一种叫做体感车（或者叫平衡电动车）的代步工具，由于其便捷灵活，使得其颇为流行，并被称为“最后一公里神器”.其运作原理主要是建立在一种被称为“动态稳定”的基本原理上，也就是车辆本身的自动平衡能力。
以内置的精密固态陀螺仪来判断车身所处的姿势状态，透过精密且高速的中央计算出适当的指令后，驱动马达来做到平衡的效果。
下文采用AVR Atmega16芯片作为主，设计制作了两轮的自平衡电动车。
文中分析了测量角度和角速度传感器的选择，通过ATMEGA16单片机多路信号AD采集陀螺仪和加速度计的信号，经过Kalman滤波算法计算动态的角度和角速度，通过LCD1602显示角速度和角度的值、转向值。
利用PID控制算法控制自平衡车的平衡状态，使车体在平衡位置稳定。
利用大功率MOS管设计，通过单片机有效地控制电机的转速、电机的转向，从而有效地控制自平衡车的前进、后退及转弯功能。
我们来看看具体的设计细节吧。
1 研究意义随着科学技术水平的不断进步，交通工具正朝着小型、节能、环保的方向发展，“电动车”正是在这个背景下孕育而生并为人们所熟识。
据不完全统计，我国的电动车保有量已超过1.2亿辆，是增长速度最快的交通工具。
随着石油储量的不断减少和人们环保意识的增强，“电动车”无疑将成为未来交通工具的主力军。
就目前而言，电动车的种类主要有电动自行车、电动摩托车和电动汽车。
由于电动机制造水平的提高，尤其是大功率直流无刷电动机制造工艺的成熟，带动了电动自行车和电动摩托车行业的飞速发展。
同时，人们也根据两轮自平衡机器人工作原理，设计出了一些新式电动车--两轮自平衡电动车。
它是一种新型的交通工具，它一改电动自行车和摩托车车轮前后排列方式，而是采用两轮并排固定的方式，这种结构将给人们带来一种全新的驾驭感受。
两轮自平衡电动车仅靠两个轮子支撑车体，采用蓄电池提供动力，由电动机驱动，采用微处理器、姿态感知系统、控制算法及车体机械装置共同协调控制车体的平衡，仅靠人体重心的改变便可以实现车辆的启动、加速、减速、停止等功能。
基于PID控制器的两轮自平衡小车设计
基于PID控制器的两轮自平衡小车设计一、引言在自动控制领域中，PID控制器是一种常用的控制器。
它通过对系统输出进行反馈，来调节系统的输入，使系统的输出尽可能接近预期值。
本文将基于PID控制器设计一个两轮自平衡小车。
二、系统模型两轮自平衡小车是由两个驱动电机控制的，通过控制电机的转速来实现小车的前进、后退、转向等功能。
小车的整体结构是一个倒立摆，通过控制电机的转速，使其保持垂直状态。
系统的输入是电机转速，输出是小车的倾斜角度。
三、PID控制器PID控制器由比例（P）、积分（I）和微分（D）三个部分组成。
这三个部分根据误差来计算控制信号，实现对系统的控制。
1.比例控制部分：比例控制器根据误差的大小来计算控制信号。
误差是指系统输出与期望输出之间的差异。
比例控制器的计算公式为u_p=K_p*e(t)，其中u_p是比例输出，K_p是比例增益，e(t)是误差。
2. 积分控制部分：积分控制器根据误差的累积值来计算控制信号。
积分控制器的计算公式为u_i = K_i * ∫e(t)dt，其中u_i是积分输出，K_i是积分增益，∫e(t)dt是误差的累积值。
3. 微分控制部分：微分控制器根据误差的变化率来计算控制信号。
微分控制器的计算公式为u_d = K_d * de(t)/dt，其中u_d是微分输出，K_d是微分增益，de(t)/dt是误差的变化率。
PID控制器的输出信号为u(t)=u_p+u_i+u_d，其中u(t)是控制信号。
四、设计与实现在设计两轮自平衡小车的PID控制器时，需要根据系统的特性来选择合适的参数。
通常可以通过试验或仿真来获得系统的模型，进而进行参数调节。
1.参数调节：首先，可以将系统的转角作为输入信号，通过试验或仿真来获得小车的倾斜角度与转角的关系。
然后，可以根据比例、积分和微分控制部分的特性，来选择合适的增益参数。
比例增益越大，系统的响应速度越快，但可能会引起震荡；积分增益可以消除静态误差，但可能会引起过冲；微分增益可以减小震荡，但可能会引起超调。
两轮自平衡小车的PID控制
两轮自平衡小车的PID控制【摘要】两轮自平衡小车的核心问题是平衡控制问题和运动控制问题。
两轮自平衡小车需要始终保持车身直立，同时还需要完成各种机动动作，如行进、旋转、左转弯、右转弯等。
PID控制算法是应用最为普遍的一种算法，其特点是构造简单，应用有效及具备了许多成熟的稳定性分析的方法，有很高的可靠性。
针对两轮自平衡小车的非线性和不稳定性，利用非线性PD控制算法和PID差动结构可以实现小车的平衡控制和运动控制。
【关键词】两轮自平衡小车；PID控制；平衡控制；运动控制；控制算法1.引言两轮自平衡小车是一种典型的欠驱动系统（underactuated system）、非完整系统（nonholonomic system）。
其核心问题是对小车的平衡控制和运动控制，其中两轮自平衡小车的姿态平衡控制类似于倒立摆的平衡问题，所不同的是两轮自平衡小车可以在二维甚至三维空间内运动。
两轮自平衡小车不仅需要始终保持车身的直立，还需要在保持直立的同时在二维甚至三维空间内运动。
两轮自平衡小车有4个自由度：2个平面支撑运动自由度，2个姿态角运动自由度。
然而其中只有2个平面支撑运动自由度，即左轮和右轮可以驱动。
对于两轮自平衡小车，姿态平衡控制可以通过改变左轮和右轮的运动速度和运动方向来控制的。
当小车的车身发生倾斜时，左右电机产生相应的力矩来调节左右两轮运动速度和运动方向，使小车恢复平衡直立的状态。
小车的运动轨迹控制则是通过调整行进速度和行进方向来控制的。
两轮自平衡小车的行进速度是左轮线速度和右轮线速度的平均值，也是通过左右电机产生的力矩来调节。
行进方向则需要左轮和右轮的差动来调节，即对左轮和右轮施加不同的作用力矩，以产生不同的运动速度，从而实现两轮自平衡小车航向的控制。
PID控制算法是一种应用广泛、使用简单有效的经典的自动控制算法，两轮自平衡小车的平衡控制和运动控制都可以采用PID控制策略。
在1997年，日本的Hiraoka和Noritsugu研究出一种采用PID算法控制速度和位置的两轮平行小车[1]。
自平衡小车程序
static float pitch_rate;
static float interval;
static float timer0_seconds_conv;
static float p_gain;
static float d_gain;
static Uint16 stand=0x00;
static Uint16 last_balance_s0;
static Uint16 samples[6],ticks;
static float left_steer_cof=0.05;
static float right_steer_cof=0.05;
}
return 1;
}
Uint16 bat_judge( float x)
static float adclo=0.0;
static Uint16 totsamples_0;
unsigned int count;
#define OCRMAX 1023
unsigned int pwm;
static float left_cof=1.00;
static float right_cof=1.00;
void InitAdc(void);
void InitGpio(void);
struct gyro_filter {
float angle;
float ay_bias;
//float ax_bias;
float rate_bias;
float steer_knob_bias;
float rate;
pid基础介绍(包含代码)
pid基础介绍总所周知，PID算法是个很经典的东西。
而做自平衡小车，飞行器PID是一个必须翻过的坎。
因此本节我们来好好讲解一下PID，根据我在学习中的体会，力求通俗易懂。
并举出PID的形象例子来帮助理解PID。
一、首先介绍一下PID名字的由来：P：Proportion（比例），就是输入偏差乘以一个常数。
I：Integral（积分），就是对输入偏差进行积分运算。
D：Derivative（微分），对输入偏差进行微分运算。
注：输入偏差=读出的被控制对象的值-设定值。
比如说我要把温度控制在26度，但是现在我从温度传感器上读出温度为28度。
则这个26度就是”设定值“，28度就是“读出的被控制对象的值”。
然后来看一下，这三个元素对PID算法的作用，了解一下即可，不懂不用勉强。
P：打个比方，如果现在的输出是1，目标输出是100，那么P的作用是以最快的速度达到100，把P理解为一个系数即可；而I呢？大家学过高数的，0的积分才能是一个常数，I就是使误差为0而起调和作用；D呢？大家都知道微分是求导数，导数代表切线是吧，切线的方向就是最快到至高点的方向。
这样理解，最快获得最优解，那么微分就是加快调节过程的作用了。
二、然后要知道PID算法具体分两种：一种是位置式的，一种是增量式的。
在小车里一般用增量式，为什么呢？位置式PID的输出与过去的所有状态有关，计算时要对e（每一次的控制误差）进行累加，这个计算量非常大，而明显没有必要。
而且小车的PID控制器的输出并不是绝对数值，而是一个△，代表增多少，减多少。
换句话说，通过增量PID算法，每次输出是PWM要增加多少或者减小多少，而不是PWM的实际值。
所以明白增量式PID就行了。
三、接着讲PID参数的整定，也就是PID公式中，那几个常数系数Kp，Ti，Td等是怎么被确定下来然后带入PID算法中的。
如果要运用PID，则PID参数是必须由自己调出来适合自己的项目的。
通常四旋翼，自平衡车的参数都是由自己一个调节出来的，这是一个繁琐的过程。
基于PID控制的两轮平衡小车(附原理图和程序讲解)
课程设计题目基于PID控制的两轮平衡小车学院XXXXX 专业班级XXXXXX小组成员XXXX 指导教师XXXXX X年 XX 月 XXX小组成员介绍及分工小组成员信息小组成员分工目录机电系统实践与实验设计 (1)一、研究背景与意义 (2)二、平衡原理 (2)2.1 平衡车的机械结构 (2)2.2 自平衡车倾倒原因的受力分析 (3)2.3 平衡的方法 (3)三、两轮平衡小车总体设计 (4)3.1 整体构思 (4)3.2 姿态检测系统 (4)3.3 控制算法 (5)四、matlab建模及仿真 (6)4.1 机械模型建模及仿真（Matlab_simulink） (6)4.2 联合控制器仿真（理想状态PID） (8)五、硬件电路设计 (9)5.1、硬件电路整体框架 (9)5.2、系统运作流程介绍 (10)5.3、硬件电路模块 (10)5.31 电源供电部分 (10)5.32 主控制器部分: (10)5.33 传感器部分; (11)5.34 驱动电路部分 (11)5.35 蓝牙控制模块 (12)5.36 超声波检测模块 (13)5.37 寻迹模块 (13)六、软件控制模块 (14)6.1 系统软件设计结构 (14)6.2 整体初始化过程 (14)6.3 程序设计 (15)6.31 PID-三个参数的调整 (15)6.32 OLED显示信息 (16)6.33 PID-采集信息 (16)6.34 PID-数据计算 (17)6.35 PID-结果输出 (18)6.36 超声波避障 (18)6.37 蓝牙控制 (18)6.38 寻迹实现 (19)七、总结 (19)附录 (21)摘要：两轮自平衡车结合了两轮同轴、独立驱动、悬架结构的自平衡原理，是一种在微处理器控制下始终保持平衡的集智能化与娱乐性于一体的新型代步工具。
整车由底盘、动力装置、控制装置和转向装置组成。
机械结构采用了双轮双马达驱动；控制主要采用的是反馈调节，为了使车体更好的平衡，使用了PID调节方式；硬件上采用陀螺仪GY521 MPU-6050来采集车体的旋转角度以及旋转角加速度，采用加速度传感器来间接测量车体旋转角度，同时，加入超声波检测模块，使小车能够自动完成避障功能；通过在两轮平衡车上加入两个寻迹模块（光电传感器）来识别场地上的黑白线，使得两轮自平衡车能够沿着黑线进行寻迹完成循迹功能。
PID算法优化(以平衡小车代码为例)
PID算法优化（以平衡小车代码为例）PID控制是一种常用的反馈控制方法，广泛应用于自动控制系统中。
在平衡小车的代码中，PID算法被用于控制小车的平衡。
本文将以平衡小车代码为例，探讨如何优化PID算法。
优化PID算法的第一步是确定合适的初始参数。
可以使用经验法确定初始参数，然后再进行调整。
例如，可以将初始参数都设置为1，然后观察系统的响应情况，根据实际效果逐步调整。
其次，可以使用自适应或自校正的方法来优化PID算法。
自适应PID算法可以根据系统的实时状态动态调整PID参数，从而提高系统的控制效果。
自校正PID算法可以通过系统的实时反馈信息来判断是否需要调整PID参数，从而提高系统的稳定性和鲁棒性。
此外，可以考虑在PID算法中引入限制项，以避免超出系统能够承受的范围。
例如，在平衡小车代码中，可以限制小车的加速度和角度的变化范围，以防止系统过载或翻倒。
在优化PID算法过程中，还需要考虑系统的实时响应速度和稳定性。
必要时，可以进行系统参数的调整，以提高系统的响应速度和稳定性。
例如，可以调整调节参数的时间常数，改变PID算法的响应速度。
另外，可以考虑使用非线性控制方法来优化PID算法。
非线性控制方法可以更好地适应复杂的控制系统，提高系统的性能。
在平衡小车代码中，可以尝试使用基于神经网络的非线性控制方法，从而提高平衡小车系统的控制精度和稳定性。
最后，需要进行系统的实时监测和调试。
通过对系统的实时监测和调试，可以及时发现和解决问题，进一步提高系统的控制效果。
例如，可以使用PID调试工具对系统的实时响应进行监测和调试，从而找到问题的根源，进行相应的调整和优化。
综上所述，通过确定合适的初始参数、使用自适应或自校正的方法、引入限制项、调整系统参数、使用非线性控制方法和进行实时监测和调试，可以对PID算法进行优化，提高系统的控制效果和稳定性。
在平衡小车代码中，这些优化方法可以帮助实现更精确和稳定的平衡控制。
基于arduino的自平衡车代码
基于arduino的自平衡车代码#include "Wire.h"#include "I2Cdev.h"#include "MPU6050.h" MPU6050 accelgyro;int16_t ax, ay, az;int16_t gx, gy, gz;int timeChange;int E1 = 6;int M1 = 7;int E2 = 5;int M2 = 4;#define LED_PIN 13#define pi 3.14159#define Gry_offset -20#define Gyr_Gain 0.00763358bool blinkState = false;float angleA,omega,f_angle;float kp, ki, kd;float SampleTime = 0.08; //-------------------互补滤波+PID 采样时间0.08 sunsigned long lastTime;float Input, Output, Setpoint;float errSum,dErr,error,lastErr;float LSpeed_Need=0.0,RSpeed_Need=0.0;float LOutput,ROutput;unsigned long preTime = 0;void setup() {Wire.begin();Serial.begin(38400);Serial.println("Initializing I2C devices...");accelgyro.initialize();Serial.println("Testing device connections...");Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");pinMode(LED_PIN, OUTPUT);pinMode(M1, OUTPUT);pinMode(M2, OUTPUT);}void loop() {accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);angleA= atan2(ay , az) * 180 / pi+0.5;omega= Gyr_Gain * (gx + Gry_offset);blinkState = !blinkState;digitalWrite(LED_PIN, blinkState);Serial.print(angleA);Serial.print(omega);if (abs(angleA)<45) {PIDfilter();PWMB();}}void PIDfilter(){kp =0.9;ki =0.0002;kd =0.26;unsigned long now = millis(); // 当前时间(ms)float dt = (now - preTime) / 1000.0; // 微分时间(ms)preTime = now;float K = 0.8;float A = K / (K + dt);f_angle = A * (f_angle + omega * dt) + (1-A) * angleA;Serial.println(f_angle);timeChange = (now - lastTime);if(timeChange>=SampleTime){Setpoint=LSpeed_Need;// =0，(+ -值使电机前进或后退)Input =f_angle;error = Setpoint- Input;errSum += error* timeChange;dErr = (error - lastErr)/ timeChange;//PID OutputOutput = kp * error + ki * errSum + kd * dErr;LOutput=Output+RSpeed_Need+0.34;//左电机ROutput=Output-RSpeed_Need+0.34;//右电机lastErr = error;lastTime = now;Serial.print( Output);Serial.print( ROutput);Serial.println( LOutput);}}void PWMB(){if(LOutput>0.5)//左电机-------或者取0{digitalWrite(M1, HIGH);//digitalWrite(E1, LOW);Serial.print( 'a');analogWrite(E1,min(255,abs(LOutput)+255)); //PWM调速a==0-255 }else if(LOutput<-0.5)//-------或者取0{digitalWrite(M1, LOW);//digitalWrite(E1, HIGH);analogWrite(E1,min(255,abs(LOutput)+255)); //PWM调速a==0-255 Serial.print( 'b');}else //刹车-------取0后可以不用{digitalWrite(M1, LOW);digitalWrite(E1, 0);Serial.print( 'c');}if(ROutput>0.5)//右电机--------或者取0{digitalWrite(M2, HIGH);//digitalWrite(E2, LOW);analogWrite(E2,min(255,abs(ROutput)+253));Serial.print( 'd');}else if(ROutput<-0.5)//-------或者取0{digitalWrite(M2, LOW);//digitalWrite(E2, HIGH);analogWrite(E2,min(255,abs(ROutput)+253));Serial.print( 'e');}else//刹车-------取0后可以不用{digitalWrite(M2, LOW);digitalWrite(E2, 0);Serial.print( 'f');}// analogWrite(E1,min(255,abs(LOutput)+25)); //PWM调速a==0-255 //analogWrite(E2,min(255,abs(ROutput)+23));}。
自平衡小车程序
#include "DSP281x_Device.h" // Headerfile Include File#include "DSP281x_Examples.h" //#define HIST 16unsigned int k=0;static float adc_current_samples[6][HIST];//定义行为6，列为16的二维数组static float adclo=0.0;static Uint16 totsamples_0;unsigned int count;#define OCRMAX 1023unsigned int pwm;static float left_cof=1.00;static float right_cof=1.00;struct gyro_filter pitch_filter;static Uint16 mode=0x00;//0x00----balance mode; 0x01-------assist mode static Uint16 still=0x00; //still=1.0, no move; still=0.0,move. static Uint16 stand=0x00;static Uint16 last_balance_s0;static Uint16 samples[6],ticks;static float left_steer_cof=0.05;static float right_steer_cof=0.05;static float steer_lim=0.13;static float initial_angle=0.0000874;static float hard_speed_lim=0.90;static float bus_current;static float cmd;static float lpf_angle;static float lpf_angrate;static float lpf_steer_knob;static float left_motor_pwm, right_motor_pwm, steer_cmd;static float left_motor_pwm, right_motor_pwm, steer_cmd;static float batt_voltage1, ay, ax, in_steer_knob;static float pitch_rate;static float interval;static float timer0_seconds_conv;static float p_gain;static float d_gain;static float p1_gain;static float d1_gain;static float p0_gain;static float d0_gain;static float tp0,td0,tp1,td1,tp,td;static float d_step=0.0001;static float p_step;static float cor_bat;static float p_cmd,d_cmd;//next_cmd;static float sample_conv = 1.0/1024.0/(float)HIST*5.0;//=3.052exp(-4) static float sample_conv1 = 1.0/1024.0/(float)HIST*2.07;static float sample_conv2 = 1.0/1024.0/(float)HIST*1.9;static float sample_conv3 = 1.0/1024.0/(float)HIST*333.3*2.5;interrupt void T2_AD_isr(void);void InitEv(void);void InitAdc(void);void InitGpio(void);struct gyro_filter {float angle;float ay_bias;//float ax_bias;float rate_bias;float steer_knob_bias;float rate;float steer_knob;float curr_bias;float curr;Uint16 inited;};void check_mode (){if((!pitch_rate) && (!in_steer_knob)) mode = 0x00;else mode = 0x01; }void delay (void){ Uint16 xt;static Uint16 k_temp;for (xt=0;xt<15;xt++){k_temp++; }}void delay1(void){ Uint16 xt;static Uint16 k_temp;for (xt=0;xt<15000;xt++){k_temp++; }}int adc_collect_samples(Uint16 *samples, Uint16 *lasts0){unsigned int i,j;if (*lasts0 == totsamples_0) {return 0;//if no new adc samples, return with 0}*lasts0 = totsamples_0;for (i=0; i<6; i++) {Uint16 tot=0;for (j=0; j<HIST; j++){tot+=adc_current_samples[i][j];}samples[i] = tot;//samples[i]=Σadc_current_samples[i][j](j=1,2,...,ADC_HIST) }return 1;}Uint16 bat_judge( float x){if(x>4.30) return 0x5;//25.8else if(x>4.18) return 0x4;//25.1else if(x>4.07) return 0x3;//24.4else if(x>3.95) return 0x2;//23.7else if(x>3.83) return 0x1;//23.0else return 0x0;}void lpf_update(float *state, float tc, float interval, float input){float frac=interval/tc;if (frac>1.0) frac=1.0;*state = input*frac + *state * (1.0-frac);}float fmax(float a, float b){if (a>=b) {return a;} else {return b;}}float fmin(float a, float b){if (a<=b) {return a;} else {return b;}}float flim(float x, float lo, float hi){if (x>hi) return hi;if (x<lo) return lo;return x;}void gyro_init(struct gyro_filter *it){ it->inited =0x00;it->angle=0.0;it->ay_bias=0.0;// it->ax_bias=0.0;it->rate_bias=0.0;it->steer_knob_bias=0.0;it->rate=0.0;it->steer_knob=0.0;it->curr_bias=0.0;it->curr=0.0;lpf_angle=0.0;lpf_angrate=0.0;lpf_steer_knob=0.0;}void gyro_sample(struct gyro_filter *it, float in_rate,float in_y, float in_x, float in_steer,float curri,float interval){flim( in_y, -1.0,1.0);flim( in_x, -1.0,1.0);if (!it->inited) {it->angle=0.0;it->rate_bias = in_rate;it->ay_bias = in_y;it->curr_bias = curri;it->steer_knob_bias = in_steer;it->inited=1;return; }it->rate = in_rate - it->rate_bias;it->angle =in_y-it->ay_bias;//it->angle+(in_y-it->ay_bias)*last_ax-in_x*(last_ay-it->ay_bias); it->curr= curri-it->curr_bias;it->steer_knob= flim( in_steer-it->steer_knob_bias,-5.0,5.0);}void set_motor_idle(){GpioDataRegs.GPFDA T.bit.GPIOF12=1; //SD highGpioDataRegs.GPFDA T.bit.GPIOF13=1; //OE highstand =0x00;EvaRegs.T2CON.all =0x1440;gyro_init(&pitch_filter);return;}void set_motor(float level_left, float level_right){int leveli_left,leveli_right;if (fabs(level_left)<0.002 || fabs(level_right)<0.002) return;leveli_left = (int16)(level_left*OCRMAX);leveli_right = (int16)(level_right*OCRMAX);if (leveli_left<-(OCRMAX-2)) leveli_left=-(OCRMAX-2);if (leveli_left>OCRMAX-2) leveli_left=OCRMAX-2;if (leveli_right<-(OCRMAX-2)) leveli_right=-(OCRMAX-2);if (leveli_right>OCRMAX-2) leveli_right=OCRMAX-2;if(still) {GpioDataRegs.GPFDA T.bit.GPIOF12=0; // SD=0// asm("nop");while(GpioDataRegs.GPFDA T.bit.GPIOF12==1) continue;GpioDataRegs.GPADA T.bit.GPIOA13=1; //PORTC |= 0x50; //enable CCW0GpioDataRegs.GPBDA T.bit.GPIOB13=1;// delay();GpioDataRegs.GPADA T.bit.GPIOA14=1; //PORTC |= 0xa0; //disable CW0 GpioDataRegs.GPBDA T.bit.GPIOB14=1;//asm("nop");continue;still=0x00;}if (leveli_left<0) {EvaRegs.ACTRA.all=0x00C2;pwm=-leveli_left;}else {EvaRegs.ACTRA.all=0x002C;pwm=leveli_left;}if (leveli_right<0) {EvbRegs.ACTRB.all=0x002C;pwm=-leveli_right;}else {EvbRegs.ACTRB.all=0x00C2;pwm=leveli_right;}}void motor_ready(void){gyro_init(&pitch_filter);GpioDataRegs.GPADA T.bit.GPIOA13=1; //PORTC |= 0x50; //enable CCW0 GpioDataRegs.GPBDA T.bit.GPIOB13=1;GpioDataRegs.GPADA T.bit.GPIOA14=1; //PORTC |= 0xa0; //disable CW0 GpioDataRegs.GPBDA T.bit.GPIOB14=1;GpioDataRegs.GPFDA T.bit.GPIOF12=0; //SD lowGpioDataRegs.GPFDA T.bit.GPIOF13=0; //OE lowEvaRegs.T1CON.bit.TENABLE=1; //启动定时器T1EvbRegs.T3CON.bit.TENABLE=1; //启动定时器T3EvaRegs.T2CON.all =0x1440; // 16分频,使能定时器操作，连续增模式}void balance(void){ float steer_cmd_abs,temp_steer_cmd;unsigned int T2cnt;if (!adc_collect_samples(samples, &last_balance_s0))return;{T2cnt=EvaRegs.T2CNT; //读取定时器2的值ticks = OCRMAX * count + T2cnt;interval = timer0_seconds_conv * (float)ticks;}count=0;EvaRegs.T2CON.all = 0x1400; //关闭定时器2EvaRegs.T2CNT = 0x0000;EvaRegs.T2CON.all = 0x1440; //启动定时器2EvaRegs.GPTCONA.bit.T1TOADC = 2;ay = sample_conv1*samples[4]-sample_conv2*samples[0];//1.034pitch_rate = samples[1]*sample_conv3;in_steer_knob = (samples[2]+samples[3])*sample_conv;bus_current = samples[4]*sample_conv;batt_voltage1 = samples[5]*sample_conv;check_mode();if(!pitch_filter.inited){if(batt_voltage1<3.0) {set_motor_idle();return;}elsecor_bat=-0.857*batt_voltage1+4.58;//-0.857=(1-1.3)/(4.18-3.83),4.58=1+0.857*4.18 cor_bat=flim(cor_bat,1.0,1.3);}gyro_sample(&pitch_filter, pitch_rate,ay,ax,in_steer_knob,bus_current, interval); EvaRegs.GPTCONA.bit.T1TOADC = 0;if(!mode) {if(fabs(pitch_filter.angle)<initial_angle) {GpioDataRegs.GPADA T.bit.GPIOA14=0;GpioDataRegs.GPBDA T.bit.GPIOB14=0;GpioDataRegs.GPADA T.bit.GPIOA13=0;GpioDataRegs.GPBDA T.bit.GPIOB13=0;GpioDataRegs.GPFDA T.bit.GPIOF12=1;still=0x01;return; }EvaRegs.GPTCONA.bit.T1TOADC = 2;lpf_steer_knob = -pitch_filter.steer_knob;if(stand) {p_gain = fmax(p0_gain,p_gain-p_step);//0.1d_gain = fmin(d0_gain,d_gain+d_step);tp=tp0;td=td0; }else { p_gain = fmin(p1_gain,p_gain+p_step);d_gain = fmax(d1_gain,d_gain-d_step);//0.1tp=tp1;td=td1;}lpf_update(&lpf_angle,tp, interval, -pitch_filter.angle*cor_bat);//0.6lpf_update(&lpf_angrate,td, interval, -pitch_filter.rate*cor_bat);//0.5p_cmd=p_gain*lpf_angle*fabs(lpf_angle);d_cmd=d_gain*lpf_angrate;cmd=p_cmd + d_cmd;cmd=flim(cmd, -hard_speed_lim, hard_speed_lim);if(lpf_steer_knob>0.0) steer_cmd = flim(left_steer_cof * lpf_steer_knob*(1-fabs(cmd)), -steer_lim,steer_lim);else steer_cmd = flim(right_steer_cof * lpf_steer_knob*(1-fabs(cmd)),-steer_lim,steer_lim);steer_cmd_abs = fabs(steer_cmd);if ((lpf_steer_knob>0.0 && cmd>0.01)||(lpf_steer_knob<0.0 && cmd<-0.01))temp_steer_cmd = flim(steer_cmd+0.1*interval,-steer_cmd_abs,steer_cmd_abs); //?????????? else if ((lpf_steer_knob>0.0 && cmd<-0.01)||(lpf_steer_knob<0.0 && cmd>0.01))temp_steer_cmd = flim(steer_cmd-0.1*interval,-steer_cmd_abs,steer_cmd_abs); //?????????? else temp_steer_cmd=0.0;EvaRegs.GPTCONA.bit.T1TOADC = 0;if (cmd>=0.0) {left_motor_pwm = 1.0-cmd*left_cof - temp_steer_cmd;right_motor_pwm =1.0-cmd*right_cof + temp_steer_cmd;} else {left_motor_pwm = -(1.0+cmd*left_cof) + temp_steer_cmd;right_motor_pwm =-(1.0+cmd*right_cof)- temp_steer_cmd;}set_motor(left_motor_pwm, right_motor_pwm);}}void stabilize_analog(void){Uint16 samples[6];Uint16 i;Uint16 last_s0=0;for (i=0; i<HIST; ) {if (adc_collect_samples(samples, &last_s0)) i++;}}void stand_test(){if(GpioDataRegs.GPBDA T.bit.GPIOB6&&GpioDataRegs.GPBDA T.bit.GPIOB7) {stand=0x02;return;}else{if(GpioDataRegs.GPBDA T.bit.GPIOB6||GpioDataRegs.GPBDA T.bit.GPIOB7){stand=0x01;return;}else{stand=0x00;return;}}}void main(void){InitSysCtrl();EALLOW;DINT;IER = 0x0000; //禁止VPU中断并清除所有CPU中断标志位IFR = 0x0000;InitPieCtrl();InitPieVectTable();InitGpio();InitAdc();InitEv();EALLOW; // This is needed to write to EALLOW protected registersPieVectTable.T2PINT=&T2_AD_isr;EDIS; // This is needed to disable write to EALLOW protected registersPieCtrlRegs.PIEIER3.bit.INTx1=1;//T2pint中断IER |= M_INT1;//开启中断1PieCtrlRegs.PIEIER1.bit.INTx7=1;EINT; // Enable Global interrupt INTMERTM; // Enable Global realtime interrupt DBGMfor(;;){DisableDog();GpioDataRegs.GPADA T.bit.GPIOA12=1;GpioDataRegs.GPADA T.bit.GPIOA15=1;GpioDataRegs.GPBDA T.bit.GPIOB12=1;GpioDataRegs.GPBDA T.bit.GPIOB15=1;GpioDataRegs.GPFDA T.bit.GPIOF12=0; //SD low GpioDataRegs.GPFDA T.bit.GPIOF13=0; //OE lowGpioDataRegs.GPADA T.bit.GPIOA13=0;GpioDataRegs.GPADA T.bit.GPIOA14=0;GpioDataRegs.GPBDA T.bit.GPIOB13=0;GpioDataRegs.GPBDA T.bit.GPIOB14=0;InitAdc();stabilize_analog();InitEv();gyro_init(&pitch_filter);motor_ready();for(;;){balance();if(!stand) break;}set_motor_idle();}}interrupt void T2_AD_isr(void){if(k<16){adc_current_samples[0][k]=((float)AdcRegs.ADCRESULT0)*3.0/65520.0+adclo;adc_current_samples[1][k]=((float)AdcRegs.ADCRESULT1)*3.0/65520.0+adclo;adc_current_samples[2][k]=((float)AdcRegs.ADCRESULT2)*3.0/65520.0+adclo;adc_current_samples[3][k]=((float)AdcRegs.ADCRESULT3)*3.0/65520.0+adclo;adc_current_samples[4][k]=((float)AdcRegs.ADCRESULT4)*3.0/65520.0+adclo;adc_current_samples[5][k]=((float)AdcRegs.ADCRESULT5)*3.0/65520.0+adclo;k++;AdcRegs.ADCST.bit.INT_SEQ1_CLR=1; //清除状态字AdcRegs.ADCTRL2.bit.RST_SEQ1=1;//复位seq1EvaRegs.EV AIFRB.bit.T2PINT=1;//清除中断标志EvaRegs.EV AIMRB.bit.T2PINT=1;//中断允许PieCtrlRegs.PIEACK.bit.ACK3=1;//向cpu申请中断}else{totsamples_0++;k=0;count=totsamples_0;AdcRegs.ADCST.bit.INT_SEQ1_CLR=1; //清除状态字AdcRegs.ADCTRL2.bit.RST_SEQ1=1;//复位seq1EvaRegs.EV AIFRB.bit.T2PINT=1;//清除中断标志// EvaRegs.EV AIMRB.bit.T2PINT=1;//中断允许EvaRegs.GPTCONA.bit.T1TOADC = 0;}}//--------------------------------------------------------------------------- // InitEv://--------------------------------------------------------------------------- // This function initializes to a known state.//void InitEv(void){EALLOW;/*****************************//***********配置ACTR**********//*****************************//* EvaRegs.ACTRA.bit.CMP1ACT=2;//PW1引脚高电平有效EvaRegs.ACTRA.bit.CMP2ACT=1;//PWM2引脚低电平有效*//*****************************//*******配置死区寄存器********//*****************************/EvaRegs.DBTCONA.bit.DBT=5;//死区定时器周期为5EvaRegs.DBTCONA.bit.EDBT1=1;//死区定时器1使能EvaRegs.DBTCONA.bit.DBTPS=1;//死区定时器预定标因子，死区时钟为HSPCLK/2/*****************************//*********配置COMCONA*********//*****************************/CONA.bit.CENABLE=1;//使能比较单元的比较操作CONA.bit.CLD=2;//当给CMPR1赋新值时，立即装载CONA.bit.FCOMPOE=1;//全比较操作，PWM输出由相应的比较逻辑驱动/*****************************//**********设置T1CON**********//*****************************/EvaRegs.T1CON.bit.TMODE=2;//T1工作于连续增模式EvaRegs.T1CON.bit.TPS=3;//输入时钟预定标因子为X/8EvaRegs.T1CON.bit.TENABLE=1;//禁止定时器操作EvaRegs.T1CON.bit.TCLKS10=0;//使用内部时钟EvaRegs.T1CON.bit.TECMPR=1;//使能定时器比较操作/*****************************//**********设置T1PR等*********//*****************************/EvaRegs.T1PR =OCRMAX;EvaRegs.T1CNT = 0x0; ///定时器1初值设为0EvaRegs.CMPR1=pwm;EvaRegs.CMPR2=pwm;// EvaRegs.T1CON.bit.TENABLE=1; //启动定时器T1/* EvbRegs.ACTRB.bit.CMP7ACT=2;//PWM7引脚高电平有效EvbRegs.ACTRB.bit.CMP8ACT=1;//PWM8引脚低电平有效*//*****************************//*******配置死区寄存器********//*****************************/EvbRegs.DBTCONB.bit.DBT=5;//死区定时器周期为5EvbRegs.DBTCONB.bit.EDBT1=1;//死区定时器1使能EvbRegs.DBTCONB.bit.DBTPS=3;//死区定时器预定标因子，死区时钟为HSPCLK/8/*****************************//*********配置COMCONA*********//*****************************/CONB.bit.CENABLE=1;//使能比较单元的比较操作CONB.bit.CLD=2;//当给CMPR1赋新值时，立即装载CONB.bit.FCOMPOE=1;//全比较操作，PWM输出由相应的比较逻辑驱动/*****************************//**********设置T1CON**********//*****************************/EvbRegs.T3CON.bit.TMODE=2;//T1工作于连续增模式EvbRegs.T3CON.bit.TPS=3;//输入时钟预定标因子为X/8EvbRegs.T3CON.bit.TENABLE=1;//禁止定时器操作EvbRegs.T3CON.bit.TCLKS10=0;//使用内部时钟EvbRegs.T3CON.bit.TECMPR=1;//使能定时器比较操作/*****************************//**********设置T1PR等*********//*****************************/EvbRegs.T3PR =OCRMAX;EvbRegs.T3CNT = 0; ///定时器3初值设为0EvbRegs.CMPR4=pwm;EvbRegs.CMPR5=pwm;// EvbRegs.T3CON.bit.TENABLE=1;//启动定时器T3EvaRegs.GPTCONA.all=0;EvaRegs.T2PR =OCRMAX; // 定时器2的周期为20kEvaRegs.T2CMPR=0x0000;EvaRegs.T2CNT =0x0000; // Timer2 counter//EvaRegs.T2CON.all =0x1440;//16分频,使能定时器操作，连续增模式EvaRegs.EV AIMRB.bit.T2PINT = 1;//定时器2周期中断允许EvaRegs.EV AIFRB.bit.T2PINT = 1;//清除标志// EvaRegs.GPTCONA.bit.T1TOADC = 2;EDIS;}void InitAdc(void){//unsigned int p;// To powerup the ADC the ADCENCLK bit should be set first to enable// clocks, followed by powering up the bandgap and reference circuitry.// After a 5ms delay the rest of the ADC can be powered up. After ADC// powerup, another 20us delay is required before performing the first// ADC conversion. Please note that for the delay function below to// operate correctly the CPU_CLOCK_SPEED define statement in the// DSP28_Examples.h file must contain the correct CPU clock period in// nanoseconds. For example:AdcRegs.ADCTRL1.bit.RESET=1; //ADC模块软件复位// asm(" NOP");AdcRegs.ADCTRL1.bit.RESET=0; //这一句可要可不要，因为硬件会自动返回0AdcRegs.ADCTRL1.bit.SUSMOD=3; //仿真挂起的模式3 ？？AdcRegs.ADCTRL1.bit.ACQ_PS=0x0; //决定启动转换新号SOC的脉冲宽度，SOC用于控制持续多长时间的采样开关闭合，SOC脉冲宽度大小为ADCLK周期的（ADCTRL(11:8)+1)倍AdcRegs.ADCTRL1.bit.CPS=0; //对外设时钟HSPCLK进行分频，1分频AdcRegs.ADCTRL1.bit.CONT_RUN=1;//连续运转模式AdcRegs.ADCTRL1.bit.SEQ_CASC=1;//级联模式，SEQ1和SEQ2工作时作为一个16位状态排序器（SEQ）//ADC模块控制寄存器3（ADCTRL3）AdcRegs.ADCTRL3.bit.ADCBGRFDN=3;//ADC带间隙和参考电路上电// for(p=0;p<10000;p++) asm(" NOP"); //延时等待ADC上电AdcRegs.ADCTRL3.bit.ADCPWDN=1; //ADC除间隙和参考电路之外的模拟电路上电// for(p=0;p<5000;p++) asm(" NOP"); //延时等待ADC上电AdcRegs.ADCTRL3.bit.ADCCLKPS=5; //内核时钟分频，产生ADC内核时钟ADCLK；ADCLK=HSPCLK/[30*(ADCTRL1.7+1)];ADCTRL1.7为AdcRegs.ADCTRL1.bit.CPS=0。
MiniBalanceV3.5【大功率版】程序结构和数据融合、控制算法说明
MiniBalanceV3.5【大功率版】程序结构和数据融合、控制算法说明认真读完整篇文档有利于您更好的理解整个平衡小车程序。
开发平台:MDK5.11、我们的代码使用MPU6050的INT的引脚每5ms触发的中断作为控制的时间基准，严格保证系统的时序！2.根据不同阶层的学习者，我们提供了复杂程度不同的代码：1.针对普通用户，提供了以下三个代码：MiniBalanceV3.5平衡小车源码（DMP版）MiniBalanceV3.5平衡小车源码（互补滤波版）MiniBalanceV3.5平衡小车源码（卡尔曼滤波版）以上代码除了使用DMP、卡尔曼滤波、互补滤波分别获取姿态角外，还提供了超声波避障代码。
2.针对入门用户，提供以下代码：MiniBalanceV3.5平衡小车源码（精简入门版）去除所有附加的代码，使用最少的代码量实现小车直立。
3.针对大神用户，提供以下代码：MiniBalanceV3.5平衡小车源码（顶配版含无线模块和线性CCD）普通版的基础上增加无线模块的驱动和线性CCD的采集代码，大家可以拓展体感控制和小车巡线。
3.整个程序应用了STM32大量的资源：ADC模块：采集电阻分压后的电池电压,采集模拟CCD摄像头数据TIM1：初始化为PWM输出，CH1，CH4输出双路10KHZ的PWM控制电机TIM2：初始化为正交编码器模式，硬件采集编码器1数据TIM3：CH3初始化为超声波的回波采集接口。
TIM4：初始化为正交编码器模式，硬件采集编码器2数据USART1：通过串口1把数据发到串口调试助手USART3：通过串口3接收蓝牙遥控的数据，接收方式为中断接收。
并发送数据给app。
IIC：利用IO模拟IIC去读取MPU6050的数据，原理图上MPU6050链接的是STM32的硬件IIC接口，但是因为STM32硬件IIC不稳定，所以默认使用模拟IIC，大家可以自行拓展。
SPI：利用IO模拟SPI去驱动OLED显示屏,硬件SPI驱动NRF24L01 GPIO：读取按键输入，控制LED，控制电机使能和正反转SWD：提供用于在线调试的SWD接口EXTI:由MPU6050的INT引脚每5ms触发一次中断，作为控制的时间基准4.程序主要用户文件说明如下：●Source Group1◆Startup_stm32f10x_md.s:stm32的启动文件●User◆Minibalance.c：放置主函数，并把超声波读取和人机交互等工作放在死循环里面。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
#include <PID_v1.h>#include "Wire.h"#include "I2Cdev.h"#include "MPU6050_6Axis_MotionApps20.h"MPU6050 mpu(0x68);#define center 0x7Fchar flag=0;char num=0;double time;signed int speeds = 0;signed int oldspeed =0;byte inByte ;// MPU control/status varsbool dmpReady = false;uint8_t mpuIntStatus;uint8_t devStatus;uint16_t packetSize;uint16_t fifoCount;uint8_t fifoBuffer[64];signed int speedcount=0;// orientation/motion varsQuaternion q; // [w, x, y, z] quaternion containerVectorFloat gravity; // [x, y, z] gravity vectorfloat ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector float angle;double Setpoint, Input, Output;double kp = 18.8,ki = 185.0,kd = 0.29;//需要你修改的参数double Setpoints, Inputs, Outputs;double sp = 0.8,si = 0,sd = 0.22;//需要你修改的参数unsigned char dl=17,count;union{signed int all;unsigned char s[2];}data;volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone highvoid dmpDataReady() {mpuInterrupt = true;}PID myPID(&Input, &Output, &Setpoint,kp,ki,kd, DIRECT);PID sPID(&Inputs, &Outputs, &Setpoints,sp,si,sd, DIRECT);void motor(int v){if(v>0){v+=dl;digitalWrite(6,0);digitalWrite(7,1);digitalWrite(8,1);digitalWrite(9,0);analogWrite(10,v);analogWrite(11,v);}else if(v<0){v=-v;v+=dl;digitalWrite(6,1);digitalWrite(7,0);digitalWrite(8,0);digitalWrite(9,1);analogWrite(10,v);analogWrite(11,v);}else{analogWrite(10,0);analogWrite(11,0);}}void motort(int v){if(v>0){v+=dl;digitalWrite(8,1);digitalWrite(9,0);analogWrite(10,v);}else if(v<0){v=-v;v+=dl;digitalWrite(8,0);digitalWrite(9,1);analogWrite(10,v);}else{analogWrite(10,0);}}void setup(){pinMode(6,OUTPUT);pinMode(7,OUTPUT);pinMode(8,OUTPUT);pinMode(9,OUTPUT);pinMode(10,OUTPUT);pinMode(11,OUTPUT);digitalWrite(6,0);digitalWrite(7,1);digitalWrite(8,1);digitalWrite(9,0);analogWrite(10,0);analogWrite(11,0);Serial.begin(115200);Wire.begin();delay(100);Serial.println("Initializing I2C devices...");mpu.initialize();Serial.println("Testing device connections...");Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");delay(2);Serial.println("Initializing DMP...");devStatus = mpu.dmpInitialize();if (devStatus == 0)Serial.println("Enabling DMP...");mpu.setDMPEnabled(true);Serial.println("Enabling interrupt detection (Arduino external interrupt 0)...");attachInterrupt(0, dmpDataReady, RISING);mpuIntStatus = mpu.getIntStatus();Serial.println("DMP ready! Waiting for first interrupt...");dmpReady = true;packetSize = mpu.dmpGetFIFOPacketSize();}else{Serial.print("DMP Initialization failed (code ");Serial.print(devStatus);Serial.println(")");}Setpoint = 22.0;myPID.SetTunings(kp,ki,kd);myPID.SetOutputLimits(-255+dl,255-dl);myPID.SetSampleTime(5);myPID.SetMode(AUTOMATIC);sPID.SetTunings(sp,si,sd);sPID.SetOutputLimits(-10,70);sPID.SetSampleTime(200);sPID.SetMode(AUTOMATIC);attachInterrupt(1,speed,RISING);}void loop(){if (!dmpReady)return;// wait for MPU interrupt or extra packet(s) availableif (!mpuInterrupt && fifoCount < packetSize)return;mpuInterrupt = false;mpuIntStatus = mpu.getIntStatus();fifoCount = mpu.getFIFOCount();if ((mpuIntStatus & 0x10) || fifoCount == 1024){mpu.resetFIFO();else if (mpuIntStatus & 0x02){while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();mpu.getFIFOBytes(fifoBuffer, packetSize);fifoCount -= packetSize;mpu.dmpGetQuaternion(&q, fifoBuffer);mpu.dmpGetGravity(&gravity, &q);mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //从DMP中取出Yaw、Pitch、Roll三个轴的角度，放入数组ypr。
单位：弧度angle=-ypr[1] * 180/M_PI;}Inputs = speedcount;pute();Setpoint = 22.0+Outputs;Input = angle;pute();if(angle>60||angle<0)Output=0;if(flag){motort(Output);flag=0;}else {motor(Output);}if (Serial.available() > 0) {inByte = Serial.read();}if(inByte == 'w'){kp+=0.5;}else if(inByte == 'q'){kp-=0.5;}else if(inByte == 'r'){ki+=10;}else if(inByte == 'e'){ki-=10;}else if(inByte == 'y'){kd+=0.01;}else if(inByte == 't'){kd-=0.01;}else if(inByte == 'i'){dl+=1;}else if(inByte == 'u'){dl-=1;}else if(inByte == 's'){sp+=0.1;}else if(inByte == 'a'){sp-=0.1;}else if(inByte == 'f'){si+=1;}else if(inByte == 'd'){si-=1;}else if(inByte == 'h'){sd+=0.01;}else if(inByte == 'g'){sd-=0.01;}else if(inByte == 'v'){ Setpoints+=2;}else if(inByte == 'b'){ Setpoints-=2;}else if(inByte == 'n'){ Setpoints+=2;flag=1;}else if(inByte == 'm'){ Setpoints-=2;flag=1;}inByte='x';sPID.SetTunings(sp,si,sd); myPID.SetTunings(kp,ki,kd);num++;if(num==20){num=0;Serial.print(kp);Serial.print(',');Serial.print(ki);Serial.print(',');Serial.print(kd);Serial.print(','); Serial.print(dl); Serial.print(" "); Serial.print(sp); Serial.print(','); Serial.print(si); Serial.print(','); Serial.print(sd);Serial.print(','); Serial.println(angle);}}void speed(){if(digitalRead(6)){speedcount+=1;}elsespeedcount-=1; }。