ROS2阿克曼开发平台开源方案介绍

#include "sys.h"

/*===================================================================
程序功能：ROS小车底层代码（全部） 差速底盘和阿克曼底盘通用
程序编写：公众号：小白学移动机器人
其他    ：如果对代码有任何疑问，可以私信小编，一定会回复的。
=====================================================================
------------------关注公众号，获得更多有趣的分享---------------------
===================================================================*/
int main(void)
{ 
	//发送计数
	char sendCount=0;
	
	GPIO_PinRemapConfig(GPIO_Remap_SWJ_Disable,ENABLE);
	GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);//禁用JTAG 启用 SWD
	
	MY_NVIC_PriorityGroupConfig(2);	//=====设置中断分组
	
	delay_init();	    	        //=====延时函数初始化
	LED_Init();                     //=====LED初始化    程序灯	
	BEEP_Init();                    //=====蜂鸣器初始化，异常报警
	
	usart1_init(115200);	        //=====串口1初始化  树莓派通信
	usart3_init(115200);            //=====串口3初始化  蓝牙，控制和调试

	IIC_Init();                     //=====IIC初始化    读取MPU6050数据
	MPU6050_initialize();           //=====MPU6050初始化	
	DMP_Init();                     //=====初始化DMP 

	Encoder_Init_TIM3();            //=====初始化右轮编码器接口
	Encoder_Init_TIM4();            //=====初始化左轮编码器接口
	
	MY_ADC_Init();                  //=====adc初始化    电池电量检测
	
	Motor_PWM_Init(7200-1, 1-1);    //=====初始化PWM 10KHZ(AT8870)，用于驱动电机 
	Steer_PWM_Init(60000-1, 24-1);  //=====初始化PWM 50HZ，用于驱动舵机

	PID_Init();                     //=====PID初始化
	
	MBOT_EXTI_Init();               //=====MPU6050 5ms定时中断初始化

	while(1)
	{
		//给树莓派发送速度，角度,这里速度已经乘以1000,角度(0.0-359.0)乘以50
		if(sendCount==0)//发送  14.4ms  发送一次数据 70Hz 左右
		{
			//发送需要一定的延时
			usartSendData(USART1,(short)leftSpeedNow,(short)rightSpeedNow,(short)(yaw*50),sendCtrlFlag);  //1ms
			//蓝牙调试时用，不调试注释
			// pcShow();                                                           //2.2ms单个float数据
			sendCount++;
		}
		else
		{
			sendCount++;
			if(sendCount==25)
				sendCount=0;
		}
		//获取角度		
		getAngle(&yaw,&yaw_acc_error);                                                     
	} 
}

//中断服务函数
void USART1_IRQHandler()
{
	if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)
 	 {
		 USART_ClearITPendingBit(USART1,USART_IT_RXNE);//首先清除中断标志位
		 usartReceiveOneData(USART1,&leftSpeedSet,&rightSpeedSet,&frontAngleSet,&receCtrlFlag);
	 }
}

/**************************************************************************
函数功能：所有的控制代码都在这里面
          5ms定时中断由MPU6050的INT引脚触发		
**************************************************************************/
void EXTI9_5_IRQHandler(void) 
{                                                         
	EXTI_ClearITPendingBit(EXTI_Line8);                             //===清除LINE8线路挂起位		
	
	Led_Flash(200);                                                 //===LED闪烁，证明程序正常运行	
	
	yaw_acc_error += FIVE_MS_ERROR;								    //===yaw漂移误差累加
	
	Get_battery_volt_average(&Voltage,100);		                    //===电池电压测量，单位mv，100次取一次平均
	
	Get_Motor_Speed(&leftSpeedNow,&rightSpeedNow);                  //===获取左右轮子真实速度
	
	pid_Task_Letf.speedSet  = leftSpeedSet;	                        //===给速度设定值和实时值赋值
	pid_Task_Right.speedSet = rightSpeedSet;
	pid_Task_Letf.speedNow  = leftSpeedNow;
	pid_Task_Right.speedNow = rightSpeedNow;
	
	Pid_Ctrl(&motorLeft,&motorRight);                               //===执行PID控制函数
	Steer_Ctrl(frontAngleSet,&motorFrontSteer);                     //===执行舵机，差速小车无需修改完全兼容

	if(Turn_Off(Voltage) == 0)                                      //===如果电量异常低
	{	
		BEEP_Disable();                                             //===正常情况，蜂鸣器失能 
		Set_Pwm(motorLeft,motorRight,motorFrontSteer);              //===赋值给PWM寄存器 		
	} 	  
	else
	{
		BEEP_Flash(20);                                            	//===电量报警
	}
}