搬砖笔记

SourceLizi's Blog

0%

RoboMaster A板下的STM32CubeMX应用

发表于 分类于

以下配置均基于RoboMaster A板

GM6020电机控制

官方例程

在CubeMX里面配好CAN1, 详细配置按照例程来(其实只要Time Quantum一样即可)

img

同时配置启用CAN1 RX0中断以便于后面处理反馈报文。另外需要按照A板的原理图，确保CAN1 TX和RX引脚为PD1和PD0

生成代码后还要按照例程加入用户初始化函数，配置过滤并启用回调函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
/**
  \* @brief init can filter, start can, enable can rx interrupt
  \* @param hcan pointer to a CAN_HandleTypeDef structure that contains
  \*     the configuration information for the specified CAN.
  \* @retval None
  */
 void can_user_init(CAN_HandleTypeDef* hcan)
 {
  CAN_FilterTypeDef can_filter;

  can_filter.FilterBank = 0;            // filter 0
  can_filter.FilterMode = CAN_FILTERMODE_IDMASK; // mask mode
  can_filter.FilterScale = CAN_FILTERSCALE_32BIT;
  can_filter.FilterIdHigh = 0;
  can_filter.FilterIdLow = 0;
  can_filter.FilterMaskIdHigh = 0;
  can_filter.FilterMaskIdLow = 0;        // set mask 0 to receive all can id
  can_filter.FilterFIFOAssignment = CAN_RX_FIFO0; // assign to fifo0
  can_filter.FilterActivation = ENABLE;      // enable can filter
  can_filter.SlaveStartFilterBank = 14;     // only meaningful in dual can mode

  HAL_CAN_ConfigFilter(hcan, &can_filter);    // init can filter
  HAL_CAN_Start(hcan);             // start can1
  HAL_CAN_ActivateNotification(hcan, CAN_IT_RX_FIFO0_MSG_PENDING); // enable can1 rx interrupt
 }

 void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan){
   if(hcan->Instance == CAN1){
     //这里写处理数据的代码
   }
 }

[GM6020的说明书](https://rm-static.djicdn.com/tem/17348/RoboMaster GM6020直流无刷电机使用说明.pdf) 说的电机反馈格式不够明确，机械角度为unsigned short而转速和电流应该为signed short. 并且三者都是反STM32的大端模式，需要转换。

这里为了兼容C620电调给多了一个参数offset，这里只要M6020_ID_OFFSET即0x204即可

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
/**
  \* @brief transfer raw data to pubilc struct
   \* @param hcan  : the CAN handler to receive raw data,
     offset : std id offset, must be one of value between C620_MOTOR_NUM and M6020_MOTOR_NUM
  \* @retval HAL_OK if success otherwise HAL_ERROR
  \* @attention 
  */
 //TODO : save multi-motor received data
 
 HAL_StatusTypeDef get_motor_data(CAN_HandleTypeDef* hcan,uint32_t offset){
   CAN_RxHeaderTypeDef rx_header;
   uint32_t motor_id=0;
   uint8_t rx_buffer[8];
   if(HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_buffer)==HAL_OK){
     motor_id = rx_header.StdId - offset;
     if(motor_id > 0 &&
       ((offset == C620_ID_OFFSET && motor_id <= C620_MOTOR_NUM)||
       (offset == M6020_ID_OFFSET && motor_id <= M6020_MOTOR_NUM))){
       mRxMsg.raw_angle = ((rx_buffer[0] << 8) | rx_buffer[1]);
       mRxMsg.speed_rpm = ((rx_buffer[2] << 8) | rx_buffer[3]);
       mRxMsg.current = ((rx_buffer[4] << 8) | rx_buffer[5]);
       mRxMsg.tempture = rx_buffer[6];
     }
   }else return HAL_ERROR;
   return HAL_OK;
 }

发送报文也需要做大小端转换，其实就是将前6个字节两两互换，id按照说明书里面的标识符选。此处可以使用Cortex-M4指令集里面的__REV16优化操作，单个指令即可完成两个16bit short的大小端转换

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
/**
  \* @brief set the motor vlotage
  \* @param hcan : the CAN handler to receive raw data
   RxMsg: the pointer to voltage/current data struct (4 motor data per struct)
   id  : choose from C620_ID_BASE,C620_ID_EXTEND,M6020_ID_BASE,M6020_ID_EXTEND
  \* @retval HAL_OK if success otherwise HAL_ERROR
  \* @attention 
  */

 HAL_StatusTypeDef set_motor_voltage(CAN_HandleTypeDef* hcan,MotorReceiveMsg* TxMsg, uint32_t id){
   CAN_TxHeaderTypeDef tx_header;
   uint8_t tx_buffer[8];
   //uint8_t* pTxMsg = (uint8_t*)TxMsg;
     uint32_t* pTxMsg = (uint32_t*)TxMsg;
   //endian convert
     /*
   for(uint8_t i=0;i<4;i++){
     tx_buffer[2*i+1]=pTxMsg[2*i];
     tx_buffer[2*i]=pTxMsg[2*i+1];
   }*/
     *(uint32_t*)(&tx_buffer[0])=__REV16(pTxMsg[0]);
   *(uint32_t*)(&tx_buffer[4])=__REV16(pTxMsg[1]);
   tx_header.DLC=8;
   tx_header.IDE = CAN_ID_STD;
     tx_header.RTR = CAN_RTR_DATA;
   tx_header.StdId = id;
   return HAL_CAN_AddTxMessage(hcan, &tx_header, tx_buffer,(uint32_t*)CAN_TX_MAILBOX0);
 }

完成了上面收发函数后，可以采用PID控制电机转到指定角度并保持稳定。首先关于角度差值的问题，电机转向的时候有两个方向可以选，我们希望它转过小一点的那个角度。实现的逻辑是，以180度角为分界线，大于这个角度的差值的应该朝反方向转（即加/减去360）。另外直接用PID控制角度效果不佳，采用串级PID（角度+转速）可以解决这个问题，最后发送报文时还需要确保输出值在给定的-30000~30000之间

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
void set_6020_angle(CAN_HandleTypeDef* hcan, uint16_t ang, pid_struct_t pPID[2]){
        __HAL_CAN_DISABLE_IT(hcan,CAN_IT_RX_FIFO0_MSG_PENDING);
                float delta_ang =  ang- mRxMsg.raw_angle*360/8192;
        int32_t current_speed = mRxMsg.speed_rpm;
        __HAL_CAN_ENABLE_IT(hcan,CAN_IT_RX_FIFO0_MSG_PENDING);
    
        if(delta_ang > 360 && delta_ang < -360) return;
        if(delta_ang > 180){
            delta_ang =  delta_ang - 360;
        }else if(delta_ang < -180){
            delta_ang =  delta_ang + 360;
        }
    
        MotorReceiveMsg mMsg;
        memset(&mMsg,sizeof(MotorReceiveMsg),0);

        mMsg.D1 = (int32_t)pid_calc(&pPID[0],pid_calc(&pPID[1],delta_ang,0),current_speed);
        
        if(mMsg.D1 > 30000) mMsg.D1=30000;
        else if(mMsg.D1 < -30000) mMsg.D1=-30000;
    
        set_motor_voltage(hcan,&mMsg,M6020_ID_BASE);
}

PID的实现可以直接拿例程里面的pid.c和pid.h，目前测试上还可以的参数pPID[0]：KP=40, KI=3, KD=0，pPID[1]：KP=30, KI=0, KD=1

DR16遥控机接收

按照文档给的参数100000Bit/s，数据长度8位，偶校验+一位停止位配好串口。打开串口全局中断和串口接收DMA，设置DMA为循环模式

img

CubeMX配置好后串口无法接收数据，参见CubeMX 串口DMA无法启动

参考遥控器用户手册附录上的代码中双缓冲区的设计，在HAL库环境下同样可以利用循环DMA实现。两帧数据传输时间间隔较长，使用串口的空闲中断可以在帧结束处及时处理数据，而同时后半段缓冲区仍然能接收数据。实际上由于处理数据的时间远小于发送间隔14ms(这对于F4来说可是相当长的一段时间)，单个缓冲区也可以满足要求。但是为了确保在添加其他用中断实现功能时不会干扰接收，可以选择增加1倍的缓冲区换取更宽裕的时间处理其他中断。

参照例程 写出自定义的串口DMA初始化函数，可以避免打开DMA自身中断以及全局变量传参不便的问题。其中RC_FRAME_LENGTH为帧长即18，RC_data_buffer为2倍帧长的缓冲区。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
/**
  \* @brief   enable remote control uart it and initialize DMA
  \* @param[in] huart: uart IRQHandler id
  \* @retval   set HAL_OK or HAL_BUSY
  */
 HAL_StatusTypeDef rc_recv_dma_init(UART_HandleTypeDef* huart){
  uint32_t state = huart->RxState;
   
   if (state == HAL_UART_STATE_READY){
     __HAL_LOCK(huart);
     huart->pRxBuffPtr = (uint8_t *)RC_data_buffer;
     huart->RxXferSize = 2*RC_FRAME_LENGTH;
     huart->ErrorCode = HAL_UART_ERROR_NONE;
 
     /* Enable the DMA Stream */
     HAL_DMA_Start(huart->hdmarx, (uint32_t)&huart->Instance->DR, (uint32_t)RC_data_buffer, 2*RC_FRAME_LENGTH);
     //HAL_DMA_Start(huart->hdmarx, (uint32_t)&huart->Instance->DR, (uint32_t)pData, Size);
     /* 
     \* Enable the DMA transfer for the receiver request by setting the DMAR bit
     \* in the UART CR3 register 
     */
     SET_BIT(huart->Instance->CR3, USART_CR3_DMAR);
   
     __HAL_UART_CLEAR_OREFLAG(huart);
     __HAL_UART_CLEAR_IDLEFLAG(huart);
     __HAL_UNLOCK(huart);
     return HAL_OK;
   }else{
     return HAL_BUSY;
   }
 }

接下来可以照搬上面例程数据处理的函数，再在串口空闲中断(stm32f4xx_it.c中的USART1_IRQHandler)中调用

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
void parse_RC_data(uint8_t* pData){
   RC_CtrlData.rc.ch0 = (int16_t)((int16_t)pData[0] | ((int16_t)pData[1] << 8)) & 0x07FF; 
   RC_CtrlData.rc.ch1 = (int16_t)(((int16_t)pData[1] >> 3) | ((int16_t)pData[2] << 5)) & 0x07FF; 
   RC_CtrlData.rc.ch2 = (int16_t)(((int16_t)pData[2] >> 6) | ((int16_t)pData[3] << 2) | 
             ((int16_t)pData[4] << 10)) & 0x07FF; 
   RC_CtrlData.rc.ch3 = (int16_t)(((int16_t)pData[4] >> 1) | ((int16_t)pData[5]<<7)) & 0x07FF; 
   
   RC_CtrlData.rc.s1 = (uint8_t)((uint8_t)(pData[5] >> 4) & 0x0C) >> 2; 
   RC_CtrlData.rc.s2 = (uint8_t)((uint8_t)(pData[5] >> 4) & 0x03); 
 
   RC_CtrlData.mouse.x = (int16_t)((int16_t)pData[6]) | ((int16_t)pData[7] << 8); 
   RC_CtrlData.mouse.y = (int16_t)((int16_t)pData[8]) | ((int16_t)pData[9] << 8); 
   RC_CtrlData.mouse.z = (int16_t)((int16_t)pData[10]) | ((int16_t)pData[11] << 8);   
 
   RC_CtrlData.mouse.press_left = (uint8_t)pData[12]; 
   RC_CtrlData.mouse.press_right = (uint8_t)pData[13]; 
  
   RC_CtrlData.keyboard.keycode = ((uint16_t)pData[14]);// | ((int16_t)pData[15] << 8); 
     
   RC_CtrlData.rc.ch0-=0x400;
   RC_CtrlData.rc.ch1-=0x400;
   RC_CtrlData.rc.ch2-=0x400;
   RC_CtrlData.rc.ch3-=0x400;
 }
 
 void UART_RxIdleCallback(UART_HandleTypeDef *huart){
   if(huart->Instance == USART1 && __HAL_UART_GET_FLAG(huart,UART_FLAG_IDLE)){
     __HAL_UART_CLEAR_IDLEFLAG(huart);
     DMA_Stream_TypeDef* uhdma = huart->hdmarx->Instance;
     receive_size = (int32_t)(2*RC_FRAME_LENGTH - uhdma->NDTR) - frame_offset;
     if(receive_size == RC_FRAME_LENGTH || receive_size == -RC_FRAME_LENGTH){
       parse_RC_data(&RC_data_buffer[frame_offset]);
       frame_offset = (int32_t)(frame_offset == 0)*RC_FRAME_LENGTH;
     }else{
       //some bytes lost, reset DMA buffer
       __HAL_DMA_DISABLE(huart->hdmarx);
       uhdma->M0AR = (uint32_t)RC_data_buffer;
       uhdma->NDTR = (uint32_t)(RC_FRAME_LENGTH*2);
       frame_offset=0;
       __HAL_DMA_ENABLE(huart->hdmarx);
     }
   }
 }

其中为了防止开始时接收到半帧（偶然情况）而导致帧开始不在缓冲区开头或中间，加入错误处理，如果发现数据长度有误则重启DMA接收。单缓冲区的错误处理也同理。摇杆的零位对应ch0~ch3的中间值1024，减去1024可以让正负符号代表方向便于处理。