STC-FOC Lite程序详解-不同控制模式的实现
本篇文章致力于对STC-FOC Lite的原版代码进行详尽的原理解释，并且提出更改/移植/裁剪建议，方便大家对这个项目进行更加适配自己的个性化更改。在STC-FOC Lite中，拥有丰富可修改的多种模式。 速度模式：控制的是速度，单位是rpm（转每分钟）。 位置模式：控制的增量位置，单位等同编码器分辨率。 开环模式：可以用来验证驱动电路是否正常，方向和速度不可修改，可控制启停。 舵机模式：拥有自动探测物理限位的功能，自动映射到-1000~+1000范围，探测到限位后自动归中。 三段开关模式：可以模拟出三段的开关，并且读取到开关的状态。 这其中，开环、速度、位置模式都是比较常见的功能，这里就着重介绍一下比较有特色的舵机模式和三段开关模式。 这里直接上代码来解释，首先是舵机模式：// 线程1, 舵机模式自动寻找左右限位Task = 1;
switch (Task_This)
{
case 0:
// 初始化任务
if (Servo_Config_Flag && Run_Flag)
{
// 读取当前舵机位置
Servo_This_Pos = Read_Postion_Int_Data();
Task_This++; // 进入下一个状态
}
break;
case 1:
// 最大值探测
if (I_Error_Cnt > I_Error_Dat)
{
// 限流保护，如果电流超过阈值
Servo_Max_Pos = (int)moto.postion; // 记录最大位置
Servo_Add_Pos = 0; // 自增清零
Task_This = 4; // 跳转到回到中心点的任务
}
else
{
// 逐步增加舵机位置
Servo_Add_Pos += 10;
moto.set_postion = Servo_This_Pos + Servo_Add_Pos;
Delay(1); // 延迟1毫秒
// 如果自增位置超过编码器的最大范围的一半
if (Servo_Add_Pos >= (Encode_bit_Max / 2))
{
Task_This = 4; // 跳转到回到中心点的任务
Servo_Max_Pos = (int)moto.postion; // 记录最大位置
Servo_Add_Pos = 0; // 强制中断
}
}
break;
case 3:
// 重新开始最大值探测
Task_This = 1;
break;
case 4:
// 回到中心点
moto.set_postion = Servo_This_Pos; // 设置舵机回到初始位置
if (I_Error_Cnt < I_Error_Dat) // 堵转保护报警消失
Delay(200); // 延迟200毫秒
break;
case 6:
// 最小值探测
if (I_Error_Cnt > I_Error_Dat)
{
// 限流保护，如果电流超过阈值
Servo_Min_Pos = (int)moto.postion; // 记录最小位置
Servo_Add_Pos = 0; // 自增清零
Task_This = 9; // 跳转到回到中心点的任务
}
else
{
// 逐步减少舵机位置
Servo_Add_Pos -= 10;
moto.set_postion = Servo_This_Pos + Servo_Add_Pos;
Delay(1); // 延迟1毫秒
// 如果自增位置低于编码器的最大范围的一半
if (Servo_Add_Pos <= -(Encode_bit_Max / 2))
{
Task_This = 9; // 跳转到回到中心点的任务
Servo_Min_Pos = (int)moto.postion; // 记录最小位置
Servo_Add_Pos = 0; // 强制中断
}
}
break;
case 8:
// 重新开始最小值探测
Task_This = 6;
break;
case 9:
// 回到中心点
moto.set_postion = Servo_This_Pos; // 设置舵机回到初始位置
if (I_Error_Cnt < I_Error_Dat) // 堵转保护报警消失
Delay(200); // 延迟200毫秒
break;
case 11:
// 自动回中
moto.set_postion = (Servo_Max_Pos + Servo_Min_Pos) / 2; // 设置舵机到最大和最小位置的中间
Delay(50); // 延迟50毫秒
break;
case 13:
// 完成最大值最小值探测，返回开头
Servo_Config_Flag = 0; // 标记配置完成
Task_This = 0; // 重置任务状态
break;
default:
Get_Delay(); // 处理其他状态
break;
}
以上代码其实是实现了一个舵机位置探测和自动校准的功能。具体步骤如下：步骤一：逐步增加舵机位置通过逐步增加舵机的绝对位置，每次增加10个单位。如果自增位置超过编码器最大范围的一半，则跳转到步骤四，记录当前最大位置，并将舵机位置重置到初始位置。超过范围意味着半圈内并没有机械限位，所以要放弃搜索，转去搜索另一个边界。步骤三：重新开始最大值探测重新开始最大值探测的任务，和任务1进行配合，完成延迟后重复的功能，类似没有死循环的for语句。步骤四：回到中心点舵机回到初始位置。如果堵转保护报警消失，则延迟200毫秒。步骤六：最小值探测进行最小值探测。如果电流超过阈值，则记录最小位置，并跳转到步骤九。否则，逐步减少舵机位置，每次减少10个单位。如果自增位置低于编码器最大范围的一半，则跳转到步骤九，记录当前最小位置，并将舵机位置重置到初始位置。步骤八：重新开始最小值探测重新开始最小值探测的任务。步骤九：回到中心点舵机回到初始位置（这个位置可能并不是中间）。如果堵转保护报警消失，则延迟200毫秒。步骤十一：自动回中舵机自动回到最大和最小位置的中间位置，并延迟50毫秒，等待转动稳定。步骤十三：完成最大值最小值探测完成最大值和最小值探测后，标记配置完成，并重置任务状态。默认处理：处理其他状态。 以上步骤中的“Task_This”变量用于控制任务的跳转，而“Servo_Add_Pos”变量则用于逐步增加或减少舵机的位置。此外，“Servo_Max_Pos”和“Servo_Min_Pos”变量分别用于记录舵机的最大和最小位置，“Servo_Config_Flag”变量用于标记配置是否完成。 这部分代码的精髓就是巧妙利用了堵转保护功能来实现边界检测，因为在较为灵敏的堵转保护和较低的转速驱动时，电机驱动过程中并不会出现大电流的情况，十分安全。通过记忆堵转后的位置，也可以获得十分精确的位置信息。这部分边界位置信息会在后面的舵机模式中，充当电子限位的功能。并且，电子限位会对边界位置信息进行内缩，保证运行过程中尽量不与机械边界进行碰撞。
讲完了舵机模式，再讲一下这个三段开关（更改一下也可以是多段开关）// 线程4，多档开关DEMO,挡位分别为0,4000,8000
// 如果超出范围，默认回到4000挡位。每个挡位的极限变化值是1500
Task = 4;
switch (Task_This)
{
case 0:
if (Mode == Set_ID_Mode)
{
if (moto.postion > 4500 || moto.postion < 3500)
moto.set_postion = 4000;
else
Task_This++;
}
break;
case 1:
#define Up_Gear 4000
#define Zero_Gear 0
#define Down_Gear -4000
#define Gear_Offset 2500
if (moto.postion > (Up_Gear - Gear_Offset) && moto.postion < (Up_Gear + Gear_Offset) && moto.set_postion != Up_Gear)
moto.set_postion = (Up_Gear);
if (moto.postion > (Zero_Gear - Gear_Offset) && moto.postion < (Zero_Gear + Gear_Offset) && moto.set_postion != Zero_Gear)
moto.set_postion = (Zero_Gear);
if (moto.postion > (Down_Gear - Gear_Offset) && moto.postion < (Down_Gear + Gear_Offset) && moto.set_postion != Down_Gear)
moto.set_postion = (Down_Gear);
if (Mode != Set_ID_Mode)
Task_This = 0; // 返回等待状态
break;
default:
Get_Delay();
break;
} 这部分代码中，定义了几个位置作为开关的中心位置。然后，通过使用一个较小力矩的PID（即欠调参数，人能拧动，但是保持有力矩），在超过边界后将设定位置切换到下一个开关中心位置，以实现阻拦->牵引的变化过程，通过调节PID的参数，即可实现一个多段开关的效果。 这个也是电机中的一种交互输入模式，类似旋钮开关。其他的模式多是一些输出场景，这个模式按道理来说是可以用来设定参数的，不过应用上我并没有深入开发，这等待大家自行探索了。
{:victory:}
页:
[1]