📄 drv_motor.c
字号:
{
temp_para = ((PARAMETER_TBL *)((PARAMETER_PACKAGE * )pData)->ppara)->para_name[i];
switch(temp_para)
{
case DC_MOTOR_SELECT_0 :
selected_obj |= DC_MOTOR0_SELECTED;
break;
case DC_MOTOR_SELECT_1 :
selected_obj |= DC_MOTOR1_SELECTED;
break;
default : break;
}
}
if(selected_obj == NULL)
{
selected_obj |= DC_MOTOR0_SELECTED;
selected_obj |= DC_MOTOR1_SELECTED;
}
if(selected_obj & DC_MOTOR0_SELECTED)
{
if(DCMotor0_Ctrl.api.ObjLoaded != OBJ_UNLOADED)
{
OSSemPend(SysBufSem_2,0,err);
pData = (INT8U * )OSMemGet(SysBuf_2,err);
*((INT8U *)pData + POINTER_LENGTH) = DC_MOTOR_KILL_TASK ;
OSMboxPost(DCMotor0_Ctrl.api.Mbox,pData);
}
else
{
*err = ERROR_CMD_REJECTED;
return;
}
}
}
/*********************************************************************************************************
** 函数名称: DCMotorSet
** 功能描述: DCMotor属性调整函数
** 输 入: 无
** 输 出: 无
** 全局变量: 无
** 调用模块: 无
**
** 作 者: 龚树强
** 日 期: 2005年4月22日
**-------------------------------------------------------------------------------------------------------
** 修改人:
** 日 期:
**-------------------------------------------------------------------------------------------------------
********************************************************************************************************/
void DCMotorSet(void * pData,INT8U * err)
{
INT8U para_num;
INT8U i;
INT8U selected_obj;
INT8U temp_para;
INT8U *pMsg;
para_num = ((PARAMETER_PACKAGE *)pData)->para_num;
selected_obj = NULL;
selected_obj |= DC_MOTOR0_SELECTED; //假如前面的参数没指定对象的话，默认是全部对象
selected_obj |= DC_MOTOR1_SELECTED;
for(i = 0; i<para_num ;i++)
{
temp_para = ((PARAMETER_TBL *)((PARAMETER_PACKAGE * )pData)->ppara)->para_name[i];
switch(temp_para)
{
case DC_MOTOR_SELECT_0 :
selected_obj &= ~DC_MOTOR1_SELECTED;
selected_obj |= DC_MOTOR0_SELECTED;
break;
case DC_MOTOR_SELECT_1 :
selected_obj &= ~DC_MOTOR0_SELECTED;
selected_obj |= DC_MOTOR1_SELECTED;
break;
default :
if(selected_obj & DC_MOTOR0_SELECTED)
{
switch(temp_para)
{
case DC_MOTOR_SET_MODE :
case DC_MOTOR_CHANGE_PARAM_I :
case DC_MOTOR_CHANGE_PARAM_N :
case DC_MOTOR_SET_I :
case DC_MOTOR_SET_P :
case DC_MOTOR_SET_DIRECTION :
case DC_MOTOR_SET_MAX_VELOCITY :
case DC_MOTOR_SET_ACCELERATE :
case DC_MOTOR_SET_MAX_S :
OSSemPend(SysBufSem_2,0,err);
pMsg = (INT8U * )OSMemGet(SysBuf_2,err);
* ((INT8U *)pMsg + POINTER_LENGTH) = temp_para ;
* (INT16U *)((INT8U *)pMsg + POINTER_LENGTH + sizeof(INT8U) ) = ((PARAMETER_TBL *)((PARAMETER_PACKAGE * )pData)->ppara)->para_value[i];
OSSemPend(DCMotor0_Ctrl.api.MboxSem ,0 ,err);
OSMboxPost(DCMotor0_Ctrl.api.Mbox,pMsg);
break;
default :
break;
}
}
break;
}
}
}
/*********************************************************************************************************
** 函数名称: DCMotorTest
** 功能描述: DCMotor测试函数
** 输 入: 无
** 输 出: 无
** 全局变量: 无
** 调用模块: 无
**
** 作 者: 龚树强
** 日 期: 2005年4月22日
**-------------------------------------------------------------------------------------------------------
** 修改人:
** 日 期:
**-------------------------------------------------------------------------------------------------------
********************************************************************************************************/
void DCMotorTest(void * pData,INT8U * err)
{
DCMotorLoad(pData,err);
OSTimeDly(OS_TICKS_PER_SEC * 2);
DCMotorUnLoad(pData,err);
}
/*********************************************************************************************************
** 函数名称: DCMotorDeamon
** 功能描述: DCMotor任务守护函数
** 输 入: 无
** 输 出: 无
** 全局变量: 无
** 调用模块: 无
**
** 作 者: 龚树强
** 日 期: 2005年4月22日
**-------------------------------------------------------------------------------------------------------
** 修改人:
** 日 期:
**-------------------------------------------------------------------------------------------------------
********************************************************************************************************/
void DCMotorDeamon(void * pData,INT8U * err)
{
INT32S tmp;
OSSemPend(Ocb_Disp.api.IdleSem,0,err);
// Ocb_Disp.op->clr(0,0,128,8,LCM_NORMAL_DISP);
Ocb_Disp.op->disp_char(0,0,(INT8U *)("DC Motor Monitor"),LCM_NORMAL_DISP);
// Ocb_Disp.op->disp_char(0,3,(INT8U *)(" Pos :"),LCM_NORMAL_DISP);
// Ocb_Disp.op->disp_int(40,3,(DCMotor0_Ctrl.property.Sr)>>7,LCM_NORMAL_DISP);
// Ocb_Disp.op->disp_char(0,5,(INT8U *)(" I :"),LCM_NORMAL_DISP);
// tmp =((((INT32S)(DCMotor0_Ctrl.property.Ir))*165/10)>>5);
// Ocb_Disp.op->disp_float(40,5,tmp,10,LCM_NORMAL_DISP);
Ocb_Disp.op->disp_int(40,1,N_Ref,LCM_NORMAL_DISP);
Ocb_Disp.op->disp_char(0,1,(INT8U *)(" N_Ref :"),LCM_NORMAL_DISP);
Ocb_Disp.op->disp_int(40,2,N_Ek,LCM_NORMAL_DISP);
Ocb_Disp.op->disp_char(0,2,(INT8U *)(" N_Ek :"),LCM_NORMAL_DISP);
Ocb_Disp.op->disp_int(40,3,(DCMotor0_Ctrl.property.Vr),LCM_NORMAL_DISP);
Ocb_Disp.op->disp_char(0,3,(INT8U *)(" Spd :"),LCM_NORMAL_DISP);
// Ocb_Disp.op->disp_char(0,3,(INT8U *)(" N_U :"),LCM_NORMAL_DISP);
// Ocb_Disp.op->disp_int(40,3,N_U,LCM_NORMAL_DISP);
Ocb_Disp.op->disp_int(40,4,I_Ref,LCM_NORMAL_DISP);
Ocb_Disp.op->disp_char(0,4,(INT8U *)(" I_Ref :"),LCM_NORMAL_DISP);
Ocb_Disp.op->disp_int(40,5,(INT32S)DCMotor0_Ctrl.property.Ir,LCM_NORMAL_DISP);
Ocb_Disp.op->disp_char(0,5,(INT8U *)(" Ir :"),LCM_NORMAL_DISP);
// tmp =((((INT32S)DCMotor0_Ctrl.property.Ir)*337/100)>>4);
// Ocb_Disp.op->disp_float(40,2,tmp,12,LCM_NORMAL_DISP);
Ocb_Disp.op->disp_int(40,6,I_Ek,LCM_NORMAL_DISP);
Ocb_Disp.op->disp_char(0,6,(INT8U *)(" I_Ek :"),LCM_NORMAL_DISP);
OSSemPost(Ocb_Disp.api.IdleSem);
}
/*******************************************************************************
********************* 任 务 定 义 ************************
********************************************************************************/
/*********************************************************************************************************
** 函数名称: DCMotorSetVelocity
** 功能描述: 直流电机速度设置函数
** 输 入: 无
** 输 出: 无
** 全局变量: 无
** 调用模块: 无
**
** 作 者: 龚树强
** 日 期: 2005年5月12日
**-------------------------------------------------------------------------------------------------------
** 修改人:
** 日 期:
**-------------------------------------------------------------------------------------------------------
********************************************************************************************************/
void DCMotor0SetVelocity( INT16S vt)
{
N_Ref = vt;
}
/*********************************************************************************************************
** 函数名称: ISR_DCMotor
** 功能描述: DCMotor周期中断控制函数
** 输 入: 无
** 输 出: 无
** 全局变量: 无
** 调用模块: 无
**
** 作 者: 龚树强
** 日 期: 2005年4月22日
**-------------------------------------------------------------------------------------------------------
** 修改人:
** 日 期:
**-------------------------------------------------------------------------------------------------------
********************************************************************************************************/
void ISR_DCMotor0(void)
{
static INT8U pwm_counter = 0;
static INT16S speed;
INT16U tmp;
INT16S stmp;
INT32S sltmp;
#if DC_MOTOR_TEST_EN > 0
static INT16U test;
static INT16U dir;
static INT8U state;
#endif
EVBIFRA |= T3PINT_FLAG;
// ADCTRL2 |= INT_FLAG_SEQ2|RST_SEQ2; //写1清0，清中断标志位，并复位SEQ2
stmp = 0x8000 - (INT16S)gADC_BUF;
stmp >>= 3;
DCMotor0_Ctrl.property.Ir = stmp;
I_Ek = I_Ref - stmp; /*Q12格式*/
I_U = I_Rk + (((INT32S)I_Ek * I_Kp)>>12); /*Q12格式*/
if(I_U > (1<<12))
stmp = 1<<12;
else if(I_U < ((-1)<<12))
stmp = (-1)<<12;
else
stmp = I_U;
I_Rk += (((INT32S)I_Kc * ( stmp - I_U ))>>12);
I_Rk += ((INT32S)I_Ki * I_Ek)>>12;
#if DC_MOTOR_TEST_EN > 0
CMPR4 = test;
if(test == T3PER)
dir = 0;
else if(test == 0)
dir = 1;
if(dir)
test ++ ;
else
test -- ;
#else
CMPR4 = ((INT32S)( stmp + (1<<12)) * I_U_Max)>>12;
#endif
// encoder_tmp = T4CNT;
// encoder_tmp = ExtQEP0Reg;
// encoder_tmp -= encoder_buf;
// encoder_buf = T4CNT;
// ExtQEP0Reg = 0;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -