差分英伟达机器人_底层控制系统
该层控制系统接入了加速度、IMU等信息
前言
提示:这是学习 “环宇机器人-差分轮底层视频” 时记录下的随笔,方便自己回顾的同时也给大家一些参考,如果有说的不对之处,请指正修改。
二、机器人底盘通讯代码
1.获取重力加速度、角速度原始数据
- 通过IIC读取信息
while(1)
{
delay_ms(46);
Huanyu_moto_Control_speed(Right_moto.Current_Speed, Right_moto.Target_Speed ,MOTO_RIGHT);
Huanyu_moto_Control_speed(Left_moto.Current_Speed, Left_moto.Target_Speed ,MOTO_LEFT );
MPU_Get_Accelerometer(&Send_Data.Sensor_Str.Link_Accelerometer); //通过IIC读取加速度信息
MPU_Get_Gyroscope(&Send_Data.Sensor_Str.Link_Gyroscope); //通过IIC读取角速度信息
Huanyu_SendTo_UbuntuPC(); //向树莓派透传数据
Huanyu_IWDG_Feed();
}
}
- 通过MPU-6000(6050)(整合性6轴运动处理组件)得到加速度值、角速度原始数据。
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
// 其他,错误代码
unsigned char MPU_Get_Accelerometer(Mpu6050_Str* Acce)
{
u8 buf[6],res;
res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
if(res==0)
{
Acce->X_data =((u16)buf[0]<<8)|buf[1];
Acce->Y_data =((u16)buf[2]<<8)|buf[3];
Acce->Z_data =((u16)buf[4]<<8)|buf[5];
}
return res;;
}
- IIC读取数据
//IIC连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取的长度
//buf:读取到的数据存储区
//返回值:0,正常
// 其他,错误代码
unsigned char MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
IIC_Start();
IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令
if(IIC_Wait_Ack()) //等待应答
{
IIC_Stop();
return 1;
}
IIC_Send_Byte(reg); //写寄存器地址
IIC_Wait_Ack(); //等待应答
IIC_Start();
IIC_Send_Byte((addr<<1)|1);//发送器件地址+读命令
IIC_Wait_Ack(); //等待应答
while(len)
{
if(len==1)*buf=IIC_Read_Byte(0);//读数据,发送nACK
else *buf=IIC_Read_Byte(1); //读数据,发送ACK
len--;
buf++;
}
IIC_Stop(); //产生一个停止条件
return 0;
}
2.向上位机传输底盘数据
- 协议格式:0XFEFEFEFE … 0XEE (33个字节)
- 包括以右手笛卡尔坐标系的X、Y、Z(旋转)三个方向的速度、电池电压、IMU等数据
- 将速度、电压信号、重力加速度原始数据、陀螺仪角速度原始数据等打包发送到上位机(20HZ)。
#define PROTOCOL_HEADER 0XFEFEFEFE // 协议头
#define PROTOCOL_END 0XEE // 协议尾
#define PROTOCL_DATA_SIZE 33 // 每帧数据长度
#pragma pack(1)
typedef struct __Mpu6050_Str_ //原始数据为XYZ方向的(重力加速度、角速度)数据
{
short X_data;
short Y_data;
short Z_data;
}Mpu6050_Str;
typedef union _Upload_Data_
{
unsigned char buffer[PROTOCL_DATA_SIZE];
struct _Sensor_Str_
{
unsigned int Header;
float X_speed; // 差分轮运动学正解得到XYZ方向的线性速度
float Y_speed;
float Z_speed;
float Source_Voltage; // 电压信号
Mpu6050_Str Link_Accelerometer; //重力加速度原始数据
Mpu6050_Str Link_Gyroscope; //陀螺仪角速度原始数据
unsigned char End_flag;
}Sensor_Str;
}Upload_Data;
3.底盘接收上位机数据
- 接收上位机的控制指令,控制电机运动(运动学逆解)
- 通过串口1(USART1)进行数据接收校验
/*
@ describetion: USART1 interrupt process function
@ param: void
@ return: none
@ author: Xuewei Zhou
@ date : 2019-4-17
@ note:
@ function: void USART1_IRQHandler(void)
*/
unsigned char Rcount = 0;
void USART1_IRQHandler(void)
{
if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)
{
Recive_Data.buffer[Rcount] = USART_ReceiveData(USART1);
(Recive_Data.buffer[0] == 0xFe)?(Rcount++):(Rcount = 0);
if (Rcount == PROTOCL_DATA_SIZE) //验证数据包的长度
{
if(Recive_Data.Sensor_Str.Header == PROTOCOL_HEADER) //验证数据包的头部校验信息
{
if(Recive_Data.Sensor_Str.End_flag == PROTOCOL_END) //验证数据包的尾部校验信息
{
//接收上位机控制命令,使机器人产生相应的运动
Kinematics_Positive(Recive_Data.Sensor_Str.X_speed, Recive_Data.Sensor_Str.Z_speed);
}
}
Rcount = 0;
}
}
}
- 对接收到的指令进行运动学解析
正解:通过差分轮的速度求出空间当中的XYZ的速度。(用于向ubantu提交里程计数据)
逆解:将空间中的XYZ三个方向的速度转化到轮子运动上
/*
@ describetion: Chassis kinematics Positive solution function
@ param: float vx,float vz
@ return: none
@ author: Xuewei Zhou
@ date : 2019-4-17
@ note:
@ function: void Kinematics_Positive(float vx,float vz)
*/
void Kinematics_Positive(float vx,float vz)
{
if(vx == 0.0f){ //原地旋转或静止
Right_moto.Target_Speed = vz * Base_Width / 2.0f;
Left_moto.Target_Speed = (-1) * Right_moto.Target_Speed;
}
else if(vz == 0.0f){ //静止或者前后运动
Right_moto.Target_Speed = Left_moto.Target_Speed = vx;
}
else{ //在前进或者后退过程中转弯
Left_moto.Target_Speed = vx - vz * Base_Width / 2.0f;
Right_moto.Target_Speed = vx + vz * Base_Width / 2.0f;
}
}
三、机器人PID控制器代码
1.
总结
提示:这里对文章进行总结:
例如:以上就是今天要讲的内容,本文仅仅简单介绍了pandas的使用,而pandas提供了大量能使我们快速便捷地处理数据的函数和方法。