尊敬的用户，您还未登录，登录之后更精彩！

首页 » STM32 » 正文
STM32

【嵌入式--伺服电机】MIT无刷电机FOC驱动程序分析

文章目录[隐藏]

系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理

前言

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！

本文先对无刷电机FOC驱动程序分析做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

提示：以下是本篇文章正文内容

一、FOC主程序分析

1、主程序步骤

（1）初始化

（1）初始化片内外设PWM\ADC\LED
（2）初始化驱动芯片
（3）初始化FOC算法
（4）初始化TIM定时器
（5）初始化CAN接收中断
（6）初始化电机参数
（7）初始化编码器位置
（8）初始化串口中断

（2）开始中断模式设置

（3）while循环

没有运行东西的，理论上全部用中断实现的，但是可以在while中调试输出每个传感器的信息

2、主程序代码

uint16_t ram_encoder_data = 0;		//±àÂëÆ÷Êý¾ÝµÄÄÚ´æ±äÁ¿
int main()
{
/*³õʼ»¯Æ¬ÄÚÍâÉèµÄ*/
    controller.v_bus = V_BUS;			//ÉèÖõçѹ24V
    controller.mode = 0;					//»¹Ã»ÉèÖõç»úģʽ
    Init_All_HW(&gpio);           //³õʼ»¯PWM, ADC, LEDÓ²¼þƬÄÚÍâÉè
    wait(0.1);  
	
/*³õʼ»¯Çý¶¯Ð¾Æ¬µÄ*/
    gpio.enable->write(1);        //ʹÄÜDrv8323xÇý¶¯Ð¾Æ¬£¬DRV832xʹÄÜÒý½ÅÀ­¸ß£¬mbedµ×²ãµÄд·¨£¬Õâ¸öÒý½ÅÔÚÉÏÃæ³õʼ»¯¹ýµÄ
    wait_us(100);
    drv.calibrate();               //Çý¶¯ICÆ«ÒÆУ׼µÄµçÁ÷¼ì²â·Å´óÆ÷ABCµÄ¶ÌÊäÈë
    wait_us(100);
    drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1);                 //ÅäÖÃÇý¶¯Æ÷¿ØÖƵļĴæÆ÷,¾ßÌå¿´Çý¶¯ICµÄÊý¾ÝÊÖ²á
    wait_us(100);
    drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x0, 0x0, 0x0, SEN_LVL_1_0);       //ÅäÖõçÁ÷¼ì²â·Å´óÆ÷¼Ä´æÆ÷
    wait_us(100);
    drv.write_OCPCR(TRETRY_4MS, DEADTIME_200NS, OCP_RETRY, OCP_DEG_8US, VDS_LVL_1_88);  //ÅäÖùýµçÁ÷±£»¤¼Ä´æÆ÷
    zero_current(&controller.adc1_offset, &controller.adc2_offset);                     //²âÁ¿µçÁ÷´«¸ÐÆ÷ÁãµãƯÒÆ
    drv.disable_gd();																																		// IC״̬ʧÄÜ
    wait(0.1);
/*³õʼ»¯FOCËã·¨*/
    reset_foc(&controller);             // ¸´Î»µçÁ÷¿ØÖÆÆ÷
    reset_observer(&observer);          // ¸´Î»¹Û²âÆ÷
/*³õʼ»¯TIM¶¨Ê±Æ÷ÖжÏ*/
    TIM1->CR1 ^= TIM_CR1_UDIS;          // ¹Ø±ÕÖжÏ
    //TIM1->CR1 |= TIM_CR1_UDIS;        // ʹÄÜÖÐ¶Ï 
    wait(0.1);  
    NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2);    // ½¨Á¢ÖÕ¶ËÏòÁ¿Á¬½Ó
/*³õʼ»¯CAN½ÓÊÕÖжÏ*/
    NVIC_SetPriority(CAN1_RX0_IRQn, 3);
    can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);                                 
    txMsg.id = CAN_MASTER;
    txMsg.len = 6;
    rxMsg.len = 8;
    can.attach(&onMsgReceived);         //½¨Á¢Á¬½Ó¡°CAN½ÓÊÕ´¦Àí¡±Öжϴ¦Àíº¯Êý  
/*³õʼ»¯µç»ú¿ØÖƲÎÊý*/		
    //Èç¹ûÓû§Ã»ÓÐÉèÖÃÊ×Ñ¡ÏÇëÉèÖÃĬÈÏ״̬
		//´¢´æÔÚflashÖеÄÖµ£¬¿ÉÒÔÓÉÓû§²Ù×÷ÐÞ¸Ä
    prefs.load();                       //ÏȶÁ flashÀïÃæµÄÊý¾Ý£¬ÓÃÓÚ³õʼ»¯µÄʱºòÉèÖõç»ú²ÎÊý
		//ÔÚÏÂÃæ¿ÉÒԸĵç»ú²ÎÊý£¨¶ÔÄÚ£©
    if(isnan(E_OFFSET))                         {E_OFFSET = 0.0f;}// ÉèÖñàÂëÆ÷µç½Ç¶ÈÆ«ÒÆ 
    if(isnan(M_OFFSET))                         {M_OFFSET = 0.0f;}// ÉèÖñàÂëÆ÷»úе½Ç¶ÈÆ«ÒÆ
    if(isnan(I_BW) || I_BW==-1)                 {I_BW = 1000;}		// ÉèÖõçÁ÷»·´ø¿í
    if(isnan(I_MAX) || I_MAX ==-1)              {I_MAX = 15;}			// ÉèÖÃÊä³öÁ¦¾ØÏÞÖÆ (current limit = torque_limit/(kt*gear ratio))£¬¸ÄÊä³öÁ¦¾ØÔÚÕâÀï¸Ä
    if(isnan(I_FW_MAX) || I_FW_MAX ==-1)        {I_FW_MAX=0;}			// ÉèÖÃ×î´óÈõ´ÅµçÁ÷
    if(isnan(CAN_ID) || CAN_ID==-1)             {CAN_ID = 1;}			// ÉèÖÃCAN ×ÜÏßÉϵç»úµÄ ID
    if(isnan(CAN_MASTER) || CAN_MASTER==-1)     {CAN_MASTER = 0;}	// ÉèÖÃCAN ×ÜÏßÖ÷É豸µÄ ID
    if(isnan(CAN_TIMEOUT) || CAN_TIMEOUT==-1)   {CAN_TIMEOUT = 0;}// ÉèÖÃCAN ×ÜÏß³¬Ê±Ê±¼ä
		init_controller_params(&controller);													//³õʼ»¯µç»úFOCÎå¸ö¿ØÖƲÎÊý
/*³õʼ»¯±àÂëÆ÷λÖã¨ÓôűàÂëÆ÷ÌṩµÄAPI¾ÍÐУ©*/  
    Encoder_spi.SetElecOffset(E_OFFSET);        // ÉèÖñàÂëÆ÷µç½Ç¶ÈÆ«ÒÆ
    Encoder_spi.SetMechOffset(M_OFFSET);        // ÉèÖñàÂëÆ÷»úе½Ç¶ÈÆ«ÒÆ
    int lut[128] = {0};													// ±àÂëÆ÷Æ«ÒÆLUT-128¸öÔªËس¤¶È±í
    memcpy(&lut, &ENCODER_LUT, sizeof(lut));  	// ¸´ÖÆ°áÊý¾Ý
    Encoder_spi.WriteLUT(lut);                  // ÉèÖÃλÖô«¸ÐÆ÷·ÇÏßÐÔ²éÕÒ±í
/*³õʼ»¯´®¿ÚÖжϣ¬²¢´òÓ¡µç»úÏà¹ØÐÅÏ¢*/
    pc.baud(921600);                            //λÖô®¿Ú²¨ÌØÂÊ
    wait(0.01);
    pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
    wait(0.01);
    printf("\n\r Debug Info:\n\r");
    printf(" Firmware Version: %s\n\r", VERSION_NUM);
    printf(" ADC1 Offset: %d    ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset);	//ÏÔʾADC±íÕ÷µçѹֵ
    printf(" Position Sensor Electrical Offset:   %.4f\n\r", E_OFFSET);																	//ÏÔʾ±àÂëÆ÷²âµ½µÄµç½Ç¶È
    printf(" Output Zero Position:  %.4f\n\r", M_OFFSET);																								//ÏÔʾ±àÂëÆ÷²âµ½µÄ»úе½Ç¶È
    printf(" CAN ID:  %d\n\r", CAN_ID);																																	//ÏÔʾCANͨѶID
    pc.attach(&serial_interrupt);               																												//Á¬½Ó´®¿ÚÖжÏ
//¿ªÊ¼Ñ¡Ôñµç»úģʽ   
		state_change = 1;
    while(1)
    {

        wait(0.1);                                  		 // µÈ´ý 100ms
        ram_encoder_data = Encoder_spi.GetRawPosition(); // »ñÈ¡±àÂëÆ÷ʵʱλÖÃת½Ç
//      drv.print_faults();        											 // µ÷ÊÔÓõģ¬ ʵʱÏÔʾÇý¶¯Æ÷´íÎó״̬£¬MITÇý¶¯ICµÄfaultsÓ¦¸ÃÊǽÓÁ˵¥Æ¬»úµÄ
//      printf("%.4f\n\r", controller.v_bus);						 // µ÷ÊÔÓõģ¬ÊµÊ±ÏÔʾµç»úµçѹÌø±ä £¬µçѹÌø±äµÄADCÊÇͨ¹ý¶¨Ê±Æ÷×öµÄ     
       /*
        if(state == MOTOR_MODE)//µ÷ÊÔÓõÄ
        {
            //printf("%.3f  %.3f  %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance);//ʵʱÏÔʾµç»úÎĵÛ
            //printf("%.3f  %.3f  %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.dtheta_elec);
            //printf("%.3f\n\r", controller.dtheta_mech);
            wait(0.002);
        }
        */
    }
}

.
.

二、中断程序分析

1.can接收中断

主要的任务

（1）接收数据指令
（2）响应数据指令

（1）驱动特殊指令（优先级最高）【用于非串口设置电机模式】

（1）进入电机模式:[OxFF, OxFF, OxFF, OxFF, OxFF, OxFF, OxFF, OxFC]
（2）退出电机模式[OxFF, OxFF, OxFF, OxFF, OxFF, OxFF, OxFF, OxFD]
（3）零位传感器-将机械位置设置为零[OxFF, OxFF, OxFF, OxFF, OxFF, OxFF, OxFF, OxFE]

（2）CAN接口指令（优先级最低）

执行函数：unpack_cmd(rxMsg, &controller);用来描述CAN指令集的
函数功能：CAN信息解压，解压后的数据搬到电机控制的内存上
电机接收的CAN命令

（1）位置设定点p_int
（2）速度设定点v_int
（3）位置增益kp_int
（4）速度增益kd_int
（5）扭矩t_int

CAN数据的格式

/// 16 bit position command, between -4*pi and 4*pi         (positon)
/// 12 bit velocity command, between -30 and + 30 rad/s     (speed)    
/// 12 bit kp, between 0 and 500 N-m/rad                    ()
/// 12 bit kd, between 0 and 100 N-m*s/rad                  ()
/// 12 bit feed forward torque, between -18 and 18 N-m
/// CAN Packet is 8 8-bit words
/// Formatted as follows.  For each quantity, bit 0 is LSB
/// 0: [position[15-8]]
/// 1: [position[7-0]] 
/// 2: [velocity[11-4]]
/// 3: [velocity[3-0], kp[11-8]]
/// 4: [kp[7-0]]
/// 5: [kd[11-4]]

（3）把电机收到的信号返回到主设备，说明电机有响应

电机发送的CAN命令

（1）电机角度
（2）电机角速度
（3）估计扭矩

应答格式

/// 16 bit position, between -4*pi and 4*pi
/// 12 bit velocity, between -30 and + 30 rad/s
/// 12 bit current, between -40 and 40;
/// CAN Packet is 5 8-bit words
/// Formatted as follows.  For each quantity, bit 0 is LSB
/// 0: [position[15-8]]
/// 1: [position[7-0]] 
/// 2: [velocity[11-4]]
/// 3: [velocity[3-0], current[11-8]]

代码

void onMsgReceived()
{
    printf("%df\n", rxMsg.id);//·´À¡¸øÖ÷É豸ÏìÓ¦¸ÃÖ¸ÁîµÄÊÇÄǸöµç»ú
    can.read(rxMsg);          //¶ÁÊý¾Ý²¢´æ´¢µ½»º´æÆ÷
    
    if(rxMsg.id == CAN_ID)    //ÊÕµ½µÄIDÊDZ¾µç»ú¶ÔÓ¦µÄID,±¾µç»úµÄIDÒѾ­ÉèÖôæÈëflashÀïÃæÁË
    {
        controller.timeout = 0;	//¸´Î»CANͨѶ³¬Ê±Ê±¼ä£¬Î¹¹·
			
			//ÏÂÃæÊÇÇý¶¯ÌØÊâÖ¸Áî
			//½øÈëµç»úģʽ[OxFF, OxFF, OxFF, OxFF, OxFF, OxFF, OxFF, OxFC]
        if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) & (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFC)))
        {
            state = MOTOR_MODE;//ÊÖ¶¯Èõç»úÍ£Ö¹£¬²¢ÉèÖÃdqÖáµÄµçѹΪ0£¬¿ÉÒÔÊÖ¶¯êþµç»ú
            state_change = 1;
        }
				
			//Í˳öµç»úģʽ[OxFF, OxFF, OxFF, OxFF, OxFF, OxFF, OxFF, OxFD]
        else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFD)))
        {
            state = REST_MODE;//ģʽûѡÔñ״̬
            state_change = 1;
            gpio.led->write(0);//°ÑÇý¶¯Ð¾Æ¬Ê§ÄÜ£¬²»Êä³ö
        }
				
			//ÉèÖûúеλÖô«¸ÐÆ÷λ0£¬[OxFF, OxFF, OxFF, OxFF, OxFF, OxFF, OxFF, OxFE]
        else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFE)))
        {
            Encoder_spi.ZeroPosition();//ÊÖ¶¯Ð£Õý±àÂëÆ÷µÄλÖÃÖµ
        }
			
				//Ö´Ðеç»úÕý³£µÄÇý¶¯Ä£Ê½£¨¶¼²»ÊÇÉÏÃæÈýÖÖÌØÊâÖ¸Á
        else if(state == MOTOR_MODE)
        {
          unpack_cmd(rxMsg, &controller);//CAN½Ó¿ÚµÄÖ¸Áî½âÎö,°ÑCAN¼Ä´æÆ÷ÀïÃæµÄÊý¾Ý°áµ½µç»ú¿ØÖƲÎÊýµÄÄÚ´æÉÏ
        }
        pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);//°ÑÊÕµ½µÄÊý¾Ý·µ»Ø¸øÖ÷É豸£¬±íÃ÷×Ô¼ºÊÕµ½ÁË£¬Êý¾ÝÌî³ä¶øÒÑ
        can.write(txMsg);	//CAN·¢ËÍ
    }
}

.

2.串口接收中断

主要的任务

（1）接收串口数据标志位用于选择电机模式
（2）接收串口8位数据设置控制电机的参数

设置电机的模式

（1）【27】电机复位状态+接收8位数据清零

设置电机复位状态
复位8未数据cmd_val[i]

（2）【REST_MODE】：电机还在菜单栏上，在这里进行电机模式设置，电机上电默认进来这个模式

c：【CALIBRATION_MODE】校准编码器模式
m：【MOTOR_MODE】电机运行模式
e：【ENCODER_MODE】编码器输出模式
s：【SETUP_MODE】 电机参数设置模式
z：设置电机机械零位模式

（3）【SETUP_MODE】：电机在参数设置模式下了，在这里进行电机参数设置

b：使用接收到的8位数据，设置电流环带宽
i：使用接收到的8位数据，设置电机在CAN总线上的ID
m：使用接收到的8位数据，设置CAN总线上主设备的ID
l：使用接收到的8位数据，设置力矩输出限制
f：使用接收到的8位数据，设置最大弱磁电流
t：使用接收到的8位数据，设置总线超时时间

（4）【ENCODER_MODE】：或设置为【REST_MODE】模式

（5）【MOTOR_MODE】

设置dq轴电流为0

代码

void serial_interrupt(void)
{
    while(pc.readable())				//µÈ´ý´®¿Ú½ÓÊÕµ½Êý¾Ý
    {
        char c = pc.getc();			//ת´æ´®¿Ú½ÓÊÕµ½µÄ×Ö·û£¬³äµ±±ê־λ
        if(c == 27)	                  //µç»úģʽ¸´Î»£¬²¢°ÑͨѶµÄ8λÊý¾ÝÇåÁã						
        {
            state = REST_MODE;			 //ÉèÖÃΪµç»ú¸´Î»×´Ì¬£¬ÌØÊâÖ¸ÁîÇ¿ÖƵç»ú¸´Î»
            state_change = 1;
            char_count = 0;
            cmd_id = 0;
            gpio.led->write(0);; 
            for(int i = 0; i<8; i++) //¸´Î»°ËλÊý¾Ý
            {
                cmd_val[i] = 0;
            }
        }
        
        if(state == REST_MODE)      	//Èôµç»úÔÚ×î³õµÄ¸´Î»×´Ì¬Ï£¬ÔÚÕâÀï½øÐеç»úµÄģʽÉèÖ㬵ç»úÉϵç±ØÐë½øÀ´Ò»´ÎµÄ	
        {
            switch (c)
            {
            case 'c':											//ÉèÖÃΪÔÚ±àÂëÆ÷УÕý״̬
                state = CALIBRATION_MODE;
                state_change = 1;
                break;
            
            case 'm':											//ÉèÖÃΪµç»úÔËÐÐģʽ
                state = MOTOR_MODE;
                state_change = 1;
                break;
            
            case 'e':											//ÉèÖÃΪ±àÂëÆ÷Êä³öģʽ
                state = ENCODER_MODE;
                state_change = 1;
                break;
            
            case 's':											//ÉèÖÃΪµç»ú²ÎÊýÉèÖÃģʽ
                state = SETUP_MODE;
                state_change = 1;
                break;
            
            case 'z':									  	//ÉèÖÃΪ»úеÁãλΪµ±Ç°±àÂëÆ÷λÖÃ
                Encoder_spi.SetMechOffset(0);
                Encoder_spi.Sample(DT);
                wait_us(20);
                M_OFFSET = Encoder_spi.GetMechPosition();
                if (!prefs.ready())//ÄÚ´æ³õʼ»¯
                    prefs.open();
                prefs.flush();          // Write new prefs to flash
                prefs.close();    
                prefs.load(); 
                Encoder_spi.SetMechOffset(M_OFFSET);
                printf("\n\r  Saved new zero position:  %.4f\n\r\n\r", M_OFFSET);
                break;
                
            default:
                break;
            }             
        }
        else if(state == SETUP_MODE)	//ÔÚµç»ú²ÎÊýÉèÖÃģʽ
        {
            if(c == 13)
            {
                switch (cmd_id)
                {
                    case 'b':		// µçÁ÷»·´ø¿í
                        I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f);
                        break;
                    
                    case 'i':		//CAN ×ÜÏßÉϵç»úµÄ ID
                        CAN_ID = atoi(cmd_val);
                        break;
                    
                    case 'm':		//CAN ×ÜÏßÖ÷É豸µÄ ID
                        CAN_MASTER = atoi(cmd_val);
                        break;
                    
                    case 'l':		// Êä³öÁ¦¾ØÏÞÖÆ (current limit = torque_limit/(kt*gear ratio))
                        I_MAX = fmaxf(fminf(atof(cmd_val), 40.0f), 0.0f);
                        break;
                    
                    case 'f':		// ×î´óÈõ´ÅµçÁ÷
                        I_FW_MAX = fmaxf(fminf(atof(cmd_val), 33.0f), 0.0f);
                        break;
                    
                    case 't':		// CAN ×ÜÏß³¬Ê±Ê±¼ä
                        CAN_TIMEOUT = atoi(cmd_val);
                        break;
                    
                    default:
                        printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id);
                        break;
                }  
                if (!prefs.ready())
                    prefs.open();
                prefs.flush();      // Write new prefs to flash
                prefs.close();    
                prefs.load();                                              
                state_change = 1;
                char_count = 0;
                cmd_id = 0;
                for(int i = 0; i<8; i++)	//¸´Î»°ËλÊý¾Ý
                {
                    cmd_val[i] = 0;
                }
            }
            else
            {
                if(char_count == 0)
                {
                    cmd_id = c;
                }
                else
                {
                    cmd_val[char_count-1] = c; 
                }
                pc.putc(c);
                char_count++;
            }
        }
        else if (state == ENCODER_MODE)	//ÔÚ±àÂëÆ÷Êä³öģʽÉÏ
        {
            switch (c)
            {
                case 27:
                    state = REST_MODE;
                    state_change = 1;
                    break;
                
                default:
                    break;
            }
        }
        else if (state == MOTOR_MODE)		//ÔÚµç»úÊä³öģʽÏÂ
        {
            switch (c)
            {
                case 'd':
                    controller.i_q_ref = 0;
                    controller.i_d_ref = 0;
                    break;
                
                default:
                    break;
            }
        }
            
    }
}

.
.

3.TIM定时器中断（运行FOC）

主要的任务

1、40khz频率下读取驱动芯片的电流值
2、SPI通讯读取磁编码器位置速度信息
3、运行电机的不同模式（包括力控FOC）
.
.

（1）传感器数据采集

（1）ADC进行两相电流采样【驱动IC引脚】

(1)温度ADC取数据
(2)两相电流ADC取数据
(3)在驱动芯片上可以读取到表征电机电流的电压值ADC取数据

（2）SPI读取位置传感器数据

(1)电角度
(2)电速度
(3)机械角度
(4)机械转速

（2）switch检查电机模式并运行不同模式的对应功能

（1）电机复位模式

操作：进入电机主菜单，通过串口或者CAN设置模式

（2）编码器校准模式

执行任务：
（1）驱动器使能
（2）检查相位顺序
（3）执行校准程序
（4）驱动器失能

（3）力矩控制模式【主要】

改变力矩模式
（1）复位FOC，测试积分器和其他回路的参数
（2）复位dq轴的电流值为0


进行力矩控制
（1）控制器回路超时并CAN通讯超时保护
		dq轴电流参考期望值设置为0，整个控制回路就没有电流了
		
（2）进行力矩控制
		用PD控制器融合用户设置的位置信息、速度信息、力矩信息，力控、位控、速控三个模式融合在一起

（3）运行FOC电流闭环

（4）电机参数配置选项提示模式

prefix
parameter
min
max
current value
CAN ID
CAN Master ID
Current Limit (A)
FW Current Limit (A)
CAN Timeout (cycles)

（5）编码器数据输出模式

SPI读取编码器三个数据
	Mechanical Angle
	Electrical Angle
	Raw

代码

void TIM1_UP_TIM10_IRQHandler(void)
{
    if (TIM1->SR & TIM_SR_UIF )
    {
        ADC1->CR2  |= 0x40000000;                   //¿ªÊ¼²ÉÓõçÁ÷²¢×ª»»£¬±íÕ÷ζÈÖµ
        Encoder_spi.Sample(DT);                     //²ÉÑùλÖô«¸ÐÆ÷
			
				///Çý¶¯Ð¾Æ¬µçÁ÷²ÉÑù¼ì²â ,¸øºóÃæÈ¡ÖµÓÃ///
        controller.adc2_raw = ADC2->DR;             //¶ÁÈ¡ADCÊý¾Ý¼Ä´æÆ÷£¬£¬ÔÚÇý¶¯Ð¾Æ¬ÉÏ¿ÉÒÔ¶Áµ½±íÕ÷µçÁ÷µÄµçѹֵ
        controller.adc1_raw = ADC1->DR;
        controller.adc3_raw = ADC3->DR;							//µç»úµçÔ´µçѹ
			
				///´Å±àÂëÆ÷£¨µç½Ç¶È¡¢»úе½Ç¶È£©Î»Öá¢Ëٶȼì²â ,¸øºóÃæÈ¡ÖµÓÃ///
        controller.theta_elec = Encoder_spi.GetElecPosition();   
        controller.theta_mech = (1.0f/GR)*Encoder_spi.GetMechPosition();
        controller.dtheta_mech = (1.0f/GR)*Encoder_spi.GetMechVelocity();  
        controller.dtheta_elec = Encoder_spi.GetElecVelocity();
			
			  ///¼ÆËãµç»úµçѹ£¬¹ýÂËÖ±Á÷Á´Â·µçѹ²âÁ¿Öµ///
        controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; 
        
        /// ¼ì²é״̬»ú״̬£¬²¢ÔËÐÐÊʵ±µÄº¯Êý ///
        switch(state)
        {
            case REST_MODE:                //½ö½ö½øÈë²Ëµ¥´®¿Ú´òÓ¡²ÎÊýÉèÖÃÑ¡Ïî¼°¸ü¸Ä£¬ÔÚ´®¿ÚÖжϽÓÊÕÖиü¸Ä
                if(state_change)   //ÔÚ¸´Î»Ä£Ê½Ï£¬¸ü¸Äµç»úģʽÐèÒª½øÈ¥²Ëµ¥ÉÏÖØÐÂÉèÖÃ
                {
                    enter_menu_state();
                }
                break;
            
            case CALIBRATION_MODE:        //ÔËÐбàÂëÆ÷У׼³ÌÐò
                if(state_change)		//ÔÚ±àÂëÆ÷УÕýģʽÏ£¬Èç¹û¸ü¸Äµç»ú״̬£¬ÐèÒªÖØÐÂУ׼
                {
                    calibrate();
                }
                break;
             
            case MOTOR_MODE:              //ÔËÐÐÁ¦¾Ø¿ØÖƳÌÐò
                if(state_change)	//ÔÚÁ¦¾Ø¿ØÖÆÕý³£ÔËÐÐģʽÏ£¬Èç¹û¸ü°®µç»ú״̬£¬¸´Î»FOCµÄ¸÷¸÷²ÎÊý
                {
                    enter_torque_mode();
                    count = 0;
                }
								//
                else
                {
                    /*
                    if(controller.v_bus>28.0f)   //Èç¹û×ÜÏߵĵçѹ¹ý¸ß£¬Ôòת¶¯Õ¤¼«Çý¶¯£¬ÒÔ·ÀÖ¹ÔÚÔÙÉú¹ý³ÌÖÐ×ÜÏß±»ÇжϷ¢ÉúFET³¡Ð§Ó¦¹Ü±¬Õ¨£¬ËãÊÇÒ»ÖÖ±£»¤
									     {        
                        gpio.
                        ->write(0);
                        controller.ovp_flag = 1;
                        state = REST_MODE;
                        state_change = 1;
                        printf("OVP Triggered!\n\r");
                        }
                     */  
                    
                    if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0))//¿ØÖÆÆ÷»Ø·³¬Ê±£¬²¢CANͨѶ³¬Ê±±£»¤£¬Ï൱ÓÚµç»ú²»Êä³ö£¬ËãÊÇÒ»ÖÖ±£»¤
                    {
                        controller.i_d_ref = 0;
                        controller.i_q_ref = 0;
                        controller.kp = 0;
                        controller.kd = 0;
                        controller.t_ff = 0;
                    } 

                    torque_control(&controller);																								//½øÐÐÁ¦¾Ø¿ØÖÆ£¨Öص㿴,focµÄÎļþ¼Ð£©
                    commutate(&controller, &observer, &gpio, controller.theta_elec);           // ÔËÐеçÁ÷±Õ»·

                    controller.timeout++;
                    count++; 
                }     
                break;
                
            case SETUP_MODE:										/*½øÈëģʽÉèÖÃÖ¸ÁÊý£¬½ö½öÖ»Óд®¿Ú´òÓ¡¹¦ÄÜ*/
                if(state_change)
                {
                    enter_setup_state();
                }
                break;
                
            case ENCODER_MODE:								//±àÂëµ÷ÊÔģʽ£¬½ö½öÊä³ö±àÂëÆ÷Êý¾Ý
                print_encoder();
                break;
            
            default:
                break;
        }                 
    }
    TIM1->SR = 0x00;                       // ¸´Î»×´Ì¬¼Ä´æÆ÷
}

.
.

三、力矩控制实现【相当于输入接口】

融合位置、速度、力矩控制的程序实现

1.公式

参考转矩=K*(设定位置-当前机械位置)+设定转矩+D*(设定速度-当前机械速度)

2.力控电机内部处理位控、速控、力控的闭环方式控制框图

在这里插入图片描述
同时能实现位置控制、速度控制和力矩控制的耦合输出。关节控制器采用了串 级 PID 的原理，具有电流/速度/位置闭环。在控制器的最内侧采用了 PI 控制器控制执行器的输入电流。【防盗标记–盒子君hzj】在外层则采用了带有力矩前馈的 PD 控制器。电流的PI 控制闭环通常由 电机驱动器内部实现。
（2）电机PD控制器的控制律为：

在这里插入图片描述

（1）Kp为比例增益
（2）Kd为微分增益
（3）tff 为电机的电流力矩常数(忽略腿部动力学通过加一个力矩常数来弥补)
（4）该控制器通过调节外环的比例增益Kp 与微分增益Kd 切换控制模式，【防盗标记–盒子君hzj】仅需将比例增益Kd与微分增益Kd 设置为 0，即可实现纯力矩控制
.
.

3.代码

void torque_control(ControllerStruct *controller)
{
	//Éú³É²Î¿¼µÄÁ¦¾Ø£º²Î¿¼Á¦¾ØÖµ=Kp(Óû§ÆÚÍûµÄλÖÃ-±àÂëÆ÷µÄ»úеλÖÃ)+Óû§ÉèÖõÄÁ¦¾ØÖµ+Kd(Óû§ÆÚÍûµÄËÙ¶È-±àÂëÆ÷µÄËÙ¶È)
    float torque_ref = controller->kp*(controller->p_des - controller->theta_mech) + controller->t_ff + controller->kd*(controller->v_des - controller->dtheta_mech);
  //¸ø¶¨qÖáµÄÏàµçÁ÷£¬Óòο¼µÄÁ¦¾Ø¸ø¶¨ 
		controller->i_q_ref = torque_ref/KT_OUT;    //i_q_refÓëת¾ØÓйأ¬Éú³ÉqÖáµÄÏàµçÁ÷=²Î¿¼Á¦¾Ø/dqÖáµÈЧµÄµçÁ÷
  //¸ø¶¨dÖáµÄÏàµçÁ÷    
		controller->i_d_ref = 0.0f;									//i_d_refÓë´ÅͨÓйØ
}

四、FOC算法电流闭环实现【核心】

1.FOC整体架构图

在这里插入图片描述

2.代码解析

以40 kHz的环路速率控制电机转矩，电机控制FOC回路频率使40Khz,即0.25ms一个周期，程序上用40khz的定时器实现的

（1）第一步：驱动器IC用ADC测量三相电流【IC电流转电压ADC两相，基尔霍夫计算第三相】

（2）第二步：DQ0变换（clark变换+park变换）

（1）clark变换：三相定子坐标系-两相的定子直角坐标系α-β：
        将Ia,Ib,Ic经过Clark变换得到Iα,Iβ

（2）park变换：两相定子坐标系α-β变换到两相转子坐标系 d-q：
       将Iα,Iβ ​经过Park变换得到​ Iq,Id
		Iq与转矩有关
		Id与磁通有关，在实际应用中常常将Id置为0
		Iq,Id这两个量不是时变的，因此可以但对对这两个量进行单独控制

（3）弱磁控制目的：改善电机的最大性能

（3）第三步：PI控制器：计算dp轴电流Iq,Id;i_d_ref、i_q_ref，经过PI控制器得到输出Vq\Vd，并对进行线性化

（1）Iq,Id通过DQ变换得到
（2）i_d_ref、i_q_ref通过PD控制器生成，并融合位置控制，速度控制和力矩控制【通过用户给定位置、速度、力矩指令】
（3）Vq\Vd线性化

（4）第四步：反DQ0变换（反clark变换+反park变换）

（0）磁编码器采集转子机械位置
	编码器获取电角与机械角

（1）反park变换：将Uq,Ud进行反Park变换得到​ Uα,Uβ
（2）反clark变换：将Uα,Uβ变换得到v_w、v_v、v_u

（5）第五步：空间矢量调制、三相逆变使得正弦波转换成为PWM波

(1)矢量空间控制SVM生成dtc_v、dtc_u、dtc_w，用来给定时器产生PWM
(2)PWM输入驱动器IC，产生SPWM触发三相逆变电路

.

3.源码

总的源码流程

void commutate(ControllerStruct *controller, ObserverStruct *observer, GPIOStruct *gpio, float theta)
	{       
        /// ¸üÐÂζȹ۲âÆ÷¹À¼ÆÖµ ///
        // ζȹ۲âÆ÷//
        float t_rise = (float)observer->temperature - 25.0f;			//25¶ÈÊDZê×¼²Î¿¼Î¶ȣ¬ÎÂÉý=ADCͨ¹ýÎÂ×èÏß¼ÆËãµÄζÈ-±ê×¼²Î¿¼Î¶È
        float q_th_in = (1.0f + .00393f*t_rise)*(controller->i_d*controller->i_d*R_PHASE*SQRT3 + controller->i_q*controller->i_q*R_PHASE*SQRT3);
        float q_th_out = t_rise*R_TH;
        observer->temperature += INV_M_TH*DT*(q_th_in-q_th_out);
				// ÎÂ×迹×迹¹Û²âÆ÷ //
        observer->resistance = (controller->v_q - SQRT3*controller->dtheta_elec*(WB))/controller->i_q;
        if(isnan(observer->resistance)){observer->resistance = R_PHASE;}
        observer->temperature2 = (double)(25.0f + ((observer->resistance*6.0606f)-1.0f)*275.5f);
        double e = observer->temperature - observer->temperature2;
        observer->temperature -= .001*e;

				
				
       ///µçÁ÷±Õ»·//
       controller->loop_count ++;   
	/*£¨µÚÒ»²½£©²âÁ¿ÈýÏàµçÁ÷*/
       if(PHASE_ORDER)  //ÕýÏòʱ£¬ ¼ì²éµçÁ÷´«¸ÐÆ÷µÄÃüÁȷ¶¨VUÏàλ˳Ðò
				   {                                                                        
           controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);    // ¸ù¾ÝADCµÄ¶ÁÊý¼ÆËãBÏàµçÁ÷
           controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);		 // ¸ù¾ÝADCµÄ¶ÁÊý¼ÆËãCÏàµçÁ÷
           }
        else					 //·´Ïòʱ
					 {
            controller->i_b = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);    
            controller->i_c = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);
           }
						controller->i_a = -controller->i_b - controller->i_c;    															 	//¸ù¾Ý»ù¶û»ô·ò¼ÆËãAÏàµçÁ÷   
       
	/*£¨µÚ¶þ²½£©¡¾ÖØÒª¡¿DQ0±ä»» (park±ä»»+clark±ä»»)*/	 
       float s = FastSin(theta); 
       float c = FastCos(theta);                            
       dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q);    //µçÁ÷dq±ä»»£¨park±ä»»+clark±ä»»£©
				
					 
	/*£¨µÚÈý²½£©¼ÆËã²Î¿¼µÄdqÖáµçÁ÷i_d_ref¡¢i_q_ref*/	
        controller->i_q_filt = 0.95f*controller->i_q_filt + 0.05f*controller->i_q;
        controller->i_d_filt = 0.95f*controller->i_d_filt + 0.05f*controller->i_d;
        //¼ÓÈëalphaµçÁ÷½Ç¶È
        controller->i_d_ref_filt = (1.0f-controller->alpha)*controller->i_d_ref_filt + controller->alpha*controller->i_d_ref;
        controller->i_q_ref_filt = (1.0f-controller->alpha)*controller->i_q_ref_filt + controller->alpha*controller->i_q_ref;
       /// Èõ´ÅÖÐ ///
       controller->fw_int += .001f*(0.5f*OVERMODULATION*controller->v_bus - controller->v_ref);
       controller->fw_int = fmaxf(fminf(controller->fw_int, 0.0f), -I_FW_MAX);
       controller->i_d_ref = controller->fw_int;
       limit_norm(&controller->i_d_ref, &controller->i_q_ref, I_MAX);//µçÁ÷ÏÞ·ùÊä³ö£¬Ä¿µÄÊÇʹÆäÊä³öÖµÔÚµçÔ´µçÁ÷I_MAXΪ°ë¾¶µÄԲȦÄÚ
       
       
       
 /*£¨µÚËIJ½£©PI ¿ØÖÆÆ÷£¬ÊäÈëi_d_refºÍi_d£¬Êä³öv_d¡¢v_q*/
			 //£¨1£©¼ÆËãPI¿ØÖÆÆ÷Îó²î
       float i_d_error = controller->i_d_ref - controller->i_d;
       float i_q_error = controller->i_q_ref - controller->i_q;//  + cogging_current;
	  	 //¼ÆËãÇ°À¡µçѹ£¬Ç°À¡µçѹ=ÏàµçÁ÷*Ïàµç×è//
			 // float v_d_ff = SQRT3*(1.0f*controller->i_d_ref*R_PHASE  - controller->dtheta_elec*L_Q*controller->i_q);   //feed-forward voltages
			 // float v_q_ff =  SQRT3*(1.0f*controller->i_q_ref*R_PHASE +  controller->dtheta_elec*(L_D*controller->i_d + 1.0f*WB));
       
       //£¨2£©¼ÆËãPI¿ØÖÆÆ÷ÔöÒæP¡¢»ý·ÖÎó²îI//
       controller->d_int += controller->k_d*controller->ki_d*i_d_error;  //ki_dÈÏΪÊÇdÖáµçÁ÷ÔöÒ棬k_d²ÅÊǵçÁ÷»·ÔöÒæ
       controller->q_int += controller->k_q*controller->ki_q*i_q_error;	 //ki_qÈÏΪʹqÖáµçÁ÷ÔöÒ棬k_q²ÅÊǵçÁ÷»·ÔöÒæ
       controller->d_int = fmaxf(fminf(controller->d_int, OVERMODULATION*controller->v_bus), - OVERMODULATION*controller->v_bus);
       controller->q_int = fmaxf(fminf(controller->q_int, OVERMODULATION*controller->v_bus), - OVERMODULATION*controller->v_bus); 
       controller->v_d = controller->k_d*i_d_error + controller->d_int ;//+ v_d_ff;  
       controller->v_q = controller->k_q*i_q_error + controller->q_int ;//+ v_q_ff; 
			 
       // £¨3£©¼ÆËã²Î¿¼µçѹ£¬¶Ôv_d¡¢v_q½øÐÐÏßÐÔ»¯
       controller->v_ref = sqrt(controller->v_d*controller->v_d + controller->v_q*controller->v_q);
       limit_norm(&controller->v_d, &controller->v_q, OVERMODULATION*controller->v_bus);       //µçѹÏÞ·ùÊä³ö£¬Ä¿µÄÊÇʹÆäÊä³öÖµÔÚµçÔ´µçѹv_busΪ°ë¾¶µÄԲȦÄÚ
       float dtc_d = controller->v_d/controller->v_bus;//dtc_d½ö½öʹһ¸ö±ÈÖµ£¬ÓÃÓÚÏßÐÔ»¯
       float dtc_q = controller->v_q/controller->v_bus;
       linearize_dtc(&dtc_d);													 //ʹÄæ±äÆ÷µÄÊä³öÏßÐÔ»¯
       linearize_dtc(&dtc_q);
       controller->v_d = dtc_d*controller->v_bus;			 //dÖáµçѹ
       controller->v_q = dtc_q*controller->v_bus;			 //qÖáµçѹ
			 
 /*£¨µÚÎå²½£©¡¾ÖØÒª¡¿ÄæDQ0±ä»» (Äæpark±ä»»+Äæclark±ä»»)*/			 			 
       abc(controller->theta_elec + 0.0f*DT*controller->dtheta_elec, controller->v_d, controller->v_q, &controller->v_u, &controller->v_v, &controller->v_w); //µçѹÄædq±ä»»
			 
 /*£¨µÚÁù²½£© ¿Õ¼äʸÁ¿µ÷ÖÆ £¬Õâ¸öʹÕýÏÒ²¨×ª»»³ÉPWM²¨µÄ*/			 
       svm(controller->v_bus, controller->v_u, controller->v_v, controller->v_w, &controller->dtc_u, &controller->dtc_v, &controller->dtc_w); //¿Õ¼äʸÁ¿µ÷ÖÆ
       if(PHASE_ORDER)													 							// ¼ì²éUVÏà˳Ðò
				 {                                                        
            TIM1->CCR3 = (PWM_ARR)*(1.0f-controller->dtc_u);  //дPWMµÄÕ¼¿Õ±È
            TIM1->CCR2 = (PWM_ARR)*(1.0f-controller->dtc_v);
            TIM1->CCR1 = (PWM_ARR)*(1.0f-controller->dtc_w);
        }
        else{
            TIM1->CCR3 = (PWM_ARR)*(1.0f-controller->dtc_u);
            TIM1->CCR1 = (PWM_ARR)*(1.0f-controller->dtc_v);
            TIM1->CCR2 =  (PWM_ARR)*(1.0f-controller->dtc_w);
        }
       controller->theta_elec = theta;   //¼Ç¼µç½Ç¶È                                       
    }

.
.

（1）反dq变换（反clark变换）函数： abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w)

输入的是dq轴上的角度、d轴的电压值v_d、q轴的电压值v_q
输出的是三相电流振幅v_u、v_v、v_w

void abc( float theta, float d, float q, float *a, float *b, float *c)
{
    float cf = FastCos(theta);
    float sf = FastSin(theta);
    
    *a = cf*d - sf*q;                // Faster Inverse DQ0 transform
    *b = (0.86602540378f*sf-.5f*cf)*d - (-0.86602540378f*cf-.5f*sf)*q;
    *c = (-0.86602540378f*sf-.5f*cf)*d - (0.86602540378f*cf-.5f*sf)*q;
}

.
.

（2）空间矢量控制函数：svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w)

输入的是电源电压v_bus、三相电压振幅uvw
输出的三相电压矢量dtc_u、dtc_v、dtc_w
实质也是理论公式转换而已

void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w)
{
    float v_offset = (fminf3(u, v, w) + fmaxf3(u, v, w))*0.5f;
    
    *dtc_u = fminf(fmaxf(((u -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX);
    *dtc_v = fminf(fmaxf(((v -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX);
    *dtc_w = fminf(fmaxf(((w -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX); 
    
    /*
    sinusoidal pwm
    *dtc_u = fminf(fmaxf((u/v_bus + .5f), DTC_MIN), DTC_MAX);
    *dtc_v = fminf(fmaxf((v/v_bus + .5f), DTC_MIN), DTC_MAX);
    *dtc_w = fminf(fmaxf((w/v_bus + .5f), DTC_MIN), DTC_MAX);
    */    
}

.
.

（3）dq变换（反clark变换）函数：dq0(float theta, float a, float b, float c, float *d, float *q)

输入的是三相电流abc，dq轴电角度theta
输出的是dq轴上的两个电流分量d,q

void dq0(float theta, float a, float b, float c, float *d, float *q)
{    
    float cf = FastCos(theta);
    float sf = FastSin(theta);
    
    *d = 0.6666667f*(cf*a + (0.86602540378f*sf-.5f*cf)*b + (-0.86602540378f*sf-.5f*cf)*c);   ///Faster DQ0 Transform
    *q = 0.6666667f*(-sf*a - (-0.86602540378f*cf-.5f*sf)*b - (0.86602540378f*cf-.5f*sf)*c);     
}

.
.

五、编码器位置角度校正及线性化

（1）位置传感器度的数据用处

（1）用于FOC

 1）计算转子的电气角度
  2）计算电机换向

（2）向主设备输出反馈电机位置信息

（2）校正程序被调用的入口

电机运行时，在程序定时器中断上（40KHZ），微控制器通过SPI读取位置传感器的输出

电机初始化时，串口标志位仅仅执行一次校正和线性化程序

（3）编码器校正

（1）编码器校准解决的问题

（1）每个电机出厂时UVW三相焊接顺序不一致的问题
一条腿上有三个电机，我们无法保证三个电机定子WUV三条线的连接顺序是一样的，在电机制作的的时候，WUV三条线可以随意焊接，经过校正后就不影响使用了

（2）编码器的零位置将与转子的d轴不对齐的问题【线性同一平面但是有相对位移的误差】
理想情况下，编码器的零位置将与转子的d轴对齐，但是因为转子上的永磁体和位置传感器安装使手动的，不可能那么精确，所以编码器的零位置不完全于转子的d轴对齐，所以需要校正编码器（对编码器做标定）

（3）电机机械部分带来的阻尼的问题
因为电机是内嵌了行星减速箱的，对于FOC会带来阻尼，使电流矢量产生误差，因为每个电机的阻尼多少不一样，电机长时间使用后，阻尼也会变化

（2）磁编码器校正的过程

（1）为了确定电机相位的顺序（UWV）

做法：【利用UVW电压输出相序和磁编码器的旋转方向关系】
定子电压矢量沿正方向缓慢旋转。如果位置传感器的输出增加，相位排序是正确的。如果位置传感器的输出减小，则在程序中交换两个电机相位

确定电机相位的函数：
order_phases(&Encoder_spi, &gpio, &controller, &prefs);

void order_phases(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs)
	{   
   //³õʼ»¯µç»úFOC¿ØÖƵıäÁ¿  
    printf("\n\r Checking phase ordering\n\r");
    float theta_ref = 0;					//¶¨ÒåÒ»¸ö²Î¿¼µÄtheta½Ç¶È
    float theta_actual = 0;				//¶¨ÒåÒ»¸öʵ¼ÊµÄtheta½Ç¶È
    float v_d = V_CAL;  					//°ÑËùÓеĵçѹ·ÅÔÚDÖáÉÏ
    float v_q = 0.0f;							//°ÑqÖáµÄµçѹÖÃ0
    float v_u, v_v, v_w = 0;			//°ÑÈýÏàÕñ·ùÖÃ0£¨±êÁ¿£©
    float dtc_u, dtc_v, dtc_w = .5f;//°ÑÈýÏàµÄʸÁ¿µçѹÖÃ0
    int sample_counter = 0;				//°Ñ²ÉÓõļÆÊýÖµÖÃ0
   ///	
		
   
    ///½«µçѹ½ÇÉèÖÃΪÁ㣬¼´ÉèÖÃת¶¯Ð£ÕýÇ°µç»úµÄλÖã¬Ò»´Î¾ÍºÃ
    abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);          //µçѹ·´dq0±ä»»£¬Í¨¹ýÉÏÃæ¸ø¶¨µÄdq×ø±êϵϵĵçѹºÍ½Ç¶È£¬¼ÆËã³öÈýÏàµÄÕñ·ù
    svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);     //¿Õ¼äʸÁ¿µ÷ÖÆ£¬Í¨¹ýµçÔ´µçѹºÍÈýÏàÕñ·ù£¬¼ÆËã³öÈýÏà¶ÔÓ¦µÄµçѹʸÁ¿ 
		for(int i = 0; i<20000; i++)												 //ÉèÖóõʼռ¿Õ±È£¬¿ªÊ¼¿ØÖƵç»úÁË£¬Õâһ˲¼äµç»úÊÇËøËÀµÄ
			{
			TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);                                
			TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
			TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
			wait_us(100);
			}
		//
    ps->Sample(DT); 																		//ͨ¹ý´Å±àÂëÆ÷¶ÔÏÖÔÚµÄ	ת×ÓλÖýøÐвÉÑù
    wait_us(1000);
		float theta_start;	 //´Å±àÂëÆ÷»ñÈ¡¾ø¶ÔµÄ»úе½Ç¶È£¬×ʼµÄʱºò£¬¶¨ÒåµÄʱºò¾ÉĬÈÏΪ0ÁË
    //float theta_start = ps->GetMechPositionFixed();                                  //»ñÈ¡³õʼ»¯×ª×ÓλÖÃ
		//	
    controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);    //¸ù¾ÝADCµÄ¶ÁÊý¼ÆËãÏàµçÁ÷
    controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);
    controller->i_a = -controller->i_b - controller->i_c;					//¸ù¾Ý»ù¶û»ô·òµçÁ÷¶¨ÂÉ£¬ÖªµÀÁ½ÏàµçÁ÷¿ÉÒÔÇóµÚÈýÏàµçÁ÷
	 
			//µçÁ÷dq0±ä»»£¬µÃµ½dqÖáÉϵÄÁ½¸öµçÁ÷·ÖÁ¿
    dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q);   
    float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2));	//¼ÆËã³ödqÖáÉϵÄÁ½¸öµçÁ÷·ÖÁ¿ºÏ³ÉµÄ×ܵçÁ÷
    printf("\n\rCurrent\n\r");
    printf("%f    %f   %f\n\r\n\r", controller->i_d, controller->i_q, current);
		///

			
    /// ¿ØÖÆÐýתµçѹ½Ç¶È£¬³ÖÐøÈÃÈýÏàµçѹʸÁ¿ÑØ×ÅÕý·½Ïòת¶¯£¬±ßת¶¯£¬±ß¼ÆËãºÍÊä³öʵ¼ÊµÄ»úе½Ç¶È
    while(theta_ref < 4*PI)//ÐýתËÄȦ£¬Êµ¼ÊÉϵç½Ç¶ÈÐýת6Ȧ£¬»úе½Ç²ÅÐýתһȦµÄ
			{                                                    //ÐýתÁ½¸öµçÖÜÆÚ
        abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);                             //µçѹ·´dq0±ä»»
        svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w);                        //¿Õ¼äʸÁ¿µ÷ÖÆ
        wait_us(100);
        TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);                                        //ÉèÖÃÕ¼¿Õ±È
        TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
        TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
       ps->Sample(DT);                                                            //²É¼¯Î»Öô«¸ÐÆ÷
       theta_actual = ps->GetMechPositionFixed();
       if(theta_ref==0){theta_start = theta_actual;}
       if(sample_counter > 200)
				{
           sample_counter = 0 ;
        printf("%.4f   %.4f\n\r", theta_ref/(NPP), theta_actual);
        }
        sample_counter++;
       theta_ref += 0.001f;
       }
		///	

    float theta_end = ps->GetMechPositionFixed(); //´Å±àÂëÆ÷»ñÈ¡¾ø¶ÔµÄ»úе½Ç¶È£¬×îºóµÄʱºò	 
    int direction = (theta_end - theta_start)>0;  //Åжϵç»úת¶¯µÄ·½Ïò£¬ÕâÀïͨ¹ýÅжÏÔËËã·û£¬Êä³öµÄÊDz¼¶ûÖµ
    printf("Theta Start:   %f    Theta End:  %f\n\r", theta_start, theta_end);
    printf("Direction:  %d\n\r", direction);
    if(direction){printf("Phasing correct\n\r");}
    else if(!direction){printf("Phasing incorrect.  Swapping phases V and W\n\r");}
    PHASE_ORDER = direction;      //¸ü¸ÄÏàλ·½Ïò˳Ðò
    }

.

（2）为了确定位置传感器偏移（消除磁编码器、转子安装误差）

做法：【利用UVW输入的角度和编码器输出的转子位置角度的线性关系】
定子UVW三相输出电压矢量，缓慢地扫过电机360度，磁编码器同时记录所得到的转子位置，从变化的转子位置和电压矢量输出的量来判断电机传感器的位置安装偏移
通过测量转子角度和电压角系列之间的差值， 减去直流偏移，并对前进和向后旋转方向进行时效处理

.

（3）消除摩擦效应（齿槽转矩和摩擦力偏移）

做法：【利用齿槽阻尼和D轴电流矢量的关系】
在该扫描期间，转子D轴跟踪定子电流矢量，该定子电流矢量是最低能量位置。由于齿槽转矩和摩擦力，电流矢量的角度与转子的角度之间存在一定的跟踪误差。 位置敏感磁铁的直流偏移是平均角度差
用移动时采集的位置样本加权平均。即等权重的FIR滤波器

.

（4）编码器线性化

编码器线性化解决的问题

【非线性不在同一平面的误差】
磁编码器IC是焊接在PCB板子上的，由于焊接的偏差和pcb板子安装的偏差，还有永磁体安装的偏差，在磁编码器采样的时候回引入非线性，这种非线性是人为造成的，因为磁编码器和永磁体磁场不在同一平面，会导致编码器输出的信号变形，引入错误的位置偏差。所以要进行编码器的线性化

.

编码器线性化过程

做法：【因为这个非线性测量一圈就能确定，并且不会改变的性质】
误差信号用作查找表，使位置传感器的输出线性化

.
.

（5）代码

void calibrate(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs)
	{
		///¶¨ÒåУÕý³ÌÐòµÄ±äÁ¿//
    printf("Starting calibration procedure\n\r");
    float * error_f;															  //ÕýÐýת¼Ç¼µÄÐýת½ÇÆ«ÒÆÁ¿
    float * error_b;																//·´Ðýת¼Ç¼µÄÐýת½ÇÆ«ÒÆÁ¿
    int * lut;																			//²é±íÓõıäÁ¿
    int * raw_f;																		//Ç°½ø·½Ïòת×ӵĽǶÈ
    int * raw_b;																		//ºóÍË·½Ïòת×ӵĽǶÈ
    float * error;																	//ÐýתÁ¿×ÜÆ«²î
    float * error_filt; 														//УÕýºóÐýתÁ¿×ÜÆ«²î	
    const int n = 128*NPP;                          // ÿ´Î»úеÐýת½Ç¶È²ÉÑùµÄλÖÃÊýÁ¿
    const int n2 = 40;                              // ±£´æ²ÉÑùÖ®¼äµÄÔöÁ¿£¨ÓÃÓÚƽ»¬Ô˶¯£©
    float delta = 2*PI*NPP/(n*n2);                  // ²ÉÑùÊý¾ÝµÄ½Ç¶È±ä»¯
    error_f = new float[n]();                       // ¸³ÖµÎó²îʸÁ¿ÏòÇ°Ðýת
    error_b = new float[n]();                       // ¸³ÖµÎó²îʸÁ¿ÏòºóÐýת
    const int  n_lut = 128;													//¸³Öµ²é±í±éÀú±äÁ¿	
    lut = new int[n_lut]();                         // ÔÚ¿ªÊ¼Ç°£¬Çå³ýËùÓо͵òéÔıí¸ñ
    error = new float[n]();
    const int window = 128;
    error_filt = new float[n]();
    ps->WriteLUT(lut); 
    raw_f = new int[n]();
    raw_b = new int[n]();
    float theta_ref = 0;														//²Î¿¼µÄtheta½Ç
    float theta_actual = 0;													//ÕæʵµÄtheta½Ç
    float v_d = V_CAL;                              //°ÑËùÓеĵçѹ·ÅÔÚDÖáÉÏ
    float v_q = 0.0f;																//ÇåÁãQÖáµçѹ
    float v_u, v_v, v_w = 0;												//ÈýÏàÏßµçѹ
    float dtc_u, dtc_v, dtc_w = .5f;								//ÈýÏàÏßµçÁ÷£¨²»Çå³þ£©
    
    ///½«µçѹ½ÇÉèÖÃΪÁ㣬µÈ´ýת×ÓλÖÃÎȶ¨£¬½ö½ö×öÒ»´Î/
    abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);      //µçѹ·´dq0±ä»»
    svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); //¿Õ¼äʸÁ¿µ÷ÖÆ
    for(int i = 0; i<40000; i++)										 //ÉèÖÃÕ¼¿Õ±È 
		{
			TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);        //¸ø¶¨VÏàµÄPWM                        
			if(PHASE_ORDER)																 //֮ǰÒѾ­¼ì²éÁËUV˳ÐòµÄ£¬°´×ÅÈ·¶¨µÄ˳Ðò¸øµçѹÐźÅ
				{                                   
					TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);		 //¸ø¶¨VÏàµÄPWM
					TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);		 //¸ø¶¨WÏàµÄPWM
				}
			else
				{
					TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);		 //¸ø¶¨VÏàµÄPWM
					TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);    //¸ø¶¨WÏàµÄPWM
				}
			wait_us(100);
		}
    ps->Sample(DT); 																//´Å±àÂëÆ÷  ¿ªÊ¼²ÉÑù
    controller->i_b = I_SCALE*(float)(controller->adc2_raw - controller->adc2_offset);     //¸ù¾ÝADCµÄ¶ÁÊý¼ÆËãÏàµçÁ÷B,ͨ¹ý£¨ÊµÊ±ADC²É¼¯Çý¶¯ICµÄµçÁ÷¼ÆËã-УÕýºóµÄµçÁ÷¼ÆÊý£©*µ¥Î»¼ÆËã¶ÔÓ¦µÄµçÁ÷£=ʵ¼ÊµÄµçÁ÷
    controller->i_c = I_SCALE*(float)(controller->adc1_raw - controller->adc1_offset);		 //¸ù¾ÝADCµÄ¶ÁÊý¼ÆËãÏàµçÁ÷C
    controller->i_a = -controller->i_b - controller->i_c;																	 //¸ù¾ÝADCµÄ¶ÁÊý¼ÆËãÏàµçÁ÷A
    dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q);   //µçÁ÷dq0±ä»»
    float current = sqrt(pow(controller->i_d, 2) + pow(controller->i_q, 2));							 //¼ÆËã×ܵçÁ÷
    printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r");
		
		
		
		///ÏòÇ°Ðýת//
    for(int i = 0; i<n; i++)		 												// ÿ´Î»úеÐýת½Ç¶È²ÉÑùµÄλÖÃÊýÁ¿
		{                                                   
       for(int j = 0; j<n2; j++) 												// ÏòÇ°Ðýת£¬·¢ÄÇô¶à´ÎPWMÊÇΪÁË¿ØÖÆÏìӦƽ»¬Ò»µã
			{   
       theta_ref += delta;			 												// ²Î¿¼µÄµç½Ç¶È°´ÔöÁ¿µÝÔö
       abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);			//µçѹ·´dq0±ä»»
       svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); //¿Õ¼äʸÁ¿µ÷ÖÆ
        TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
        if(PHASE_ORDER){                                //¼ì²éÏàÐò˳Ðò 
            TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);     // ÉèÖÃÕ¼¿Õ±È
            TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
            }
        else{
            TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
            TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
            }
            wait_us(100);
            ps->Sample(DT);
        }
       ps->Sample(DT);
       theta_actual = ps->GetMechPositionFixed();
       error_f[i] = theta_ref/NPP - theta_actual;		//¼ÆËã¸ø¶¨½Ç¶ÈºÍÐýת½Ç¶ÈµÄÆ«²î£¬È·¶¨Î»Öô«¸ÐÆ÷µÄÆ«ÒÆ
       raw_f[i] = ps->GetRawPosition();							//±àÂëÆ÷¼Ç¼µÄλÖÃ
        printf("%.4f   %.4f    %d\n\r", theta_ref/(NPP), theta_actual, raw_f[i]);//´òÓ¡¸ø¶¨µÄ»úе½Ç¶È
    }

	///ÏòºóÐýת»ñÈ¡///	
    for(int i = 0; i<n; i++)														// ÿ´Î»úеÐýת½Ç¶È²ÉÑùµÄλÖÃÊýÁ¿							
		{                                                   
			 for(int j = 0; j<n2; j++)								  			// ÏòºóÐýת
			{
			 theta_ref -= delta;														  // ²Î¿¼µÄµç½Ç¶È°´ÔöÁ¿µÝÔö
			 abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w);      //µçѹ·´dq0±ä»»
			 svm(1.0, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); //¿Õ¼äʸÁ¿µ÷ÖÆ
				TIM1->CCR3 = (PWM_ARR>>1)*(1.0f-dtc_u);
				if(PHASE_ORDER)
						{
						TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_v);
						TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_w);
						}
				else{
						TIM1->CCR1 = (PWM_ARR>>1)*(1.0f-dtc_v);
						TIM1->CCR2 = (PWM_ARR>>1)*(1.0f-dtc_w);
						}
						wait_us(100);
						ps->Sample(DT);
				}
			 ps->Sample(DT);                                  //²ÉÓÃλÖô«¸ÐÆ÷
			 theta_actual = ps->GetMechPositionFixed();       // »ñÈ¡»úе½Ç¶È
			 error_b[i] = theta_ref/NPP - theta_actual;				//¼ÆËã¸ø¶¨½Ç¶ÈºÍÐýת½Ç¶ÈµÄÆ«²î£¬È·¶¨Î»Öô«¸ÐÆ÷µÄÆ«ÒÆ
			 raw_b[i] = ps->GetRawPosition();
			 printf("%.4f   %.4f    %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]);
			 //theta_ref -= delta;
		}    
 	¼ÆËãƽ¾ùλÖô«¸ÐÆ÷Æ«ÒƲ¢ÉèÖôűàÂëÆ÷µÄÆ«ÒƼĴæÆ÷//	
        float offset = 0;                                  
        for(int i = 0; i<n; i++)
					{
					offset += (error_f[i] + error_b[n-1-i])/(2.0f*n);                   // ¼ÆËãƽ¾ùλÖô«¸ÐÆ÷Æ«ÒÆ
					}
        offset = fmod(offset*NPP, 2*PI);                                    // ½«»úе½Ç¶Èת»»ÎªµçÆø½Ç¶È   
        ps->SetElecOffset(offset);                                          // ÉèÖÃλÖô«¸ÐÆ÷Æ«ÒÆ
        __float_reg[0] = offset;
        E_OFFSET = offset;
        
	/// ÒÔÏßÐÔ»¯Î»Öô«¸ÐÆ÷Æ«ÐÄÂÊÖ´ÐÐÂ˲¨
	/// nλ²ÉÑùƽ¾ùÖµ£¬ÆäÖÐn=Ò»¸öµçÑ­»·ÖеÃÑùÆ·ÊýÁ¿
	///¸ÃÂ˲¨Æ÷ÔÚµãƵÂÊÏÂÔöÒæΪ0£¬ÇÒ¾ùΪÕû±¶Êý£¬ËùÒԳݲÛЧӦӦ¸Ã»Ø±ÜÍêÈ«Â˵ô
        float mean = 0;
        for (int i = 0; i<n; i++)   //ƽ¾ùÇ°ºóÁ½¸ö·½Ïò
					{                                              
            error[i] = 0.5f*(error_f[i] + error_b[n-i-1]);
          }
					
				//Â˲¨	
        for (int i = 0; i<n; i++)
					{
            for(int j = 0; j<window; j++)
						{
                int ind = -window/2 + j + i;                                    // Indexes from -window/2 to + window/2
                if(ind<0)
									{ind += n;}                                                  //Òƶ¯Æ½¾ùÈÆÏß 
                else if(ind > n-1) 
									{ind -= n;}
                error_filt[i] += error[ind]/(float)window;
            }
            if(i<window){
//                cogging_current[i] = current*sinf((error[i] - error_filt[i])*NPP);
                }
            //printf("%.4f   %4f    %.4f   %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]);
            mean += error_filt[i]/n;
           }
        int raw_offset = (raw_f[0] + raw_b[n-1])/2;                             //¶ÔÕâ¸ö·½Ïò´íÎó²»Ãô¸Ð£¬ËùÒÔÁ½·Ö¾Í×ã¹»ÁË
        
       // //Éú³ÉÏßÐÔ»¯²éÕÒ±í
        printf("\n\r Encoder non-linearity compensation table\n\r");
        printf(" Sample Number : Lookup Index : Lookup Value\n\r\n\r");
        for (int i = 0; i<n_lut; i++)
					 {                                          //Éú³É²éÕÒ±í
            int ind = (raw_offset>>7) + i;
            if(ind > (n_lut-1)){ 
                ind -= n_lut;
                }
            lut[ind] = (int) ((error_filt[i*NPP] - mean)*(float)(ps->GetCPR())/(2.0f*PI));
            printf("%d   %d   %d \n\r", i, ind, lut[ind]);
            wait(.001);
            }
            
        ps->WriteLUT(lut);                                                      // ½«²éÕÒ±íдÈëλÖô«¸ÐÆ÷¶ÔÏó
        memcpy(&ENCODER_LUT, lut, sizeof(lut));                                 // ½«²éÕÒ±í¸´ÖƵ½ÉÁ´æÕóÁÐ
        printf("\n\rEncoder Electrical Offset (rad) %f\n\r",  offset);
        //±£´æµ½ÉÁ´æ
        if (!prefs->ready()) prefs->open();
        prefs->flush();                                                         // ½«Æ«ÒÆÁ¿ºÍ²éÕÒ±íдÈëÉÁ´æ
        prefs->close();
        
        delete[] error_f;       //±ØÐëÊÍ·ÅÉÁ´æ
        delete[] error_b;
        delete[] lut;
        delete[] raw_f;
        delete[] raw_b;

    }

.
.

六、电机通讯及内容

STM32作为电机控制器，只运行FOC（电流环）算法并用CAN总线与SPIne通信，多圈位置解算，PD+feedfoward都放到了驱动里

下发信息

力矩、位置、速度及位置和速度的增益
.

反馈信号

位置、速度以及基于电流环测量的扭矩
.
.

总结

本工程我是用keil的单片机环境写的，注释不像vscode这样，拉上上就变成乱码了，但是我都尽量对应源码写了代码解析了，凑合看看

参考资料

【硬件】Eagle示意图、电路板布局、Gerbers和BOM
https://github.com/bgkatz/3phase integrated/tree/master/

【执行文件】电机驱动器和USB-CAN适配器的固件二进制版本
https://drive.google.com/drive/folders/16QltyOvweEwSLUS5kKFIQCJXjOVAnlm32us p=sharing

【固件源码】固件
https://os.mbed.com/users/benkatz/code/Hobbyking Cheetah/

电路板的硬件芯片清单
https://github.com/bgkatz/USBtoCAN/tree/master/eagle%20ffles

Python API接口的固件
https://os.mbed.com/users/benkatz/code/CanMastei7

在此扩展的例子
https://os.mbed.com/users/benkatz/code/MotorModuleExample/

版权声明：本文为CSDN博主「盒子君~」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/qq_35635374/article/details/121990836

标签： · · · · · ·
生成海报
点击下载海报
加载中
点赞 0

盒子君~

我还没有学会写个人说明！

暂无评论

相关推荐

趣聊51之串口通信（概念篇）
STM32 2024-03-14

趣聊51之串口通信（概念篇）

对于刚刚接触单片机的同学们来说，串口通信似乎是一个神秘感十足的东西，笔者在刚刚开始学习51单片机时，读的是郭天祥先生的那本著名的《新概念51单片机教程》，贼厚的一本书，但是等

分享到：