Robomaster电控组小白的学习经验分享（一）——用大疆C型开发板控制GM6020电机转动到既定角度

#ifndef __bsp_can_h_
#define __bsp_can_h_

#include "main.h"
#include "can.h"
#include "stm32f4xx.h"

typedef struct
{
    uint16_t can_id;//电机id
    int16_t  set_voltage;//设定的电压值
    uint16_t rotor_angle;//机械角度
    int16_t  rotor_speed;//转速
    int16_t  torque_current;//扭矩电流
    uint8_t  temp;//温度
}moto_info_t;

void can_filter_init(void);
void set_gm6020_motor_voltage(can_handletypedef* hcan,int16_t v1);

#endif

#include "bsp_can.h"

moto_info_t motor_yaw_info;
uint16_t can_cnt;

void can_filter_init(void)//筛选器配置
{
    can_filtertypedef can_filter_st;
    can_filter_st.filteractivation = enable;
    can_filter_st.filtermode = can_filtermode_idmask;
    can_filter_st.filterscale = can_filterscale_32bit;
    can_filter_st.filteridhigh = 0x0000;
    can_filter_st.filteridlow = 0x0000;
    can_filter_st.filtermaskidhigh = 0x0000;
    can_filter_st.filtermaskidlow = 0x0000;
    can_filter_st.filterbank = 0;
    can_filter_st.filterfifoassignment = can_rx_fifo0;
    hal_can_configfilter(&hcan1, &can_filter_st);
    hal_can_start(&hcan1);
    hal_can_activatenotification(&hcan1, can_it_rx_fifo0_msg_pending);
 
    can_filter_st.slavestartfilterbank = 14;
    can_filter_st.filterbank = 14;
    hal_can_configfilter(&hcan1, &can_filter_st);
    hal_can_start(&hcan1);
    hal_can_activatenotification(&hcan1, can_it_rx_fifo0_msg_pending);
}

void hal_can_rxfifo0msgpendingcallback(can_handletypedef *hcan)
{
  can_rxheadertypedef rx_header;
  uint8_t             rx_data[8];
  if(hcan->instance == can1)
  {
  hal_can_getrxmessage(hcan, can_rx_fifo0, &rx_header, rx_data); //receive can data
  switch(rx_header.stdid)
	{
	  case 0x205:
	{
    motor_yaw_info.rotor_angle    = ((rx_data[0] << 8) | rx_data[1]);
    motor_yaw_info.rotor_speed    = ((rx_data[2] << 8) | rx_data[3]);
    motor_yaw_info.torque_current = ((rx_data[4] << 8) | rx_data[5]);
    motor_yaw_info.temp           =   rx_data[6];
		break;
	}
	}
  }
}

void set_gm6020_motor_voltage(can_handletypedef* hcan,int16_t v1)
{
  can_txheadertypedef tx_header;
  uint8_t             tx_data[8] = {0};
    
  tx_header.stdid = 0x1ff;
  tx_header.ide   = can_id_std;
  tx_header.rtr   = can_rtr_data;
  tx_header.dlc   = 8;

  tx_data[0] = (v1>>8)&0xff;
  tx_data[1] =    (v1)&0xff;
	 
  hal_can_addtxmessage(&hcan1, &tx_header, tx_data,(uint32_t*)can_tx_mailbox0);
}

#ifndef __pid_h_
#define __pid_h_

#include "main.h"
#include "stm32f4xx.h"

typedef struct _pid_struct_t
{
  float kp;//比例
  float ki;//积分
  float kd;//微分
  float i_max;//积分限幅
  float out_max;//输出限幅
  
  float ref;      // target value目标角度
  float fdb;      // feedback value设定角度
  float err[2];   // error and last error差值

  float p_out;//比例输出
  float i_out;//积分输出
  float d_out;//微分输出
  float output;//pid总输出
}pid_struct_t;

void pid_init(pid_struct_t *pid,
              float kp,
              float ki,
              float kd,
              float i_max,
              float out_max);

void gimbal_pid_init(void);
float pid_calc(pid_struct_t *pid, float ref, float fdb);

#endif

#include "pid.h"

pid_struct_t gimbal_yaw_speed_pid;
pid_struct_t gimbal_yaw_angle_pid;

void pid_init(pid_struct_t *pid,
              float kp,
              float ki,
              float kd,
              float i_max,
              float out_max)//pid初始化函数
{
  pid->kp      = kp;
  pid->ki      = ki;
  pid->kd      = kd;
  pid->i_max   = i_max;
  pid->out_max = out_max;
}


float pid_calc(pid_struct_t *pid, float ref, float fdb)//pid运算函数
{
  pid->ref = ref;
  pid->fdb = fdb;
  pid->err[1] = pid->err[0];
  pid->err[0] = pid->ref - pid->fdb;
  
  pid->p_out  = pid->kp * pid->err[0];
  pid->i_out += pid->ki * pid->err[0];
  pid->d_out  = pid->kd * (pid->err[0] - pid->err[1]);
  limit_min_max(pid->i_out, -pid->i_max, pid->i_max);
  
  pid->output = pid->p_out + pid->i_out + pid->d_out;
  limit_min_max(pid->output, -pid->out_max, pid->out_max);
  return pid->output;
}

void gimbal_pid_init()//角度环和速度环的pid初始化,只是初测出来的数据，具体还需要测试
{
	pid_init(&gimbal_yaw_speed_pid, 30, 0, 0, 30000, 30000);//p=30,i=0,d=0
	pid_init(&gimbal_yaw_angle_pid, 400, 0, 0, 0, 320);//p=400,i=0,d=0
}

#include "main.h"
#include "can.h"
#include "dma.h"
#include "usart.h"
#include "gpio.h"

#include "bsp_can.h"
#include "pid.h"
#include "bsp_dbus.h"

int16_t led_cnt;
int16_t text_speed = 0;
int16_t target_yaw_speed;
float target_yaw_angle = 0;
float now_yaw_angle;
extern moto_info_t motor_yaw_info;
extern pid_struct_t gimbal_yaw_speed_pid;
extern pid_struct_t gimbal_yaw_angle_pid;


double msp(double x, double in_min, double in_max, double out_min, double out_max)//映射函数，将编码器的值（0~8191）转换为弧度制的角度值（-pi~pi）
{
	return (x-in_min)*(out_max-out_min)/(in_max-in_min)+out_min;
}


int main(void)
{
 
  hal_init();

  systemclock_config();

  mx_gpio_init();
  mx_dma_init();
  mx_can1_init();
  mx_usart3_uart_init();
  mx_usart1_uart_init();
 
  can_filter_init();//can初始化
  gimbal_pid_init();//pid初始化

  while (1)
  {
   
	led_cnt ++;
    if (led_cnt == 250)
    {
      led_cnt = 0;
      hal_gpio_togglepin(gpioh,gpio_pin_11); //blink cycle 500ms
    }
		
		now_yaw_angle=msp(motor_yaw_info.rotor_angle,0,8191,-pi,pi);//计算当前的编码器角度值，运用msp函数将编码器的值映射为弧度制

		pid_calc(&gimbal_yaw_angle_pid,target_yaw_angle, now_yaw_angle);//角度环
		pid_calc(&gimbal_yaw_speed_pid,gimbal_yaw_angle_pid.output, motor_yaw_info.rotor_speed);//速度环
		
		set_gm6020_motor_voltage(&hcan1,gimbal_yaw_speed_pid.output);//can发送函数，发送经过pid计算的电压值

		hal_delay(40);
		}

}