智能小车地盘控制(二)代码实现

1、功能分类

由于本例是基于STM32CubeIDE开发，在前一篇文章中配置好，点击保存便会自动生成初始化的代码，用户只需要添加应用代码即可。主要包括编码器功能、PWM功能、定时器功能、串口通信功能。

2、代码

2.1 main函数

main函数中主要是设置一些变量的初始化，开启各个外围接口，在while循环内发送里程信息。

int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */
  pulseR = 0; //右轮电机PWM值变量
  pulseL = 0; //左轮电机PWM值变量

  SpdSum1 = 0; //右编码器计数值
  SpdSum2 = 0; //左编码器计数值
  dir1=0; //右编码器方向 1：正，2：负
  dir2=0; //左编码器方向

  uint8_t j=0;

  ENC_Clear_Speed_Buffer(); //编码器计数值求平均时缓存初始化
  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_TIM3_Init();
  MX_USART1_UART_Init();
  MX_TIM2_Init();
  MX_TIM4_Init();
  MX_TIM1_Init();
  /* USER CODE BEGIN 2 */
  HAL_TIM_Base_Start_IT(&htim1);//timer1定时器开，设置1ms定时，用于更新PWM值等功能
  ret1 = HAL_TIM_Encoder_Start(&htim2,TIM_CHANNEL_ALL);//开启右编码器功能
  HAL_TIM_Encoder_Start(&htim3,TIM_CHANNEL_ALL); //开启左编码器功能
  delay_init(72); //利用systick定时器实现延时函数功能初始化
  //disable LN298
  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_RESET);
  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, GPIO_PIN_RESET);
  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_6, GPIO_PIN_RESET);
  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_9, GPIO_PIN_RESET);
  //开启串口接收中断
  HAL_UART_Receive_IT(&huart1, (uint8_t*)USART_RX_BUF, sizeof(USART_RX_BUF));
//开启PWM定时器
  //HAL_TIM_PWM_Stop(&htim4,TIM_CHANNEL_2);
  //HAL_TIM_PWM_Stop(&htim4,TIM_CHANNEL_3);
  HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3);
  
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */

	  /*
	   * uart overrun error self recovery
	   * 本例测试中出现串口overrun错误，在此回复串口功能，不一定需要，根据实际情况定。
	   * */
	  cr1its  = READ_REG(huart1.Instance->CR1);;
	  if((cr1its & USART_CR1_RXNEIE) ==0)
		  HAL_UART_Receive_IT(&huart1, (uint8_t *)aRxBuffer, 1);
		  
	  delay_ms(20);
	  
	  if(stateHandle&0x01)//
	  {
			 x_data.odoemtry_float=position_x;//单位mm
			 y_data.odoemtry_float=position_y;//单位mm
			 theta_data.odoemtry_float=theta;//单位rad
			 vel_linear.odoemtry_float=linear_vel;//单位mm/s
			 vel_angular.odoemtry_float=angular_vel;//单位rad/s

	   //将所有里程计数据存到要发送的数组
			 odometry_data[0] =0xAA; //发送包头字节1
			 odometry_data[1] =0x55;//发送包头字节2
			 odometry_data[2] =0x01;//功能码，未定义
			 odometry_data[3] =0x14;//发送数据包中数据的长度，分别包括X方向位移（float型，四个字节）、Y方向位移，角度、线速度、角速度，共20个字节

		for(j=0;j<4;j++)
		{
			odometry_data[j+4]=x_data.odometry_char[j];
			odometry_data[j+8]=y_data.odometry_char[j];
			odometry_data[j+12]=theta_data.odometry_char[j];
			odometry_data[j+16]=vel_linear.odometry_char[j];
			odometry_data[j+20]=vel_angular.odometry_char[j];
		}
     sndCRCVal = CRC16(odometry_data,USART_SEN_LEN-4);//计算CRC值
		 odometry_data[24] =sndCRCVal;
		 odometry_data[25] =sndCRCVal>>8;
		 odometry_data[26]=0x0d;//添加结束
		 odometry_data[27]=0x0a;//添加结束

		HAL_UART_Transmit(&huart1,odometry_data,sizeof(odometry_data),0xfff); //发送至上位机
		stateHandle&=0xFE;//
	  }

	  car_control(rightdata.d,leftdata.d);	 //将接收到的左右轮速度赋给小车

  }
  /* USER CODE END 3 */
}

2.2 1ms定时器

1ms定时器，重写更新回调函数，当定时器达到设置的时间时，会进入响应的定时器中断响应函数，中断响应函数根据不同的中断标志位判断是哪种类型的定时器中断，进而调用不同的中断处理函数。

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
	if(htim->Instance == TIM1)
	{
		stateHandle |= 0x01; //对应main函数中的while循环的状态值

		//encoder1
		SpdSum1 = (int16_t)(__HAL_TIM_GET_COUNTER(&htim2));//获取右编码器脉冲的计数值
		__HAL_TIM_SET_COUNTER(&htim2, 0);//编码器脉冲计数清零

		hSpeed_Buffer1[bSpeed_Buffer_Index] = SpdSum1;//right，存入数组作求平均用。

		//encoder2
		SpdSum2 = (int16_t)(__HAL_TIM_GET_COUNTER(&htim3));//获取左编码器脉冲的计数值
		__HAL_TIM_SET_COUNTER(&htim3, 0);//编码器脉冲计数清零

		hSpeed_Buffer2[bSpeed_Buffer_Index] = SpdSum2;//left，存入数组作求平均用。

		//储存编码数（脉冲数），用于里程计计算
		Milemeter_L_Motor= (float)SpdSum2;//(float)temp2; //储存脉冲数组
		Milemeter_R_Motor= (float)SpdSum1 ;//float)temp4;
		odometry(Milemeter_R_Motor,Milemeter_L_Motor);//计算里程

		bSpeed_Buffer_Index++;//数组移位
		if(bSpeed_Buffer_Index >=SPEED_BUFFER_SIZE)
		{
			bSpeed_Buffer_Index=0;//缓存左右轮编码数到数组变量清零
		}

		ENC_Calc_Average_Speed();//计算三次电机的平均编码数

		pulseR = (uint16_t)Gain1(); //电机A转动 PID调节控制right
		htim4.Instance->CCR2 = pulseR;//更新PWM值
		pulseL = (uint16_t)Gain2(); //电机B转动 PID调节控制left
		htim4.Instance->CCR3 = pulseL;
	}
}

2.3 串口通信—接收中断

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
	//TO-DO uart receive process
/**
 * 与上位机通信协议，自定义
 * 包头 0x55 0xAA
 * 功能码 0x01->开始，0x03->停止,0x04->others
 * length
 * data
 * CRC 2 bytes
 * 示例:0x55 0xAA 0x01 0x0a,dir1,dir2,spd1_B1,spd1_B2 spd1_B3 spd1_B4 spd2_B1 spd2_B2 spd2_B3 spd2_B4 CRCL,CRCH (dirx=1->forward,dirx=2->backward)，速度为float型，左右两便速度值各四各字节
 */
/* Prevent unused argument(s) compilation warning */
  UNUSED(huart);
  /* NOTE: This function should not be modified, when the callback is needed,
		   the HAL_UART_RxCpltCallback could be implemented in the user file
   */
  if(huart->Instance == USART1)
  {
	  if(flag!=1)
	  {
		  if(state == 0)
		  {
			  if(aRxBuffer[0] ==0x55)
			      header = 0x0055;
			  else if(aRxBuffer[0] ==0xAA)
			  {
				  header=(header<<8)|aRxBuffer[0];
				  if(header==0x55AA)
				  {//收到包头
					  state=1;
					  USART_RX_BUF[0]=0x55;
					  USART_RX_BUF[1]=0xAA;
					  recv_cnt=2;
				  }
			  }
			  else
				  header = 0;
		  }
		  else if(state ==1)
		  {
			  USART_RX_BUF[recv_cnt++]=aRxBuffer[0];
			  state =2;
		  }
		  else if(state ==2)
		  {
			  pkt_len=aRxBuffer[0];
			  USART_RX_BUF[recv_cnt++]=pkt_len;
			  if(pkt_len==10) //在此要判断一下接收到的数据长度要和用户设置的一致，不一致可能是通信丢包等问题导致这一帧数据错误，如果数据长度超过接收数组长度，会导致hardfault中断！！！！
				  state=3;
			  else
				  state=0;
		  }
		  else if(state ==3)
		  {
			 USART_RX_BUF[recv_cnt++]=aRxBuffer[0];
		     if(recv_cnt-4==pkt_len) //pkt_len
		    	 state=4;

		  }
		  else if(state == 4)
		  {
			  USART_RX_BUF[recv_cnt++]=aRxBuffer[0];
			  state=5;
		  }
		  else if(state == 5)
		  {
			  USART_RX_BUF[recv_cnt++]=aRxBuffer[0];
			  checksum = CRC16(USART_RX_BUF,USART_REC_LEN-2);
			  if (checksum == (uint16_t)(USART_RX_BUF[14]<<8|USART_RX_BUF[15]))  //ros 14<<8 sscom 15<<8
			  {
				  if(USART_RX_BUF[2]==0x01)
				  {

					  dir1 = USART_RX_BUF[4];
					  dir2 = USART_RX_BUF[5];
					  for(t=0;t<4;t++)
					  {
						  rightdata.data[t]=USART_RX_BUF[t+6];
						  leftdata.data[t]=USART_RX_BUF[t+10];
					  }

					  //储存左右轮速度命令值
					  odometry_right=rightdata.d;//单位mm/s
					  odometry_left=leftdata.d;//单位mm/s
					  if(dir1==1)
					  {
						  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_6, GPIO_PIN_RESET);
						  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, GPIO_PIN_SET);
					  }
					  if(dir1==2)
					  {
						  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_6, GPIO_PIN_SET);
						  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, GPIO_PIN_RESET);
					  }
					  if(dir2==1)
					  {
						  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_RESET);
						  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_9, GPIO_PIN_SET);
					  }
					  else if(dir2==2)
					  {
						  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_SET);
						  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_9, GPIO_PIN_RESET);
					  }
					  else
					  {

					  }
				  }
				  if(USART_RX_BUF[2]==0x03)
				  {
					  // stop motor
					  HAL_TIM_PWM_Stop(&htim4, TIM_CHANNEL_2);
					  HAL_TIM_PWM_Stop(&htim4, TIM_CHANNEL_3);
					  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_RESET);
					  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, GPIO_PIN_RESET);
					  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_6, GPIO_PIN_RESET);
					  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_9, GPIO_PIN_RESET);
				  }
			  }
			  state=0;
			  recv_cnt=0;

		  }
	  }
	  HAL_UART_Receive_IT(&huart1, (uint8_t *)aRxBuffer, 1);
  }

}

完整代码地址链接：stm32