|
两轮自平衡小车全部的制作资料下载:
自平衡小车程序.7z
(305.54 KB, 下载次数: 254)
调试记录:
2016.12.2
完成基本角度闭环程序,基本调节出角度的PD,
富民发现直立可能需要位置闭环+角度闭环+速度闭环。
2016.12.3 pm 13:00
完成基本速度闭环和位置闭环程序,基本可以按照设定方向前进,
只是中途的PID参数没有调节好有停顿现象,开始蓝牙控制模式。
2016.12.3 pm 14:43
完成基本的前进 后退蓝牙控制功能,就是有些时候出现过冲停顿。
下一步进行转弯控制程序的抒写。
2016.12.3 pm 16:55
完成基本控制功能,前进、后退、原地左转、原地右转、使用简单
的蓝牙指令控制,0x01前进、0x02后退、 0x03原地左转、 0x04原
地右转、0x00什么保持直立状态,下一步进行直立调节,优化静止
时的状态。
2016.12.5 pm 13:50
加入了手机APP蓝牙控制 出现问题 抖动过大多次控制容易出现死机
现象,需要继续直立PID调节参数。
2016.12.22 pm 17:49
加入了 模糊控制 参数调节的还算可以,只调节了直立参数 速度闭环
和位置闭环参数还没有调节等待明天调节。
2016.12.31 am 11:13
所有功能都已完毕。直立、壁障、循迹、蓝牙遥控、由于循迹采用双线
导致有些时候不稳定、不知道是机械愿意还是软件原因导致直立静止时
有些抖动。基本可以完成所有多需任务。
这个是去年做的一个两轮自平衡循迹小车,小车采用两片STM32,一块负责图像处理,另一块负责对电机的控制。两块板子之间使用串口通信传输数据。
做这个车子学会了很多,最初两块板子之间串口通信有问题,程序会跑着跑着就跑飞,很是恼火。之后每次需要串口通信才打开中断,解决了跑飞的问题。
以下是车子的图片,和源码。
主程序下面是部分程序预览(完整版本请下载本帖附件):
- #include "System_init.h"
- /*特别注意:flag变量 不可以更改 否则无法直立*/
- extern u8 flag;
- /**********主函数**********/
- int main(void)
- {
- system_init(); //整体系统初始化函数
- temp_data11 = 80;
- for(;;)
- {
- /*
- 函数说明:
- upright_Adjust函数。
- 功能:直立控制。
- 参数个数:3个。
- 1)P参数。
- 2)I参数。
- 3)D参数。
- 此函数是用于直立的函数P是PID中的比例参数
- D是PID的微分参数、直立不用I积分参数。
- */
- /*
- 函数说明:
- speed_control函数。
- 功能:蓝牙控制函数。
- 参数个数:3个。
- 1)串口采集参数(不需要更改、放着就可以)
- 2)速度控制参数(可以更改最大40最小10)
- 3)转角速度控制参数(可修改,最大40最小10)
- */
- SpeedL(3500);
- SpeedR(3500);
- if(1 == flag)
- {
-
- //upright_Adjust(400.0,0.0,25.0); //9.5
- /*特别注意:flag变量 不可以更改 否则无法直立*/
- flag = 0;
- }
- }
- }
复制代码 以stm32f103rct6为例
测试串口通信数据
MPU数据进行处理
检测X轴加速度,Y轴角速度
参考网上例程
mpu6050.c文件:
- #include "MPU6050.h"
- #include "System_init.h"
- void I2C_Congiguration(void)
- {
- GPIO_InitTypeDef GPIO_InitStructure;
- I2C_InitTypeDef I2C_InitStructure;
- /* 使能?I22C1 有关的时钟 */
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
- RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2,ENABLE);
-
- /* PB6-I2C2_SCL、PB7-I2C2_SDA*/
- GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD; // 开漏输出
- GPIO_Init(GPIOB, &GPIO_InitStructure);
-
- /* I2C 配置 */
- I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
- I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
- I2C_InitStructure.I2C_OwnAddress1 = I2C2_MPU6050;
- I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
- I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
- I2C_InitStructure.I2C_ClockSpeed = I2C_Speed;
- /* 使I2C1 */
- I2C_Cmd(I2C2, ENABLE);
- /* I2C2 初始化 */
- I2C_Init(I2C2, &I2C_InitStructure);
- /*允许1字节1应答模式*/
- I2C_AcknowledgeConfig(I2C2, ENABLE);
-
-
- }
- void MPU6050_Init(void)
- {
- delay_ms(100);
- I2C_WriteByte(PWR_MGMT_1,0x00);
- delay_ms(10);
- I2C_WriteByte(SMPLRT_DIV,0x07);
- delay_ms(10);
- I2C_WriteByte(CONFIG,0x06);
- delay_ms(10);
- I2C_WriteByte(GYRO_CONFIG,0x18);
- delay_ms(10);
- I2C_WriteByte(ACCEL_CONFIG,0x01);
- delay_ms(1000);
- }
- void I2C_WriteByte(uint8_t REG_Address, uint8_t Write_Data)
- {
- while(I2C_GetFlagStatus(I2C2, I2C_FLAG_BUSY)); // Added by Najoua 27/08/2008
- /* Send STRAT condition */
- I2C_GenerateSTART(I2C2, ENABLE);
- /* Test on EV5 and clear it */
- while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT));
- /* Send EEPROM address for write */
- I2C_Send7bitAddress(I2C2, I2C2_MPU6050, I2C_Direction_Transmitter);
-
- /* Test on EV6 and clear it */
- while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));
-
- /* Send the EEPROM's internal address to write to */
- I2C_SendData(I2C2, REG_Address);
-
- /* Test on EV8 and clear it */
- while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
- /* Send the byte to be written */
- I2C_SendData(I2C2, Write_Data);
-
- /* Test on EV8 and clear it */
- while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
-
- /* Send STOP condition */
- I2C_GenerateSTOP(I2C2, ENABLE);
- I2C_WaitEepromStandbyState();
- }
- uint8_t I2C_ReadByte(uint8_t REG_Address)
- {
- uint8_t data_byte;
- //*((u8 *)0x4001080c) |=0x80;
- while(I2C_GetFlagStatus(I2C2, I2C_FLAG_BUSY)); // Added by Najoua 27/08/2008
-
-
- /* Send START condition */
- I2C_GenerateSTART(I2C2, ENABLE);
- //*((u8 *)0x4001080c) &=~0x80;
-
- /* Test on EV5 and clear it */
- while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT));
- /* Send EEPROM address for write */
- I2C_Send7bitAddress(I2C2, I2C2_MPU6050, I2C_Direction_Transmitter);
- /* Test on EV6 and clear it */
- while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));
-
- /* Clear EV6 by setting again the PE bit */
- I2C_Cmd(I2C2, ENABLE);
- /* Send the EEPROM's internal address to write to */
- I2C_SendData(I2C2, REG_Address);
- /* Test on EV8 and clear it */
- while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
-
- /* Send STRAT condition a second time */
- I2C_GenerateSTART(I2C2, ENABLE);
-
- /* Test on EV5 and clear it */
- while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT));
-
- /* Send EEPROM address for read */
- I2C_Send7bitAddress(I2C2, I2C2_MPU6050, I2C_Direction_Receiver);
-
- /* Test on EV6 and clear it */
- while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));
-
- /* While there is data to be read */
- /* Disable Acknowledgement */
- I2C_AcknowledgeConfig(I2C2, DISABLE);
-
- /* Send STOP Condition */
- I2C_GenerateSTOP(I2C2, ENABLE);
- /* Test on EV7 and clear it */
- if(I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_RECEIVED))
- /* Read a byte from the EEPROM */
- data_byte = I2C_ReceiveData(I2C2);
- /* Enable Acknowledgement to be ready for another reception */
- I2C_AcknowledgeConfig(I2C2, ENABLE);
- //I2C_EE_WaitEepromStandbyState();
- return data_byte;
- }
- void I2C_ReadBuffer(uint8_t* Data_Buffer, uint8_t REG_Address, uint8_t Num_Byte)
- {
- //*((u8 *)0x4001080c) |=0x80;
- while(I2C_GetFlagStatus(I2C2, I2C_FLAG_BUSY)); // Added by Najoua 27/08/2008
-
-
- /* Send START condition */
- I2C_GenerateSTART(I2C2, ENABLE);
- //*((u8 *)0x4001080c) &=~0x80;
-
- /* Test on EV5 and clear it */
- while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT));
- /* Send EEPROM address for write */
- I2C_Send7bitAddress(I2C2, I2C2_MPU6050, I2C_Direction_Transmitter);
- /* Test on EV6 and clear it */
- while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));
-
- /* Clear EV6 by setting again the PE bit */
- I2C_Cmd(I2C2, ENABLE);
- /* Send the EEPROM's internal address to write to */
- I2C_SendData(I2C2, REG_Address);
- /* Test on EV8 and clear it */
- while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
-
- /* Send STRAT condition a second time */
- I2C_GenerateSTART(I2C2, ENABLE);
-
- /* Test on EV5 and clear it */
- while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT));
-
- /* Send EEPROM address for read */
- I2C_Send7bitAddress(I2C2, I2C2_MPU6050, I2C_Direction_Receiver);
-
- /* Test on EV6 and clear it */
- while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));
-
- /* While there is data to be read */
- while(Num_Byte)
- {
- if(Num_Byte == 1)
- {
- /* Disable Acknowledgement */
- I2C_AcknowledgeConfig(I2C2, DISABLE);
-
- /* Send STOP Condition */
- I2C_GenerateSTOP(I2C2, ENABLE);
- }
- /* Test on EV7 and clear it */
- if(I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_RECEIVED))
- {
- /* Read a byte from the EEPROM */
- *Data_Buffer = I2C_ReceiveData(I2C2);
- /* Point to the next location where the byte read will be saved */
- Data_Buffer++;
-
- /* Decrement the read bytes counter */
- Num_Byte--;
- }
- }
- /* Enable Acknowledgement to be ready for another reception */
- I2C_AcknowledgeConfig(I2C2, ENABLE);
- //I2C_EE_WaitEepromStandbyState();
- }
- void I2C_WaitEepromStandbyState(void)
- {
- vu16 SR1_Tmp = 0;
- do
- {
- /* Send START condition */
- I2C_GenerateSTART(I2C2, ENABLE);
- /* Read I2C2 SR1 register */
- SR1_Tmp = I2C_ReadRegister(I2C2, I2C_Register_SR1);
- /* Send EEPROM address for write */
- I2C_Send7bitAddress(I2C2, I2C2_MPU6050, I2C_Direction_Transmitter);
- }while(!(I2C_ReadRegister(I2C2, I2C_Register_SR1) & 0x0002));
-
- /* Clear AF flag */
- I2C_ClearFlag(I2C2, I2C_FLAG_AF);
- /* STOP condition */
- I2C_GenerateSTOP(I2C2, ENABLE); // Added by Najoua 27/08/2008
- }
复制代码 pazzy.c文件:
- #include "pazzy.h"
- #include "System_init.h"
- #define FMAX 100 /*语言值的满幅值*/
- //角度值
- int PFF[4]={0,8,16,24};
- /*输入量D语言值特征点*/
- // 角速度
- int DFF[4]={0,16,45,80};
- /*输出量U语言值特征点*/
- int UFF[7]={0,12,30};
- /*采用了调整因子的规则表,大误差时偏重误差,小误差时偏重误差变化*/
- /*a0=0.3,a1=0.55,a2=0.74,a3=0.89 */
- int rule[7][7]={
- //误差变化率 -3,-2,-1, 0, 1, 2, 3 // 误差
- { 2, 2, 2, 2, 2, 1, 1,}, // -3
- { 2, 1, 1, 1, 1, 1, 2,}, // -2
- { 2, 1, 0, 0, 0, 0, 0,}, // -1
- { 2, 2, 2, 2, 2, 2, 2,}, // 0
- { 0, 0, 0, 0, 0, 1, 2,}, // 1
- { 0, 1, 1, 1, 1, 1, 2,}, // 2
- { 1, 1, 2, 2, 2, 2, 2}}; // 3 // 3
- /**********************************************************/
- float Fuzzy(float P,float D) /*模糊运算引擎*/
- {
- float U; /*偏差,偏差微分以及输出值的精确量*/
- float PF[2],DF[2],UF[4]; /*偏差,偏差微分以及输出值的隶属度*/
- int Pn,Dn,Un[4];
- double temp1,temp2;
- /*隶属度的确定*/
- /*根据PD的指定语言值获得有效隶属度*/
- if(P>-PFF[3] && P<pff[3])
- {
- if(P<=-PFF[2])
- {
- Pn=-2;
- PF[0]=FMAX*((float)(-PFF[2]-P)/(PFF[3]-PFF[2]));
- }
- else if(P<=-PFF[1])
- {
- Pn=-1;
- PF[0]=FMAX*((float)(-PFF[1]-P)/(PFF[2]-PFF[1]));
- }
- else if(P<=PFF[0])
- {
- Pn=0;
- PF[0]=FMAX*((float)(-PFF[0]-P)/(PFF[1]-PFF[0]));
- }
- else if(P<=PFF[1])
- {
- Pn=1; PF[0]=FMAX*((float)(PFF[1]-P)/(PFF[1]-PFF[0]));
- }
- else if(P<=PFF[2])
- {
- Pn=2; PF[0]=FMAX*((float)(PFF[2]-P)/(PFF[2]-PFF[1]));
- }
- else if(P<=PFF[3])
- {
- Pn=3; PF[0]= FMAX*((float)(PFF[3]-P)/(PFF[3]-PFF[2]));
- }
- }
- else if(P<=-PFF[3])
- {
- Pn=-2; PF[0]=FMAX;
- }
- else if(P>=PFF[3])
- {
- Pn=3; PF[0]=0;
- }
- PF[1]=FMAX-PF[0];
- if(D>-DFF[3] && D<dff[3])
- {
- if(D<=-DFF[2])
- {
- Dn=-2;DF[0]=FMAX*((float)(-DFF[2]-D)/(DFF[3]-DFF[2]));
- }
- else if(D<=-DFF[1])
- {
- Dn=-1;
- DF[0]=FMAX*((float)(-DFF[1]-D)/(DFF[2]-DFF[1]));
- }
- else if(D<=DFF[0])
- {
- Dn=0;
- DF[0]=FMAX*((float)(-DFF[0]-D)/(DFF[1]-DFF[0]));
- }
- else if(D<=DFF[1])
- {
- Dn=1;
- DF[0]=FMAX*((float)(DFF[1]-D)/(DFF[1]-DFF[0]));
- }
- else if(D<=DFF[2])
- {
- Dn=2; DF[0]=FMAX*((float)(DFF[2]-D)/(DFF[2]-DFF[1]));
- }
- else if(D<=DFF[3])
- {
- Dn=3; DF[0]=FMAX*((float)(DFF[3]-D)/(DFF[3]-DFF[2]));
- }
- }
- else if(D<=-DFF[3])
- {
- Dn=-2;
- DF[0]=FMAX;
- }
- else if(D>=DFF[3])
- {
- Dn=3;
- DF[0]=0;
- }
- DF[1]=FMAX-DF[0];
- /*使用误差范围优化后的规则表rule[7][7]*/
- /*输出值使用13个隶属函数,中心值由UFF[7]指定*/
- /*一般都是四个规则有效*/
- Un[0]=rule[Pn-1+3][Dn-1+3];
- Un[1]=rule[Pn+3][Dn-1+3];
- Un[2]=rule[Pn-1+3][Dn+3];
- Un[3]=rule[Pn+3][Dn+3];
- if(PF[0]<=DF[0])
- UF[0]=PF[0];
- else
- UF[0]=DF[0];
- if(PF[1]<=DF[0])
- UF[1]=PF[1];
- else
- UF[1]=DF[0];
- if(PF[0]<=DF[1])
- UF[2]=PF[0];
- else
- UF[2]=DF[1];
- if(PF[1]<=DF[1])
- UF[3]=PF[1];
- else
- UF[3]=DF[1];
- /*同隶属函数输出语言值求大*/
- if(Un[0]==Un[1])
- {
- if(UF[0]>UF[1])
- UF[1]=0;
- else
- UF[0]=0;
- }
- if(Un[0]==Un[2])
- {
- if(UF[0]>UF[2])
- UF[2]=0;
- else
- UF[0]=0;
- }
- if(Un[0]==Un[3])
- {
- if(UF[0]>UF[3])
- UF[3]=0;
- else
- UF[0]=0;
- }
- if(Un[1]==Un[2])
- {
- if(UF[1]>UF[2])
- UF[2]=0;
- else
- UF[1]=0;
- }
- if(Un[1]==Un[3])
- {
- if(UF[1]>UF[3])
- UF[3]=0;
- else
- UF[1]=0;
- }
- if(Un[2]==Un[3])
- {
- if(UF[2]>UF[3])
- UF[3]=0;
- else
- UF[2]=0;
- }
- /*重心法反模糊*/
- /*Un[]原值为输出隶属函数标号,转换为隶属函数值*/
- if(Un[0]>=0)
- Un[0]=UFF[Un[0]];
- else
- Un[0]=-UFF[-Un[0]];
- if(Un[1]>=0)
- Un[1]=UFF[Un[1]];
- else
- Un[1]=-UFF[-Un[1]];
- if(Un[2]>=0)
- Un[2]=UFF[Un[2]];
- else
- Un[2]=-UFF[-Un[2]];
- if(Un[3]>=0)
- Un[3]=UFF[Un[3]];
- else
- Un[3]=-UFF[-Un[3]];
- temp1=UF[0]*Un[0]+UF[1]*Un[1]+UF[2]*Un[2]+UF[3]*Un[3]; //因为选定隶属函数为三角形,且对称,故可以证明中心在中间的特征点上(横坐标)。
- temp2=UF[0]+UF[1]+UF[2]+UF[3];
- U=temp1/temp2;
- return U; //最后的输出
- }
复制代码
|
评分
-
查看全部评分
|