📄 drv_s_motor.c
字号:
** 输 出: 无
** 全局变量: 无
** 调用模块: 无
**
** 作 者: 龚树强
** 日 期: 2005年5月12日
**-------------------------------------------------------------------------------------------------------
** 修改人:
** 日 期:
**-------------------------------------------------------------------------------------------------------
********************************************************************************************************/
void StepMotor0SetVelocity( INT16S vt)
{
INT32U tmp;
INT8U i;
static INT16S buf_vt;
if(buf_vt != vt)
{
buf_vt = vt;
if( vt > 0 )
DIR_SIG0_SET();
else
{
vt = -vt;
DIR_SIG0_CLR();
}
if(vt != 0)
{
tmp = SYSCLK / 2 /vt/STEP_MOTOR_CONTROL_PERIOD_DIVIDER/PWM_REPEATE_TIMES;
for(i = 0;i < 8 ;i++)
{
if(tmp > 0xFFFF)
tmp /= 2;
else
break;
}
if(i<8)
{
T2CON &= ~TENABLE;
T2CON &= (~(TPS2|TPS1|TPS0));
T2CON |= i << 8;
T2PER = ( SYSCLK >> (i+1)) / vt /STEP_MOTOR_CONTROL_PERIOD_DIVIDER/PWM_REPEATE_TIMES ;
T2CON |= TENABLE;
}
else
{
T2CON &= ~TENABLE;
T2CON |= TPS2|TPS1|TPS0;
T2PER = 0xFFFF ;
T2CON |= TENABLE;
}
}
}
}
/*********************************************************************************************************
** 函数名称: Task_StepMotor
** 功能描述: StepMotor控制任务函数
** 输 入: 无
** 输 出: 无
** 全局变量: 无
** 调用模块: 无
**
** 作 者: 龚树强
** 日 期: 2005年4月22日
**-------------------------------------------------------------------------------------------------------
** 修改人:
** 日 期:
**-------------------------------------------------------------------------------------------------------
********************************************************************************************************/
void Task_StepMotor(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;
STEPMOTOR_PROPERTY buf_StepMotorSetting;
STEPMOTOR_CTRL * pStepMotor_Ctrl;
pStepMotor_Ctrl = (STEPMOTOR_CTRL *)pData;
buf_StepMotorSetting .dir = DIRECTION_FORWARD ;
buf_StepMotorSetting .dV = 1;
buf_StepMotorSetting .Vm = 0;
buf_StepMotorSetting .St = 0;
pStepMotor_Ctrl->property .Vt = 0;
pStepMotor_Ctrl->property .mode = 0;
do
{
ptr = OSMboxPend(pStepMotor_Ctrl->api.Mbox, STEP_MOTOR_CONTROL_PERIOD ,&err);
if(err == OS_NO_ERR )
{
temp = *((INT8U *)ptr + POINTER_LENGTH);
switch(temp)
{
case STEPMOTOR_KILL_TASK:
kill_task = 1;
break;
case STEPMOTOR_SET_DIRECTION:
switch(*(INT16U *)((INT8U *)ptr + POINTER_LENGTH + sizeof(INT16U)))
{
case 'F' :
case 'f' : buf_StepMotorSetting .dir = DIRECTION_FORWARD;
break;
case 'B' :
case 'b' : buf_StepMotorSetting .dir = DIRECTION_BACKWARD;
break;
default : break;
}
break;
case STEPMOTOR_SET_ACCELERATE:
buf_StepMotorSetting .dV = *(INT16U *)((INT8U *)ptr + POINTER_LENGTH + sizeof(INT16U));
break;
case STEPMOTOR_SET_MAX_VELOCITY:
buf_StepMotorSetting .Vm = *(INT16U *)((INT8U *)ptr + POINTER_LENGTH + sizeof(INT16U));
break;
case STEPMOTOR_SET_MAX_STEPS:
buf_StepMotorSetting .St = *(INT16U *)((INT8U *)ptr + POINTER_LENGTH + sizeof(INT16U));
break;
case STEPMOTOR_ACTION:
if(( buf_StepMotorSetting .St > 0 ) && (pStepMotor_Ctrl->property .Vt == 0)) //只有当当前速度为0时，才允许设定位移
{
pStepMotor_Ctrl->property .dir = buf_StepMotorSetting .dir;
pStepMotor_Ctrl->property .dV = buf_StepMotorSetting .dV;
pStepMotor_Ctrl->property .Sr = 0;
pStepMotor_Ctrl->property .mode = STEP_MOTOR_ACTION;
if(buf_StepMotorSetting .dir == DIRECTION_FORWARD)
{
pStepMotor_Ctrl->property .St = buf_StepMotorSetting .St;
if( ( ((buf_StepMotorSetting .Vm) / (buf_StepMotorSetting .dV) + 1 )* (buf_StepMotorSetting .Vm)) <= ( buf_StepMotorSetting .St ) )
{
pStepMotor_Ctrl->property .Vm = buf_StepMotorSetting .Vm;
threadhold_s = buf_StepMotorSetting .St - ((buf_StepMotorSetting) .Vm / (buf_StepMotorSetting) .dV + 1 ) * ((buf_StepMotorSetting) .Vm) / 2;
}
else
{
INT8U i;
for(i = (buf_StepMotorSetting) .Vm / (buf_StepMotorSetting) .dV;i--;)
{
if(( (i * (i+1) )* (buf_StepMotorSetting) .dV ) < buf_StepMotorSetting .St )
break;
}
pStepMotor_Ctrl->property .Vm = buf_StepMotorSetting .dV * i ;
threadhold_s = buf_StepMotorSetting .St - ( i * (i+1) ) *(buf_StepMotorSetting) .dV / 2;
}
}
else
{
pStepMotor_Ctrl->property .St = -buf_StepMotorSetting .St;
if( ( ((buf_StepMotorSetting .Vm) / (buf_StepMotorSetting .dV) + 1 )* (buf_StepMotorSetting .Vm) ) <= (buf_StepMotorSetting .St ) )
{
pStepMotor_Ctrl->property .Vm = -buf_StepMotorSetting .Vm;
threadhold_s = -buf_StepMotorSetting .St + ((buf_StepMotorSetting) .Vm / (buf_StepMotorSetting) .dV + 1 ) * (buf_StepMotorSetting .Vm) / 2;
}
else
{
INT8U i;
for(i = (buf_StepMotorSetting) .Vm / (buf_StepMotorSetting) .dV;i--;)
{
if(( (i * (i+1) ) *(buf_StepMotorSetting) .dV ) < buf_StepMotorSetting .St )
break;
}
pStepMotor_Ctrl->property .Vm = -buf_StepMotorSetting .dV * i ;
threadhold_s = -buf_StepMotorSetting .St + ( i * (i+1)) *(buf_StepMotorSetting) .dV / 2;
}
}
Vm = (pStepMotor_Ctrl->property) .Vm;
St = threadhold_s + Vm;
}
else if(buf_StepMotorSetting .St == 0)
{
pStepMotor_Ctrl->property .dir = buf_StepMotorSetting .dir;
pStepMotor_Ctrl->property .dV = buf_StepMotorSetting .dV;
pStepMotor_Ctrl->property .Sr = 0;
pStepMotor_Ctrl->property .mode = STEP_MOTOR_ACTION;
if(buf_StepMotorSetting .dir == DIRECTION_FORWARD)
{
pStepMotor_Ctrl->property .St = buf_StepMotorSetting .St;
pStepMotor_Ctrl->property .Vm = buf_StepMotorSetting .Vm;
}
else
{
pStepMotor_Ctrl->property .St = -buf_StepMotorSetting .St;
pStepMotor_Ctrl->property .Vm = -buf_StepMotorSetting .Vm;
}
St = pStepMotor_Ctrl->property .St;
Vm = pStepMotor_Ctrl->property .Vm;
}
Sr = 0;
break;
case STEPMOTOR_HALT :
pStepMotor_Ctrl->property .mode = STEP_MOTOR_HALT;
pStepMotor_Ctrl->property .dir = 0;
pStepMotor_Ctrl->property .dV = 0;
pStepMotor_Ctrl->property .Sr = 0;
pStepMotor_Ctrl->property .St = 0;
pStepMotor_Ctrl->property .Vt = 0;
pStepMotor_Ctrl->property .Vm = 0;
buf_StepMotorSetting .St = 0;
buf_StepMotorSetting .Vm = 0;
buf_StepMotorSetting .Sr = 0;
buf_StepMotorSetting .Vt = 0;
break;
default :
break;
}
OSMemPut( SysBuf_2, (void *)ptr);
OSSemPost(SysBufSem_2);
OSSemPost(pStepMotor_Ctrl->api.MboxSem);
}
else if( err == OS_TIMEOUT)
{
if(pStepMotor_Ctrl->property .mode == STEP_MOTOR_ACTION)
{
Vt = (pStepMotor_Ctrl->property) .Vt;
dV = (pStepMotor_Ctrl->property) .dV;
Sr = (pStepMotor_Ctrl->property) .Sr;
if(pStepMotor_Ctrl->property .St)
{
if(pStepMotor_Ctrl->property .dir == DIRECTION_FORWARD)
{
if(pStepMotor_Ctrl->property .Sr > threadhold_s)
{
St += (pStepMotor_Ctrl->property) .Vm;
if( (pStepMotor_Ctrl->property) .Vm >= dV )
(pStepMotor_Ctrl->property) .Vm -= dV;
else
(pStepMotor_Ctrl->property) .Vm = 0;
(pStepMotor_Ctrl->property) .Vt = (St - Sr);
if((pStepMotor_Ctrl->property) .Vt > Vm )
(pStepMotor_Ctrl->property) .Vt = Vm ;
pStepMotor_Ctrl->func .SetVelocity(pStepMotor_Ctrl->property .Vt) ;
}
else
{
if( Vt <= (Vm - dV))
(pStepMotor_Ctrl->property) .Vt += (pStepMotor_Ctrl->property) .dV;
else if (Vt >= (Vm + dV))
(pStepMotor_Ctrl->property) .Vt -= (pStepMotor_Ctrl->property) .dV;
else
(pStepMotor_Ctrl->property) .Vt = (pStepMotor_Ctrl->property) .Vm;
pStepMotor_Ctrl->func .SetVelocity(pStepMotor_Ctrl->property .Vt) ;
}
}
else
{
if(pStepMotor_Ctrl->property .Sr < threadhold_s)
{
St += (pStepMotor_Ctrl->property) .Vm;
if( (pStepMotor_Ctrl->property) .Vm <= (-dV) )
(pStepMotor_Ctrl->property) .Vm += dV;
else
(pStepMotor_Ctrl->property) .Vm = 0;
(pStepMotor_Ctrl->property) .Vt = (St - Sr);
if((pStepMotor_Ctrl->property) .Vt < Vm )
(pStepMotor_Ctrl->property) .Vt = Vm ;
pStepMotor_Ctrl->func .SetVelocity(pStepMotor_Ctrl->property .Vt) ;
}
else
{
if( Vt <= ( Vm - dV))
(pStepMotor_Ctrl->property) .Vt += (pStepMotor_Ctrl->property) .dV;
else if ( Vt >= ( Vm + dV) )
(pStepMotor_Ctrl->property) .Vt -= (pStepMotor_Ctrl->property) .dV;
else
(pStepMotor_Ctrl->property) .Vt = (pStepMotor_Ctrl->property) .Vm;
pStepMotor_Ctrl->func .SetVelocity(pStepMotor_Ctrl->property .Vt) ;
}
}
}
else
{
if( Vt <= ( Vm - dV))
(pStepMotor_Ctrl->property) .Vt += (pStepMotor_Ctrl->property) .dV;
else if ( Vt >= ( Vm + dV))
(pStepMotor_Ctrl->property) .Vt -= (pStepMotor_Ctrl->property) .dV;
else
(pStepMotor_Ctrl->property) .Vt = (pStepMotor_Ctrl->property) .Vm;
pStepMotor_Ctrl->func .SetVelocity(pStepMotor_Ctrl->property .Vt) ;
}
}
}
}while(!kill_task);
pStepMotor_Ctrl->func .SetVelocity(0);
pStepMotor_Ctrl->api.ObjLoaded = OBJ_UNLOADED;
OSSemDel(pStepMotor_Ctrl->api.MboxSem,OS_DEL_ALWAYS,&err);
OSMboxDel(pStepMotor_Ctrl->api.Mbox,OS_DEL_ALWAYS,&err);
OSTaskDel(OS_PRIO_SELF);
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -