📄 drv_motor.c
字号:
stmp = ExtQEP0Reg;
ExtQEP0Reg = 0; /*清空扩展QEP模块的计数寄存器*/
speed += stmp;
if((pwm_counter--) == 0)
{
pwm_counter = CURRENT_MODULATE_CYCLE;
// pwm_counter = CURRENT_MODULATE_CYCLE * OS_TICKS_PER_SEC * 2;
// I_Ref = - I_Ref;
N_Ek = N_Ref - speed; /*Q12格式*/
DCMotor0_Ctrl.property.Vr = speed;
DCMotor0_Ctrl.property.Sr += speed;
speed = 0;
N_U = N_Rk + ((INT32S)N_Ek * N_Kp)>>12; /*Q12格式*/
if(N_U > N_U_Max)
sltmp = N_U_Max;
else if(N_U < N_U_Min)
sltmp = N_U_Min;
else
sltmp = N_U;
N_Rk += ((INT32S)N_Kc * ( sltmp - N_U ))>>12; /*Q12格式*/
N_Rk += ((INT32S)N_Ki * N_Ek)>>12;
I_Ref = sltmp ;
// I_Ref = 800 ;
}
}
/*********************************************************************************************************
** 函数名称: Task_DCMotor
** 功能描述: DCMotor控制任务函数
** 输 入: 无
** 输 出: 无
** 全局变量: 无
** 调用模块: 无
**
** 作 者: 龚树强
** 日 期: 2005年4月22日
**-------------------------------------------------------------------------------------------------------
** 修改人:
** 日 期:
**-------------------------------------------------------------------------------------------------------
********************************************************************************************************/
void Task_DCMotor(void *pData)
{
INT8U err = OS_NO_ERR;
INT8U i;
INT8U temp;
void *ptr;
INT8U kill_task = 0;
INT16S threadhold_s;
INT16S Vt,Vm,dV,St,Sr;
static INT8U selected_param = DC_MOTOR_CHANGE_PARAM_N;
DC_MOTOR_PROPERTY buf_DCMotorSetting;
DC_MOTOR_CTRL * pDCMotor_Ctrl;
pDCMotor_Ctrl = (DC_MOTOR_CTRL *)pData;
buf_DCMotorSetting .dir = DIRECTION_FORWARD ;
buf_DCMotorSetting .dV = 10;
buf_DCMotorSetting .Vm = 0;
buf_DCMotorSetting .St = 0;
pDCMotor_Ctrl->property .Sr = 0;
pDCMotor_Ctrl->property .Vt = 0;
pDCMotor_Ctrl->property .mode = 0;
do
{
ptr = OSMboxPend(pDCMotor_Ctrl->api.Mbox, DC_MOTOR_CONTROL_CYCLE ,&err);
if(err == OS_NO_ERR )
{
temp = *((INT8U *)ptr + POINTER_LENGTH);
switch(temp)
{
case DC_MOTOR_KILL_TASK:
kill_task = 1;
break;
case DC_MOTOR_CHANGE_PARAM_I:
selected_param = DC_MOTOR_CHANGE_PARAM_I;
break;
case DC_MOTOR_CHANGE_PARAM_N:
selected_param = DC_MOTOR_CHANGE_PARAM_N;
break;
case DC_MOTOR_SET_I :
if(selected_param == DC_MOTOR_CHANGE_PARAM_I)
tmp_I_Ki= *(INT16U *)((INT8U *)ptr + POINTER_LENGTH + sizeof(INT16U));
else if(selected_param == DC_MOTOR_CHANGE_PARAM_N)
tmp_N_Ki= *(INT16U *)((INT8U *)ptr + POINTER_LENGTH + sizeof(INT16U));
break;
case DC_MOTOR_SET_P :
if(selected_param == DC_MOTOR_CHANGE_PARAM_I)
tmp_I_Kp= *(INT16U *)((INT8U *)ptr + POINTER_LENGTH + sizeof(INT16U));
else if(selected_param == DC_MOTOR_CHANGE_PARAM_N)
tmp_N_Kp= *(INT16U *)((INT8U *)ptr + POINTER_LENGTH + sizeof(INT16U));
break;
case DC_MOTOR_SET_DIRECTION:
switch(*(INT16U *)((INT8U *)ptr + POINTER_LENGTH + sizeof(INT16U)))
{
case 'F' :
case 'f' : buf_DCMotorSetting .dir = DIRECTION_FORWARD;
break;
case 'B' :
case 'b' : buf_DCMotorSetting .dir = DIRECTION_BACKWARD;
break;
default : break;
}
break;
case DC_MOTOR_SET_ACCELERATE:
buf_DCMotorSetting .dV = *(INT16U *)((INT8U *)ptr + POINTER_LENGTH + sizeof(INT16U));
break;
case DC_MOTOR_SET_MAX_VELOCITY:
buf_DCMotorSetting .Vm = *(INT16U *)((INT8U *)ptr + POINTER_LENGTH + sizeof(INT16U));
break;
case DC_MOTOR_SET_MAX_S:
buf_DCMotorSetting .St = *(INT16U *)((INT8U *)ptr + POINTER_LENGTH + sizeof(INT16U));
break;
case DC_MOTOR_ACTION:
I_Kp = tmp_I_Kp ; //电流调节比例系数 (Q12格式)
I_Ki = tmp_I_Ki ; //电流调节积分系数 (Q12格式)
I_Kc = ((INT32S)I_Ki<<12)/I_Kp ; //电流调节积分饱和修正系数 (Q12格式)
N_Kp = tmp_N_Kp ; //电流调节比例系数 (Q12格式)
N_Ki = tmp_N_Ki ; //电流调节积分系数 (Q12格式)
N_Kc = ((INT32S)N_Ki<<12)/N_Kp ; //电流调节积分饱和修正系数 (Q12格式)
if(( buf_DCMotorSetting .St > 0 ) && (pDCMotor_Ctrl->property .Vt == 0)) //只有当当前速度为0时，才允许设定位移
{
pDCMotor_Ctrl->property .dir = buf_DCMotorSetting .dir;
pDCMotor_Ctrl->property .dV = buf_DCMotorSetting .dV;
pDCMotor_Ctrl->property .Sr = 0;
pDCMotor_Ctrl->property .mode = DC_MOTOR_ACTION;
if(buf_DCMotorSetting .dir == DIRECTION_FORWARD)
{
pDCMotor_Ctrl->property .St = buf_DCMotorSetting .St;
if( ( ((buf_DCMotorSetting .Vm) / (buf_DCMotorSetting .dV) + 1 )* (buf_DCMotorSetting .Vm)) <= ( buf_DCMotorSetting .St ) )
{
pDCMotor_Ctrl->property .Vm = buf_DCMotorSetting .Vm;
threadhold_s = buf_DCMotorSetting .St - ((buf_DCMotorSetting) .Vm / (buf_DCMotorSetting) .dV + 1 ) * ((buf_DCMotorSetting) .Vm) / 2;
}
else
{
INT8U i;
for(i = (buf_DCMotorSetting) .Vm / (buf_DCMotorSetting) .dV;i--;)
{
if(( (i * (i+1) )* (buf_DCMotorSetting) .dV ) < buf_DCMotorSetting .St )
break;
}
pDCMotor_Ctrl->property .Vm = buf_DCMotorSetting .dV * i ;
threadhold_s = buf_DCMotorSetting .St - ( i * (i+1) ) *(buf_DCMotorSetting) .dV / 2;
}
}
else
{
pDCMotor_Ctrl->property .St = -buf_DCMotorSetting .St;
if( ( ((buf_DCMotorSetting .Vm) / (buf_DCMotorSetting .dV) + 1 )* (buf_DCMotorSetting .Vm) ) <= (buf_DCMotorSetting .St ) )
{
pDCMotor_Ctrl->property .Vm = -buf_DCMotorSetting .Vm;
threadhold_s = -buf_DCMotorSetting .St + ((buf_DCMotorSetting) .Vm / (buf_DCMotorSetting) .dV + 1 ) * (buf_DCMotorSetting .Vm) / 2;
}
else
{
INT8U i;
for(i = (buf_DCMotorSetting) .Vm / (buf_DCMotorSetting) .dV;i--;)
{
if(( (i * (i+1) ) *(buf_DCMotorSetting) .dV ) < buf_DCMotorSetting .St )
break;
}
pDCMotor_Ctrl->property .Vm = -buf_DCMotorSetting .dV * i ;
threadhold_s = -buf_DCMotorSetting .St + ( i * (i+1)) *(buf_DCMotorSetting) .dV / 2;
}
}
Vm = (pDCMotor_Ctrl->property) .Vm;
St = threadhold_s + Vm;
}
else if(buf_DCMotorSetting .St == 0)
{
pDCMotor_Ctrl->property .dir = buf_DCMotorSetting .dir;
pDCMotor_Ctrl->property .dV = buf_DCMotorSetting .dV;
pDCMotor_Ctrl->property .Sr = 0;
pDCMotor_Ctrl->property .mode = DC_MOTOR_ACTION;
if(buf_DCMotorSetting .dir == DIRECTION_FORWARD)
{
pDCMotor_Ctrl->property .St = buf_DCMotorSetting .St;
pDCMotor_Ctrl->property .Vm = buf_DCMotorSetting .Vm;
}
else
{
pDCMotor_Ctrl->property .St = -buf_DCMotorSetting .St;
pDCMotor_Ctrl->property .Vm = -buf_DCMotorSetting .Vm;
}
St = pDCMotor_Ctrl->property .St;
Vm = pDCMotor_Ctrl->property .Vm;
}
Sr = 0;
break;
case DC_MOTOR_HALT :
pDCMotor_Ctrl->property .mode = DC_MOTOR_HALT;
pDCMotor_Ctrl->property .dir = 0;
pDCMotor_Ctrl->property .dV = 0;
pDCMotor_Ctrl->property .Sr = 0;
pDCMotor_Ctrl->property .St = 0;
pDCMotor_Ctrl->property .Vt = 0;
pDCMotor_Ctrl->property .Vm = 0;
buf_DCMotorSetting .St = 0;
buf_DCMotorSetting .Vm = 0;
buf_DCMotorSetting .Sr = 0;
buf_DCMotorSetting .Vt = 0;
break;
default :
break;
}
OSMemPut( SysBuf_2, (void *)ptr);
OSSemPost(SysBufSem_2);
OSSemPost(pDCMotor_Ctrl->api.MboxSem);
}
else if( err == OS_TIMEOUT)
{
if(pDCMotor_Ctrl->property .mode == DC_MOTOR_ACTION)
{
Vt = (pDCMotor_Ctrl->property) .Vt;
dV = (pDCMotor_Ctrl->property) .dV;
Sr = (pDCMotor_Ctrl->property) .Sr;
if(pDCMotor_Ctrl->property .St)
{
if(pDCMotor_Ctrl->property .dir == DIRECTION_FORWARD)
{
if(pDCMotor_Ctrl->property .Sr > threadhold_s)
{
St += (pDCMotor_Ctrl->property) .Vm;
if( (pDCMotor_Ctrl->property) .Vm >= dV )
(pDCMotor_Ctrl->property) .Vm -= dV;
else
(pDCMotor_Ctrl->property) .Vm = 0;
(pDCMotor_Ctrl->property) .Vt = (St - Sr);
if((pDCMotor_Ctrl->property) .Vt > Vm )
(pDCMotor_Ctrl->property) .Vt = Vm ;
pDCMotor_Ctrl->func .SetVelocity(pDCMotor_Ctrl->property .Vt) ;
}
else
{
if( Vt <= (Vm - dV))
(pDCMotor_Ctrl->property) .Vt += (pDCMotor_Ctrl->property) .dV;
else if (Vt >= (Vm + dV))
(pDCMotor_Ctrl->property) .Vt -= (pDCMotor_Ctrl->property) .dV;
else
(pDCMotor_Ctrl->property) .Vt = (pDCMotor_Ctrl->property) .Vm;
pDCMotor_Ctrl->func .SetVelocity(pDCMotor_Ctrl->property .Vt) ;
}
}
else
{
if(pDCMotor_Ctrl->property .Sr < threadhold_s)
{
St += (pDCMotor_Ctrl->property) .Vm;
if( (pDCMotor_Ctrl->property) .Vm <= (-dV) )
(pDCMotor_Ctrl->property) .Vm += dV;
else
(pDCMotor_Ctrl->property) .Vm = 0;
(pDCMotor_Ctrl->property) .Vt = (St - Sr);
if((pDCMotor_Ctrl->property) .Vt < Vm )
(pDCMotor_Ctrl->property) .Vt = Vm ;
pDCMotor_Ctrl->func .SetVelocity(pDCMotor_Ctrl->property .Vt) ;
}
else
{
if( Vt <= ( Vm - dV))
(pDCMotor_Ctrl->property) .Vt += (pDCMotor_Ctrl->property) .dV;
else if ( Vt >= ( Vm + dV) )
(pDCMotor_Ctrl->property) .Vt -= (pDCMotor_Ctrl->property) .dV;
else
(pDCMotor_Ctrl->property) .Vt = (pDCMotor_Ctrl->property) .Vm;
pDCMotor_Ctrl->func .SetVelocity(pDCMotor_Ctrl->property .Vt) ;
}
}
}
else
{
if( Vt <= ( Vm - dV))
(pDCMotor_Ctrl->property) .Vt += (pDCMotor_Ctrl->property) .dV;
else if ( Vt >= ( Vm + dV))
(pDCMotor_Ctrl->property) .Vt -= (pDCMotor_Ctrl->property) .dV;
else
(pDCMotor_Ctrl->property) .Vt = (pDCMotor_Ctrl->property) .Vm;
pDCMotor_Ctrl->func .SetVelocity(pDCMotor_Ctrl->property .Vt) ;
}
}
}
}while(!kill_task);
pDCMotor_Ctrl->func .SetVelocity(0);
pDCMotor_Ctrl->api.ObjLoaded = OBJ_UNLOADED;
OSSemDel(pDCMotor_Ctrl->api.MboxSem,OS_DEL_ALWAYS,&err);
OSMboxDel(pDCMotor_Ctrl->api.Mbox,OS_DEL_ALWAYS,&err);
OSTaskDel(OS_PRIO_SELF);
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -