参考开源闭环步进电机驱动：XDrive
个人软硬件仓库地址：https://gitee.com/qlexcel/closed-loop-stepper-motor
开环步进电机
丢步概念
步进电动机正常工作时，每接收一个控制脉冲就移动一个步距角，即前进一步。若连续地输入控制脉冲，电动机就相应地连续转动。
步进电动机失步包括丢步和越步。丢步时，转子前进的步数小于脉冲数；越步时，转子前进的步数多于脉冲数。一次丢步和越步的步距数等于运行拍数的整数倍。丢步严重时，将使转子停留在一个位置上或围绕一个位置振动，越步严重时，机床将发生过冲。
丢步原因及策略
转子的加速度慢于步进电动机的旋转磁场
解释：
转子的加速度慢于步进电动机的旋转磁场，即低于换相速度时，步进电动机会产生失步。这是因为输入电动机的电能不足，在步进电动机中产生的同步力矩无法使转子速度跟随定子磁场的旋转速度，从而引起失步。由于步进电动机的动态输出转矩随着连续运行频率的上升而降低，因而，凡是比该频率高的工作频率都将产生丢步。这种失步说明步进电动机的转矩不足，拖动能力不够。
解决方法：
a)、使步进电动机本身产生的电磁转矩增大。为此可在额定电流范围内适当加大驱动电流；在高频范围转矩不足时，可适当提高驱动电路的驱动电压；改用转矩大的步进电动机等。
b)、使步进电动机需要克服的转矩减小。为此可适当降低电动机运行频率，以便提高电动机的输出转矩；设定较长的加速时间，以便转子获得足够的能量。
转子的平均速度高于定子磁场的平均旋转速度
解释：
转子的平均速度高于定子磁场的平均旋转速度，这时定子通电励磁的时间较长，大于转子步进一步所需的时间，则转子在步进过程中获得了过多的能量，使得步进电动机产生的输出转矩增大，从而使电动机越步。当用步进电动机驱动那些使负载上、下动作的机构时，更易产生越步现象，这是因为负载向下运动时，电动机所需的转矩减小。
解决方法：
a）减小步进电动机的驱动电流，以便降低步进电动机的输出转矩。
步进电动机及所带负载存在惯性
解释：
由于步进电动机自身及所带负载存在惯性，使得电动机在工作过程中不能立即起动和停止，而是在起动时出现丢步，在停止时发生越步。
解决方法：
a）通过一个加速和减速过程，即以较低的速度起动，而后逐渐加速到某一速度运行，再逐渐减速直至停止。进行合理、平滑的加减速控制是保证步进驱动系统可靠、高效、精确运行的关键。
步进电动机产生共振
共振也是引起失步的一个原因。步进电动机处于连续运行状态时，如果控制脉冲的频率等于步进电动机的固有频率，将产生共振。在一个控制脉冲周期内，振动得不到充分衰减，下一个脉冲就来到，因而在共振频率附近动态误差最大并会导致步进电动机失步。
解决方法：
a）适当减小步进电动机的驱动电流；采用细分驱动方法；采用阻尼方法，包括机械阻尼法。以上方法都能有效消除电动机振荡，避免失步现象发生。
改变方向时丢脉冲
解释：
表现为往任何一个方向都准，但一改变方向就累计偏差，并且次数越多偏得越多；
解决方案：
a）一般的步进驱动器对方向和脉冲信号都有一定的要求，如：方向信号在第一个脉冲上升沿或下降沿（不同的驱动器要求不一样）到来前数微秒被确定，否则会有一个脉冲所运转的角度与实际需要的转向相反，最后故障现象表现为越走越偏，细分越小越明显，解决办法主要用软件改变发脉冲的逻辑或加延时。
软件缺陷
解释：
控制程序导致失步也不少见，需要检查控制程序是不是有问题。
解决方案：
一时找不到问题原因，也有工程师会让步进电机运行一段时间就重新找原点归位。
闭环控制
闭环驱动的原理
首先我们要知道电机分为转子和定子，本质就是两块磁铁。转子是永磁铁，定子是电磁铁，定子的磁性大小和方向可以控制。当两个磁铁没有夹角时，电机就锁住，定子磁场越大，锁定扭矩越大。
当控制定子磁场跟转子产生夹角，转子就会被带动旋转。
转子的位置可以由编码器测量得到，于是我们可以通过算法调整定子的磁场，让磁场跟转子磁场始终保持有夹角，然后定子的磁场大小由相电流大小控制，于是就可以控制电机转速了。
当两个磁场夹角90度，可以获得最大的扭矩。
步进电机的相电流波形如下，两相电流相位差90度。
开环闭环控制区别
讲一下二相步进电机闭环驱动和开环驱动的区别。假设现在上位机主机发DIR/STEP指令给步进驱动器的指令角度是90.5度。
开环的驱动器收到指令后即根据细分电流表调整步进电机的两相电流，将电流矢量也指向90.5度，然后步进电机永磁转子就会在电磁力作用下运动到90.5度这个位置上了，这其中步进电机AB两相的驱动电流在设置好后就不变了，只要接下来没有新的角度指令那就一直保持原样不改变。
闭环控制器则不然，当闭环控制器收到90.5度的指令角度时，首先利用角度传感器（编码器）测出步进电机现在的实际角度假设是90度，然后跟开环的驱动器一样根据细分电流表调整步进电机的两相电流，但是要将电流矢量指向91.8度（指令角度比实际角度大那就在实际角度上加1.8度，反之减1.8度），与开环控制器不同的是闭环的驱动电流大小不是恒定不变的，而是根据角度误差经过PID算法计算出来的，简单的讲就是指令角度和实际角度的误差大那驱动电流就大相反误差小驱动电流就小，误差为0那驱动电流就为0（这就是闭环步进低发热省电的原因）。
硬件讲解
单片机采用STM32F103CBT6
编码器使用MT6816
用一块OLED12832来显示数据。参考链接
电机驱动芯片使用TB67H450（也可以使用其他类似功能的芯片，有很多，比如TI或国产）
驱动电路这里可以讲一下，上图左边是PWM经过低通滤波后得到模拟电压输出，PWM占空比越高模拟电压值越高。低成本的DAC。
右边是两个驱动芯片分别驱动电机的两根相线。
要知道我们的目的是让相电流波形如下，因此我们需要能控制相电流的方向，可以从A+流向A-，也可以从A-流向A+。同时也需要能控制相电流的大小，让相电流呈正弦波变化。
恰好TB67H450就有这样的功能，给IN1和IN2输入不同电平就可以控制电流方向
给VREF输入不同模拟电压值就可以控制OUT1和OUT2之间的电流大小。
代码讲解
主函数
主函数不用多说，就是初始化外设。初始化完成把中断频率改成20KHz，每20KHz频率计算一次相电流方向和大小来控制TB67H450。
void loop(void)
{
Button_Init(); //按键初始化
SSD1306_Init(); //OLED初始化
XDrive_REINui_Init(); //UI初始化
HAL_Delay(1000); //进行关键外设初始化前等待电源稳定
// //存储数据恢复
// Slave_Reg_Init(); //校验Flash数据并配置参数
REIN_DMA_Init();
REIN_ADC_Init(); //初始化ADC DMA方式采集母线电压
REIN_MT6816_Init(); //MT6816传感器初始化
REIN_HW_Elec_Init(); //初始化A4950的控制引脚，4个IO+2个PWM
Signal_MoreIO_Init(); //MoreIO接口初始化
//控制初始化(Control)
Calibration_Init(); //校准器初始化
Motor_Control_Init(); //电机控制初始化
//通讯接口初始化
REIN_GPIO_Modbus_Init(); //RS485_DIR
REIN_UART_Modbus_Init(); //串口初始化
Signal_Modbus_Init(); //Modbu接口初始化
//调整中断配置
LoopIT_Priority_Overlay(); //重写-中断优先级覆盖
LoopIT_SysTick_20KHz(); //重写-系统计时器修改为20KHz
//启动时使用按键触发校准
if(HAL_GPIO_ReadPin(BUTTON_DOWN_GPIO_Port, BUTTON_DOWN_Pin) == GPIO_PIN_RESET)
{
encode_cali.trigger = true; //触发校准
XDrive_REINui_ToCalibration(); //进入UI校准界面
}
//主循环
for(;;)
{
// major_cycle_count++;
time_second_run();
Calibration_Loop_Callback(); //校准器主程序回调 用于校准环节最后的数据解算
}
}
运动控制
下面就是20KHz调用一次的运动控制算法，也是所有程序的核心了。分成5个部分：读编码器、位置补偿、运动控制、硬目标提取、软目标提取。
读编码器：获取当前编码器值，计算当前位置。
位置补偿：计算电机转速。因为从读编码器位置到计算再到硬件输出存在延迟，因此估计下当硬件真正生效时转子的位置差，补偿下。
运动控制：输入当前速度和目标速度（或当前位置和目标位置），使用PID或DCE算法，计算相电流的方向和大小，最终控制TB67H450。
硬目标提取：就是从硬件上获取目标速度（或目标位置），比如脉冲输入、通信设定等。如果是Motor_Mode_PULSE_Location模式，就是就是读取外部脉冲输入个数来获取目标位置。
软目标提取：获取到硬目标后，需要做加减速曲线来使当前速度到达目标速度，如果目标位置太大是不能直接做PID的。经过加减速过程计算后的输出就是软目标。运动控制也是使用软目标和当前值做PID。
void Motor_Control_Callback(void) //电机控制，20K频率执行
{
/************************************ 数据采集 ************************************/
/************************************ 数据采集 ************************************/
int32_t sub_data; //用于各个算差
motor_control.real_lap_location_last = motor_control.real_lap_location; //上次编码器值
motor_control.real_lap_location = mt6816.rectify_angle; //本次编码器值
//跨圈检测
sub_data = motor_control.real_lap_location - motor_control.real_lap_location_last; //电机旋转角度
if(sub_data > (Move_Pulse_NUM >> 1)) sub_data -= Move_Pulse_NUM; //跨圈旋转角度纠正
else if(sub_data < -(Move_Pulse_NUM >> 1)) sub_data += Move_Pulse_NUM;
//读取位置
motor_control.real_location_last = motor_control.real_location; //上次电机位置
motor_control.real_location += sub_data; //电机位置
/************************************ 数据估计 ************************************/
/************************************ 数据估计 ************************************/
//估计速度
motor_control.est_speed_mut += ( ((motor_control.real_location - motor_control.real_location_last) * (CONTROL_FREQ_HZ)) //计算1秒的速度
+ ((int32_t)(motor_control.est_speed << 5) - (int32_t)(motor_control.est_speed)) //est_speed_mut=本次变化值+31倍est_speed
);
motor_control.est_speed = (motor_control.est_speed_mut >> 5); //(取整)(向0取整)(保留符号位) est_speed_mut=32倍est_speed
motor_control.est_speed_mut = ((motor_control.est_speed_mut) - ((motor_control.est_speed << 5))); //(取余，留给下次计算使用)(向0取整)(保留符号位)
//估计位置
motor_control.est_lead_location = Motor_Control_AdvanceCompen(motor_control.est_speed); //根据估计速度计算位置补偿值 补偿计算时间的延迟，延迟时间固定，但是速度越快，位置差越大
motor_control.est_location = motor_control.real_location + motor_control.est_lead_location; //估计位置=实际位置+补偿值
//估计误差
motor_control.est_error = motor_control.soft_location - motor_control.est_location;
/************************************ 运动控制 ************************************/
/************************************ 运动控制 ************************************/
//运行模式分支
switch(motor_control.mode_run)
{
//测试
case Motor_Mode_Debug_Location: Control_DCE_To_Electric(motor_control.soft_location, motor_control.soft_speed); break;
case Motor_Mode_Debug_Speed: Control_PID_To_Electric(motor_control.soft_speed); break;
//停止
case Control_Mode_Stop: REIN_HW_Elec_SetSleep(); break;
//DIG(CAN/RS485)
case Motor_Mode_Digital_Location: Control_DCE_To_Electric(motor_control.soft_location, motor_control.soft_speed); break;
case Motor_Mode_Digital_Speed: Control_PID_To_Electric(motor_control.soft_speed); break;
case Motor_Mode_Digital_Current: Control_Cur_To_Electric(motor_control.soft_current); break;
case Motor_Mode_Digital_Track: Control_DCE_To_Electric(motor_control.soft_location, motor_control.soft_speed); break;
//MoreIO(PWM/PUL)
case Motor_Mode_PWM_Location: Control_DCE_To_Electric(motor_control.soft_location, motor_control.soft_speed); break; //传入目标位置、速度，做PID
case Motor_Mode_PWM_Speed: Control_PID_To_Electric(motor_control.soft_speed); break; //传入目标速度，做PID，计算结果控制A4950
case Motor_Mode_PWM_Current: Control_Cur_To_Electric(motor_control.soft_current); break;
case Motor_Mode_PULSE_Location: Control_DCE_To_Electric(motor_control.soft_location, motor_control.soft_speed); break;
//其他非法模式
default: break;
}
/************************************ 硬目标提取 ************************************/
/************************************ 从硬件上获取的目标，比如脉冲输入、通信设定等 ************************************/
switch(motor_control.mode_run){
//MoreIO(PWM/PUL)
case Motor_Mode_PWM_Location:
case Motor_Mode_PWM_Speed:
case Motor_Mode_PWM_Current:
Signal_MoreIO_Capture_Goal(); //MoreIO接口获取数据
motor_control.goal_location = signal_moreio.goal_location; //获取目标位置
motor_control.goal_speed = signal_moreio.goal_speed; //获取目标速度
motor_control.goal_current = signal_moreio.goal_current; //获取目标电流
motor_control.goal_disable = signal_moreio.goal_disable; //获取目标失能
motor_control.goal_brake = signal_moreio.goal_brake; //获取目标刹车
break;
case Motor_Mode_PULSE_Location:
Signal_MoreIO_Capture_Goal(); //MoreIO接口获取数据
motor_control.goal_location += signal_moreio.goal_location; //提取目标位置(Count模式借用目标位置存放目标位置增量)
motor_control.goal_disable = signal_moreio.goal_disable; //提取目标失能
motor_control.goal_brake = signal_moreio.goal_brake; //提取目标刹车
break;
//其他非法模式
default: break;
}
/************************************ 软目标提取 ************************************/
/************************************ 根据硬目标，软件计算后的目标值，比如加速曲线等 ************************************/
//提取(软位置,软速度,软电流)
switch(motor_control.mode_run){
//测试
case Motor_Mode_Debug_Location: Motor_MultiDebug_Location(); break;
case Motor_Mode_Debug_Speed: Motor_MultiDebug_Speed(); break;
//停止
case Control_Mode_Stop: break;
//DIG(CAN/RS485)
case Motor_Mode_Digital_Location: Location_Tracker_Capture_Goal(motor_control.goal_location);
motor_control.soft_location = location_tck.go_location;
motor_control.soft_speed = location_tck.go_speed;
break;
case Motor_Mode_Digital_Speed: Speed_Tracker_Capture_Goal(motor_control.goal_speed);
motor_control.soft_speed = speed_tck.go_speed;
break;
case Motor_Mode_Digital_Current: Current_Tracker_Capture_Goal(motor_control.goal_current);
motor_control.soft_current = current_tck.go_current;
break;
case Motor_Mode_Digital_Track: Move_Reconstruct_Capture_Goal(motor_control.goal_location, motor_control.goal_speed);
motor_control.soft_location = move_reco.go_location;
motor_control.soft_speed = move_reco.go_speed;
break;
//MoreIO(PWM/PUL)
case Motor_Mode_PWM_Location: Location_Tracker_Capture_Goal(motor_control.goal_location);
motor_control.soft_location = location_tck.go_location;
motor_control.soft_speed = location_tck.go_speed;
break;
case Motor_Mode_PWM_Speed: Speed_Tracker_Capture_Goal(motor_control.goal_speed); //根据目标速度计算接下来的速度
motor_control.soft_speed = speed_tck.go_speed; //把计算结果赋值给 soft_speed ，soft_speed再传入PID
break;
case Motor_Mode_PWM_Current: Current_Tracker_Capture_Goal(motor_control.goal_current);
motor_control.soft_current = current_tck.go_current;
break;
case Motor_Mode_PULSE_Location: Location_Interp_Capture_Goal(motor_control.goal_location);
motor_control.soft_location = location_interp.go_location;
motor_control.soft_speed = location_interp.go_speed;
break;
}
}
如果选择的输入脉冲个数控制电机位置模式（Motor_Mode_PULSE_Location），运动控制部分会调用Control_DCE_To_Electric(motor_control.soft_location, motor_control.soft_speed)，此函数参数是软位置、软速度。
函数的前面就是根据位置误差、速度误差计算相电流大小，误差越大相电流越大。
如果相电流是正的定子磁场就超前转子磁场90度电机正转。相电流是负的定子磁场就滞后转子磁场90度电机反转。
void Control_DCE_To_Electric(int32_t _location, int32_t _speed) //DCE电流控制 输入目标位置、速度
{
//误差
dce.p_error = _location - motor_control.est_location; //位置误差=目标位置-实际位置
dce.v_error = (_speed - motor_control.est_speed) >> 7; //速度误差=目标速度-实际速度 缩小128倍
if(dce.p_error > ( 3200)) dce.p_error = ( 3200); //限制位置误差在1/16圈内(51200/16)
if(dce.p_error < (-3200)) dce.p_error = (-3200);
if(dce.v_error > ( 4000)) dce.v_error = ( 4000); //限制速度误差在10r/s内(51200*10/128)
if(dce.v_error < (-4000)) dce.v_error = (-4000);
//op输出计算
dce.op = ((dce.kp) * (dce.p_error));
//oi输出计算
dce.i_mut += ((dce.ki) * (dce.p_error));
dce.i_mut += ((dce.kv) * (dce.v_error));
dce.i_dec = (dce.i_mut >> 7);
dce.i_mut -= (dce.i_dec << 7);
dce.oi += (dce.i_dec);
if(dce.oi > ( Current_Rated_Current << 10 )) dce.oi = ( Current_Rated_Current << 10 ); //限制为额定电流 * 1024
else if(dce.oi < (-(Current_Rated_Current << 10))) dce.oi = (-(Current_Rated_Current << 10)); //限制为额定电流 * 1024
//od输出计算
dce.od = ((dce.kd) * (dce.v_error));
//综合输出计算
dce.out = (dce.op + dce.oi + dce.od) >> 10;
if(dce.out > Current_Rated_Current) dce.out = Current_Rated_Current; //限幅
else if(dce.out < -Current_Rated_Current) dce.out = -Current_Rated_Current;
//输出FOC电流
motor_control.foc_current = dce.out;
//输出FOC位置
if(motor_control.foc_current > 0) motor_control.foc_location = motor_control.est_location + Move_Divide_NUM; //电流>0，产生磁场超前90度
else if(motor_control.foc_current < 0) motor_control.foc_location = motor_control.est_location - Move_Divide_NUM; //电流<0，产生磁场滞后90度
else motor_control.foc_location = motor_control.est_location; //电流=0，磁场等于转子角度，锁住
//输出任务到驱动
REIN_HW_Elec_SetDivideElec(motor_control.foc_location, motor_control.foc_current); //传入磁场角度和大小，产生磁场
//CurrentControl_Out_FeedTrack(motor_control.foc_location, motor_control.foc_current, false, true);
}
把相电流相位、相电流大小输入REIN_HW_Elec_SetDivideElec函数，控制TB67H450输出
/**
* @brief 设置输出细分电流
* @param divide: 细分 (0 ~ 细分数)
* @param dac_reg: 电流 (0 ~ 3300mA)
* @retval NULL
*/
void REIN_HW_Elec_SetDivideElec(uint32_t divide, int32_t elec_ma) //输出磁场
{
//由细分数获得数组指针
coil_b.conver = (divide) & (0x000003FF); //对1024取余 磁场360度对应1024
coil_a.conver = (coil_b.conver + (256)) & (0x000003FF); //对1024取余 另一相磁场偏差90度
//由数据指针获得整形数据(空间换时间方案)
coil_a.sin_data = sin_pi_m2[coil_a.conver];
coil_b.sin_data = sin_pi_m2[coil_b.conver];
//由整形数据获得DAC寄存器数据
uint32_t dac_reg = abs(elec_ma); //电压电流关系为1:1(检流电阻为0.1欧)
dac_reg = (uint32_t)(dac_reg * 5083) >> 12; //(dac_reg * 4095 / 3300)的变种
dac_reg = dac_reg & (0x00000FFF); //(对4096取余)(向小取整)(舍弃符号位)
coil_a.dac_reg = (uint32_t)(dac_reg * abs(coil_a.sin_data)) >> sin_pi_m2_dpiybit; //(--- / sin_1024_dpiy)的变种
coil_b.dac_reg = (uint32_t)(dac_reg * abs(coil_b.sin_data)) >> sin_pi_m2_dpiybit; //(--- / sin_1024_dpiy)的变种
//DAC输出
CurrentControl_VREF_12Bit_MixOut(coil_a.dac_reg, coil_b.dac_reg); //设置电流大小
if(coil_a.sin_data > 0) { Out_AP_H(); Out_AM_L(); }
else if(coil_a.sin_data < 0) { Out_AP_L(); Out_AM_H(); }
else { Out_AP_H(); Out_AM_H(); }
if(coil_b.sin_data > 0) { Out_BP_H(); Out_BM_L(); }
else if(coil_b.sin_data < 0) { Out_BP_L(); Out_BM_H(); }
else { Out_BP_H(); Out_BM_H(); }
}
REIN_HW_Elec_SetDivideElec就是根据输入的角度值、大小值来控制两相电流的大小
两相电流相差90度。
再来看硬目标怎么转成软目标的：
void Location_Interp_Capture_Goal(int32_t goal_location)
{
//记录源信号
location_interp.record_location_last = location_interp.record_location; //上次计算时的目标位置
location_interp.record_location = goal_location; //本次的目标位置
//估计源信号速度
location_interp.est_speed_mut += ( ((location_interp.record_location - location_interp.record_location_last) * CONTROL_FREQ_HZ) //计算1秒的目标变化速度
+ ((int32_t)(location_interp.est_speed << 6) - (int32_t)(location_interp.est_speed)) //est_speed_mut=本次变化值+63倍est_speed
);
location_interp.est_speed = (location_interp.est_speed_mut >> 6); //(对64取整)(向0取整)(保留符号位) est_speed_mut=64倍est_speed
location_interp.est_speed_mut = (location_interp.est_speed_mut - (location_interp.est_speed << 6)); //(取余，留给下次计算使用)(向0取整)(保留符号位)
//估计源信号位置
location_interp.est_location = location_interp.record_location;
//输出
location_interp.go_location = location_interp.est_location;
location_interp.go_speed = location_interp.est_speed; //输入脉冲的速度作为运动的目标速度
}
本次输入脉冲数-上次输入脉冲数，就可以得到输入脉冲速度，用作目标速度。
编码器矫正
因为磁编码器芯片存在非线性误差，与传统的光电编码器相比精度较差，解决的方法是以步进电机本身的全步精度为基准进行校正，因为普通的步进电机全步精度都可以达到0.08度以下，校正后的磁编码器跟1000线的光电编码器具有同等级的精度。
就是让电机正反转，转一个点就读编码器的值，最后根据线性关系来矫正编码器。代码如下，注释比较详尽了。
//校准器实例
Encode_Cali_Typedef encode_cali; //定义一个校准器
//私有函数
uint32_t CycleRem(uint32_t a,uint32_t b); //取余(函数形式实现,防止编译器优化)
int32_t CycleSub(int32_t a, int32_t b, int32_t cyc); //取循环差(函数形式实现,防止编译器优化)
int32_t CycleAverage(int32_t a, int32_t b, int32_t cyc); //取循环平均值(函数形式实现,防止编译器优化)
void Calibration_Data_Check(void); //校准器原始数据检查
/**
* 取余,兼容一个周期的负数(核心控制-参数自动转换为无符号整形)
* @param NULL
* @retval NULL
**/
uint32_t CycleRem(uint32_t a,uint32_t b)
{
return (a+b)%b;
}
/**
* 计算a、b的差
* @param NULL
* @retval NULL
**/
int32_t CycleSub(int32_t a, int32_t b, int32_t cyc)
{
int32_t sub_data;
sub_data = a - b;
if(sub_data > (cyc >> 1)) sub_data -= cyc;
if(sub_data < (-cyc >> 1)) sub_data += cyc;
return sub_data;
}
/**
* 计算a、b的平均值
* @param NULL
* @retval NULL
**/
int32_t CycleAverage(int32_t a, int32_t b, int32_t cyc)
{
int32_t sub_data;
int32_t ave_data;
sub_data = a - b;
ave_data = (a + b) >> 1; //计算a、b的平均值
if(abs(sub_data) > (cyc >> 1))
{
if(ave_data >= (cyc >> 1)) ave_data -= (cyc >> 1);
else ave_data += (cyc >> 1);
}
return ave_data;
}
/**
* 对data中的length个数据计算平均值
* @param NULL
* @retval NULL
**/
int32_t DataAverage(uint16_t *data, uint16_t length, int32_t cyc)
{
int32_t sum_data = 0; //和
int32_t sub_data;
int32_t diff_data;
//加载第一值
sum_data += (int32_t)data[0];
//加载后面的值
for(uint16_t i=1; i<length; i++){
diff_data = (int32_t)data[i];
sub_data = (int32_t)data[i] - (int32_t)data[0]; //两次编码器读取值做差，如果大于16384/2或小于-16384/2，说明编码器读取值跨圈了
if(sub_data > (cyc >> 1)) diff_data = (int32_t)data[i] - cyc; //如果跨圈了，把数据矫正回来
if(sub_data < (-cyc >> 1)) diff_data = (int32_t)data[i] + cyc;
sum_data += diff_data; //数据和
}
//计算平均值
sum_data = sum_data / length;
//平均值标准化
if(sum_data < 0) sum_data += cyc;
if(sum_data > cyc) sum_data -= cyc;
return sum_data;
}
/**
* @brief 校准器原始数据检查
* @param NULL
* @retval NULL
*/
void Calibration_Data_Check(void)
{
uint32_t count; //用于各个计数
int32_t sub_data; //用于各个算差
/******************** 检查平均值连续性及方向 ********************/
//求解平均值数据
for(count=0; count<Move_Step_NUM+1; count++) //coder_data_f、coder_data_r分别是正转和反转时同一个点的编码器读取值，现在计算他们的平均值并赋给coder_data_f
{
encode_cali.coder_data_f[count] = (uint16_t)CycleAverage((int32_t)encode_cali.coder_data_f[count], (int32_t)encode_cali.coder_data_r[count], CALI_Encode_Res);
}
//平均值数据检查
sub_data = CycleSub((int32_t)encode_cali.coder_data_f[0], (int32_t)encode_cali.coder_data_f[Move_Step_NUM-1], CALI_Encode_Res); //coder_data_f[0]-coder_data_f[199] 计算第一个和最后一个数据的差
if(sub_data == 0) { encode_cali.error_code = CALI_Error_Average_Dir; return;} //=0说明有问题
if(sub_data > 0) { encode_cali.dir = true; } //电机正转，编码器值是增加的
if(sub_data < 0) { encode_cali.dir = false;} //电机正转，编码器值是减少的 可调换电机相线顺序来修改
for(count=1; count<Move_Step_NUM; count++)
{
sub_data = CycleSub((int32_t)encode_cali.coder_data_f[count], (int32_t)encode_cali.coder_data_f[count-1], CALI_Encode_Res); //coder_data_f[1]-coder_data_f[0] 计算前一个和后一个数据的差
//CALI_Gather_Encode_Res=16384/200=81 一个步距角，编码器值应该改变81 如果实际改变大于了81*1.5或小于了81*0.5，都说明有问题
if(abs(sub_data) > (CALI_Gather_Encode_Res * 3 / 2)){ encode_cali.error_code = CALI_Error_Average_Continuity; encode_cali.error_data = count; return; } //两次数据差过大
if(abs(sub_data) < (CALI_Gather_Encode_Res * 1 / 2)){ encode_cali.error_code = CALI_Error_Average_Continuity; encode_cali.error_data = count; return; } //两次数据差过小
if(sub_data == 0) { encode_cali.error_code = CALI_Error_Average_Dir; encode_cali.error_data = count; return; }
if((sub_data > 0) && (!encode_cali.dir)) { encode_cali.error_code = CALI_Error_Average_Dir; encode_cali.error_data = count; return; }
if((sub_data < 0) && (encode_cali.dir)) { encode_cali.error_code = CALI_Error_Average_Dir; encode_cali.error_data = count; return; }
}
//寻找编码器值跨圈的地方
uint32_t step_num = 0;
if(encode_cali.dir)
{
for(count=0; count<Move_Step_NUM; count++)
{
sub_data = (int32_t)encode_cali.coder_data_f[CycleRem(count+1, Move_Step_NUM)] - (int32_t)encode_cali.coder_data_f[CycleRem(count, Move_Step_NUM)]; //coder_data_f[1]-coder_data_f[0] 计算前一个和后一个数据的差
if(sub_data < 0) //前一个和后一个数据的差平时都是正的81左右，当跨圈时会变成负 比如前一个是16380，后一个变成了78
{
step_num++;
encode_cali.rcd_x = count;//记录跨圈的地方 比如从11(值16380)到12(值78)发生了跨圈，rcd_x=11，rcd_y=3
encode_cali.rcd_y = (CALI_Encode_Res-1) - encode_cali.coder_data_f[CycleRem(encode_cali.rcd_x, Move_Step_NUM)]; //16383-16380=3
}
}
if(step_num != 1)
{
encode_cali.error_code = CALI_Error_PhaseStep; //如果没有找到跨圈的地方，说明有问题
return;
}
}
else
{
for(count=0; count<Move_Step_NUM; count++)
{
sub_data = (int32_t)encode_cali.coder_data_f[CycleRem(count+1, Move_Step_NUM)] - (int32_t)encode_cali.coder_data_f[CycleRem(count, Move_Step_NUM)];
if(sub_data > 0)
{
step_num++;
encode_cali.rcd_x = count;//记录跨圈的地方
encode_cali.rcd_y = (CALI_Encode_Res-1) - encode_cali.coder_data_f[CycleRem(encode_cali.rcd_x+1, Move_Step_NUM)];
}
}
if(step_num != 1)
{
encode_cali.error_code = CALI_Error_PhaseStep; //如果没有找到跨圈的地方，说明有问题
return;
}
}
//校准OK
encode_cali.error_code = CALI_No_Error;
return;
}
/**
* @brief 校准器初始化
* @param NULL
* @retval NULL
*/
void Calibration_Init(void)
{
//信号
encode_cali.trigger = false;
encode_cali.error_code = CALI_No_Error;
encode_cali.error_data = 0;
//读取过程
encode_cali.state = CALI_Disable;
encode_cali.out_location = 0;
//coder_data_f[]
//coder_data_r[]
//dir
//全段域校准过程数据
encode_cali.rcd_x = 0;
encode_cali.rcd_y = 0;
encode_cali.result_num = 0;
}
/**
* @brief 校准器中断回调
* @param NULL
* @retval NULL
*/
void Calibration_Interrupt_Callback(void)
{
#define AutoCali_Speed 2 //自动校准速度(主要用于编码器预数据采集)
#define Cali_Speed 1 //校准速度(用于精确的数据采集)
//唤醒电流控制
//CurrentControl_OutWakeUp();
//状态变换
switch(encode_cali.state)
{
//失能状态
case CALI_Disable:
//ELEC_Set_Sleep();
if(encode_cali.trigger)
{
REIN_HW_Elec_SetDivideElec(encode_cali.out_location, Current_Cali_Current);
//CurrentControl_Out_FeedTrack(encode_cali.out_location, Current_Cali_Current, true, true);
encode_cali.out_location = Move_Pulse_NUM; //输出到1圈位置 200*256
encode_cali.gather_count = 0; //采集清零
encode_cali.state = CALI_Forward_Encoder_AutoCali; //--->编码器正转自动校准
//初始化标志
encode_cali.error_code = CALI_No_Error;
encode_cali.error_data = 0;
}
break;
//编码器正转自动校准
case CALI_Forward_Encoder_AutoCali://正转1圈 (1 * Motor_Pulse_NUM) -> (2 * Motor_Pulse_NUM)
encode_cali.out_location += AutoCali_Speed;
REIN_HW_Elec_SetDivideElec(encode_cali.out_location, Current_Cali_Current);
//CurrentControl_Out_FeedTrack(encode_cali.out_location, Current_Cali_Current, true, true);
if(encode_cali.out_location == 2 * Move_Pulse_NUM) //1圈转动完成
{
encode_cali.out_location = Move_Pulse_NUM;
encode_cali.state = CALI_Forward_Measure; //--->正向测量
}
break;
//正向测量
case CALI_Forward_Measure://(Motor_Pulse_NUM) -> (2 * Motor_Pulse_NUM)
if((encode_cali.out_location % Move_Divide_NUM) == 0) //每到达采集细分量点采集一次数据，1.8步距角，共有200步，每步256细分
{
encode_cali.coder_data_gather[encode_cali.gather_count++] = mt6816.angle_data; //在同一个点读取16次，取平均值
if(encode_cali.gather_count == Gather_Quantity) //如果采集了16个数据了
{
//记录数据
encode_cali.coder_data_f[(encode_cali.out_location - Move_Pulse_NUM) / Move_Divide_NUM]
= DataAverage(encode_cali.coder_data_gather, Gather_Quantity, CALI_Encode_Res); //计算16个数据的平均值
//采集计数清零
encode_cali.gather_count = 0;
//转动到下一个位置
encode_cali.out_location += Cali_Speed; //加1
}
}
else
{
//转动到下一个位置
encode_cali.out_location += Cali_Speed; //加1
}
REIN_HW_Elec_SetDivideElec(encode_cali.out_location, Current_Cali_Current);
//CurrentControl_Out_FeedTrack(encode_cali.out_location, Current_Cali_Current, true, true);
if(encode_cali.out_location > (2 * Move_Pulse_NUM)) //如果转动距离大于了1圈 正向转完了1圈
{
encode_cali.state = CALI_Reverse_Ret; //--->反向回退
}
break;
//反向回退
case CALI_Reverse_Ret://(2 * Motor_Pulse_NUM) -> (2 * Motor_Pulse_NUM + Motor_Divide_NUM * 20)
encode_cali.out_location += Cali_Speed; //加1
REIN_HW_Elec_SetDivideElec(encode_cali.out_location, Current_Cali_Current);
//CurrentControl_Out_FeedTrack(encode_cali.out_location, Current_Cali_Current, true, true);
if(encode_cali.out_location == (2 * Move_Pulse_NUM + Move_Divide_NUM * 20)) //继续往前正转256*20个点
{
encode_cali.state = CALI_Reverse_Gap;//--->反向消差
}
break;
//反向消差
case CALI_Reverse_Gap://(2 * Motor_Pulse_NUM + Motor_Divide_NUM * 20) -> (2 * Motor_Pulse_NUM)
encode_cali.out_location -= Cali_Speed; //减1
REIN_HW_Elec_SetDivideElec(encode_cali.out_location, Current_Cali_Current);
//CurrentControl_Out_FeedTrack(encode_cali.out_location, Current_Cali_Current, true, true);
if(encode_cali.out_location == (2 * Move_Pulse_NUM)) //往后转到刚好2圈的位置
{
encode_cali.state = CALI_Reverse_Measure;//--->反向测量
}
break;
//反向测量
case CALI_Reverse_Measure://(2 * Motor_Pulse_NUM) -> (Motor_Pulse_NUM)
if((encode_cali.out_location % Move_Divide_NUM) == 0)//每到达采集细分量点采集一次数据，1.8步距角，共有200步，每步256细分
{
encode_cali.coder_data_gather[encode_cali.gather_count++] = mt6816.angle_data; //在同一个点读取16次，取平均值
if(encode_cali.gather_count == Gather_Quantity) //如果采集了16个数据了
{
//记录数据
encode_cali.coder_data_r[(encode_cali.out_location - Move_Pulse_NUM) / Move_Divide_NUM]
= DataAverage(encode_cali.coder_data_gather, Gather_Quantity, CALI_Encode_Res); //计算16个数据的平均值
//采集计数清零
encode_cali.gather_count = 0;
//移动位置
encode_cali.out_location -= Cali_Speed; //减1
}
}
else
{
//移动位置
encode_cali.out_location -= Cali_Speed; //减1
}
REIN_HW_Elec_SetDivideElec(encode_cali.out_location, Current_Cali_Current);
//CurrentControl_Out_FeedTrack(encode_cali.out_location, Current_Cali_Current, true, true);
if(encode_cali.out_location < Move_Pulse_NUM)
{
encode_cali.state = CALI_Operation;//--->解算
}
break;
//解算
case CALI_Operation:
//进行校准运算中
REIN_HW_Elec_SetDivideElec(0, 0);
//CurrentControl_Out_FeedTrack(0, 0, true, true);
break;
default:
break;
}
}
/**
* @brief 校准器主程序回调
* @param NULL
* @retval NULL
*/
void Calibration_Loop_Callback(void)
{
int32_t data_i32;
uint16_t data_u16;
//非解算态退出
if(encode_cali.state != CALI_Operation)
return;
//PWM输出衰减态
REIN_HW_Elec_SetSleep();
//校准器原始数据检查
Calibration_Data_Check();
/********** 进行快速表表格建立 **********/
if(encode_cali.error_code == CALI_No_Error)
{
//数据解析
/******************** 全段域校准 完全拟合传感器数据与电机实际相位角非线性关系 ********************/
int32_t step_x, step_y;
encode_cali.result_num = 0;
Stockpile_Flash_Data_Empty(&stockpile_quick_cali); //擦除数据区
Stockpile_Flash_Data_Begin(&stockpile_quick_cali); //开始写数据区
if(encode_cali.dir)
{ //编码器读取值的0值，在跨圈处这里，因此从跨圈处开始写数据 假如从11(值16380)到12(值78)发生了跨圈，rcd_x=11，rcd_y=3
for(step_x = encode_cali.rcd_x; step_x < encode_cali.rcd_x + Move_Step_NUM + 1; step_x++)
{ //编码器一圈是16384 电机一圈是200*256=51200 一个步距角编码器的变化量对应电机变化256 256的变化是准的，但是编码器的变化量不一定，因此用256的变化量来矫正编码器
//16384对应到51200，只有变化量是对应的就行，0值和变化方向都不需要对应，比如编码器的0值可以对应100、编码器的1值可以对应103或97，这是不影响的
data_i32 = CycleSub(encode_cali.coder_data_f[CycleRem(step_x+1,Move_Step_NUM)],encode_cali.coder_data_f[CycleRem(step_x,Move_Step_NUM)],CALI_Encode_Res); //前一个数据减后一个数据
if(step_x == encode_cali.rcd_x) //跨圈处会存在一个步距角被分成首尾两部分的问题 比如跨圈处编码器变化了82，coder_data_f[12]=78，因此首部分编码器值范围0~78，有79个数据
{ //data_i32=coder_data_f[12]-coder_data_f[11]=82
for(step_y = encode_cali.rcd_y; step_y < data_i32; step_y++) //rcd_y=3，data_i32=82，这里要写79个数据
{
data_u16 = CycleRem(Move_Divide_NUM * step_x + Move_Divide_NUM * step_y / data_i32,Move_Pulse_NUM); //第一个数据data_u16=256*11+256*3/82=2825
Stockpile_Flash_Data_Write_Data16(&stockpile_quick_cali, &data_u16, 1);
encode_cali.result_num++;
}
}
else if(step_x == encode_cali.rcd_x + Move_Step_NUM) //跨圈处编码器变化了82，coder_data_f[11]=16380，因此尾部分编码器值范围16381~16383，有3个数据
{ //data_i32=coder_data_f[12]-coder_data_f[11]=82
for(step_y = 0; step_y < encode_cali.rcd_y; step_y++) //rcd_y=3，这里要写3个数据
{
data_u16 = CycleRem(Move_Divide_NUM * step_x + Move_Divide_NUM * step_y / data_i32,Move_Pulse_NUM);
Stockpile_Flash_Data_Write_Data16(&stockpile_quick_cali, &data_u16, 1);
encode_cali.result_num++;
}
}
else //完整的步距角
{
for(step_y = 0; step_y < data_i32; step_y++)
{ //把编码器变化量data_i32放缩到Move_Divide_NUM=256
data_u16 = CycleRem(Move_Divide_NUM * step_x + Move_Divide_NUM * step_y / data_i32,Move_Pulse_NUM);
Stockpile_Flash_Data_Write_Data16(&stockpile_quick_cali, &data_u16, 1);
encode_cali.result_num++;
}
}
}
}
else
{
for(step_x = encode_cali.rcd_x + Move_Step_NUM; step_x > encode_cali.rcd_x - 1; step_x--)
{
data_i32 = CycleSub(encode_cali.coder_data_f[CycleRem(step_x, Move_Step_NUM)],encode_cali.coder_data_f[CycleRem(step_x+1, Move_Step_NUM)],CALI_Encode_Res);
if(step_x == encode_cali.rcd_x+Move_Step_NUM) //开始边缘
{
for(step_y = encode_cali.rcd_y; step_y < data_i32; step_y++)
{
data_u16 = CycleRem(Move_Divide_NUM * (step_x+1) - Move_Divide_NUM * step_y / data_i32,Move_Pulse_NUM);
Stockpile_Flash_Data_Write_Data16(&stockpile_quick_cali, &data_u16, 1);
encode_cali.result_num++;
}
}
else if(step_x == encode_cali.rcd_x) //结束边缘
{
for(step_y = 0; step_y < encode_cali.rcd_y; step_y++)
{
data_u16 = CycleRem(Move_Divide_NUM * (step_x+1) - Move_Divide_NUM * step_y / data_i32,Move_Pulse_NUM);
Stockpile_Flash_Data_Write_Data16(&stockpile_quick_cali, &data_u16, 1);
encode_cali.result_num++;
}
}
else //中间
{
for(step_y = 0; step_y < data_i32; step_y++)
{
data_u16 = CycleRem(Move_Divide_NUM * (step_x+1) - Move_Divide_NUM * step_y / data_i32,Move_Pulse_NUM);
Stockpile_Flash_Data_Write_Data16(&stockpile_quick_cali, &data_u16, 1);
encode_cali.result_num++;
}
}
}
}
Stockpile_Flash_Data_End(&stockpile_quick_cali); //结束写数据区
if(encode_cali.result_num != CALI_Encode_Res) //如果写的数据个数不等于16384
encode_cali.error_code = CALI_Error_Analysis_Quantity;
}
//确认校准结果
if(encode_cali.error_code == CALI_No_Error)
{
mt6816.rectify_valid = true; //校准有效
}
else
{
mt6816.rectify_valid = false; //校准无效
Stockpile_Flash_Data_Empty(&stockpile_quick_cali); //清除校准区数据
}
Signal_MoreIO_Capture_Goal(); //读取信号端口数据以清除校准期间采样的信号
motor_control.stall_flag = true; //触发堵转保护,即校准后禁用运动控制
encode_cali.state = CALI_Disable; //清理校准信号
encode_cali.trigger = false; //清除校准触发
}
扫一扫
663
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
