差分英伟达机器人_底层控制系统

该层控制系统接入了加速度、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提供了大量能使我们快速便捷地处理数据的函数和方法。