📄 motor_drv.c
字号:
#include "regw79e800.h"
#include "ctype.h"
#include "pindef.h"
#include "external.h"
#include "Motor_drv.h"
Byte DATA Motor1_step_index,StepNumber1,Motor1StepCyc,Motor1Run,Motor1_Dir;
Byte DATA Motor2_step_index,StepNumber2,Motor2StepCyc,Motor2Run,Motor2_Dir;
Byte DATA RunSpeed,x_value,y_value,x_max,y_max;
Byte DATA CommandType,Adjust_step, Rem_Adjust_step,ToCenterStep,RemToCenterStep,ActionType;
Bool Motor1_StartRun,Motor2_StartRun,Motor1_Direct,Motor2_Direct,Motor1_StopFlag,Motor2_StopFlag;
/*
when in left side,x_value =0;
when in down side, y_value = 0;
*/
void Motor_RunWayInit(Byte steps,bit mode,Byte direct)
{
Motor1_Direct = mode;
if(direct <=2)
{
StepNumber1 = steps;
Motor1_Dir = direct;
CommandType |=MotorRunHorizonValue;
Motor1_StartRun = TRUE;
} else
{
StepNumber2 = steps;
Motor2_Dir = direct;
CommandType |=MotorRunVeritcalValue;
Motor2_StartRun = TRUE;
}
}
#if 0
/*---------------------------------------------------
INPUT: steps/基准倍数
OUTPUT: null
FUNCTION: machine run to left.
-----------------------------------------------------*/
void Motor_RunToLeft(Byte steps) //motor1
{
Motor_RunWayInit(steps,0,Motor_DirLeft);
}
/*---------------------------------------------------
INPUT: steps/基准倍数
OUTPUT: null
FUNCTION: machine run to right.
-----------------------------------------------------*/
void Motor_RunToRight(Byte steps) //motor1
{
Motor_RunWayInit(steps,0,Motor_DirRight);
}
/*---------------------------------------------------
INPUT: steps/基准倍数
OUTPUT: null
FUNCTION: machine run up.
-----------------------------------------------------*/
void Motor_RunUpWard(Byte steps) //motor2
{
Motor_RunWayInit(steps,0,Motor_DirUp);
}
/*---------------------------------------------------
INPUT: steps/基准倍数
OUTPUT: null
FUNCTION: machine run down.
-----------------------------------------------------*/
void Motor_RunDownWard(Byte steps) //motor2
{
Motor_RunWayInit(steps,0,Motor_DirDown);
}
#endif //del
void Motor_SetSpeed(Byte speed)
{
RunSpeed = speed;
}
void Motor_Adjust(void)
{
CommandType |=MotorAdjust;
Adjust_step = ToLeft;
}
void Motor_AutoPatrol(Byte Type)
{
CommandType |= MotorAutoPatrol;
ActionType = Type;
}
void Motor_RunToCenter(void)
{
CommandType |= MotorRunToCenter;
ToCenterStep = HorizonOfMiddle;
}
void Motor_Server(void)
{
switch(CommandType)
{
case MotorRunHorizonValue:
if(StepNumber1 ==0)
{
Motor1_StopFlag = TRUE;
CommandType &=~BIT1;
}
break;
case MotorRunVeritcalValue:
if(StepNumber2 ==0)
{
Motor2_StopFlag = TRUE;
CommandType &=~BIT2;
}
break;
case MotorAdjust:
Motor_AdjustStep();
Motor_AdjustPositionCheck();
break;
case MotorAutoPatrol:
Motor_AutoPatrolCtrl();
break;
case MotorRunToCenter:
Motor_RunToCenterCtrl();
break;
default:
break;
}
if(Motor1_StopFlag)
{
Motor1_VariableReset();
Motor1_Stop();
}
else if(Motor2_StopFlag)
{
Motor2_VariableReset();
Motor2_Stop();
}
}
void Motor_AutoPatrolCtrl(void)
{
#if 0
if(ActionType == 1)
{
Horizon_Patrol();
ActionType = 4;
}
else if(ActionType ==2)
{
Vertical_Patrol();
ActionType = 4;
}
else if(ActionType ==3)
{
}
else if(ActionType ==3)
{
Patrol_Check();
}
#endif
#if 1
switch(ActionType)
{
case 1:
Horizon_Patrol();
ActionType = 4;
break;
case 2:
Vertical_Patrol();
ActionType = 4;
break;
case 3:
AllDirect_Patrol();
ActionType = 4;
break;
case 4:
Patrol_Check();
break;
default:
break;
}
#endif
}
void Horizon_Patrol(void)
{
if(POSITION_IS_LEFT || position_value==0)
{
Motor_RunWayInit(0,1,Motor_DirRight);
}
else if(POSITION_IS_RIGHT)
{
Motor_RunWayInit(0,1,Motor_DirLeft);
}
}
void Vertical_Patrol(void)
{
#if 0
if(POSITION_IS_DOWN || position_value==0)
{
Motor_RunWayInit(0,1,Motor_DirUp);
}
else if(POSITION_IS_UP)
{
Motor_RunWayInit(0,1,Motor_DirDown);
}
#endif
}
void AllDirect_Patrol(void)
{
// Horizon_Patrol();
//Vertical_Patrol();
}
void Patrol_Check(void)
{
#if 0
if(POSITION_IS_LEFT)
{
Motor_RunWayInit(0,1,Motor_DirRight);
}
else if(POSITION_IS_RIGHT)
{
Motor_RunWayInit(0,1,Motor_DirLeft);
}
if(POSITION_IS_DOWN)
{
Motor_RunWayInit(0,1,Motor_DirUp);
}
else if(POSITION_IS_UP)
{
Motor_RunWayInit(0,1,Motor_DirDown);
}
#endif
}
void Motor_RunToCenterCtrl(void)
{
#if 0
switch(ToCenterStep)
{
case HorizonOfMiddle:
if(x_max/2 >x_value)
{
StepNumber1 = x_max/2 - x_value;
Motor_RunToRight(StepNumber1);
}
else
{
StepNumber1 = x_value - x_max/2;
Motor_RunToLeft(StepNumber1);
}
RemToCenterStep = HorizonOfMiddle;
break;
case VeriticalOfMiddle:
if(y_max/2 >y_value)
{
StepNumber2 = y_max/2 - y_value;
Motor_RunUpWard(StepNumber2);
}
else
{
StepNumber2 = y_value - y_max/2;
Motor_RunDownWard(StepNumber2);
}
RemToCenterStep = VeriticalOfMiddle;
break;
case Center_Check:
SetToCenterCheck();
break;
default:
break;
}
#endif
}
void SetToCenterCheck(void)
{
if(RemToCenterStep == HorizonOfMiddle)
{
if(x_value ==x_max/2)
{
ToCenterStep = VeriticalOfMiddle;
Motor1_StopFlag = TRUE;
}
}
else if(RemToCenterStep == VeriticalOfMiddle)
{
if(y_value ==y_max/2)
{
ToCenterStep = InitCenter;
RemToCenterStep = InitCenter;
CommandType &=~BIT5;
Motor2_StopFlag = TRUE;
}
}
}
void Motor_AdjustStep(void)
{
switch(Adjust_step)
{
case ToLeft:
if(!POSITION_IS_LEFT)
Motor_RunWayInit(0,1,Motor_DirLeft);
Adjust_step = Position_Check;
Rem_Adjust_step = ToLeft;
break;
case ToRight:
if(!POSITION_IS_RIGHT)
Motor_RunWayInit(0,1,Motor_DirRight);
Adjust_step = Position_Check;
Rem_Adjust_step = ToRight;
break;
case ToDown:
if(!POSITION_IS_DOWN)
Motor_RunWayInit(0,1,Motor_DirDown);
Adjust_step = Position_Check;
Rem_Adjust_step = ToDown;
break;
case Toup:
if(!POSITION_IS_UP)
Motor_RunWayInit(0,1,Motor_DirUp);
Adjust_step = Position_Check;
Rem_Adjust_step = Toup;
break;
case ToHorizonCenter:
StepNumber1 = x_max/2;
Motor_RunWayInit(StepNumber1,0,Motor_DirLeft);
Rem_Adjust_step = ToHorizonCenter;
break;
case ToVericalCenter:
StepNumber2 = y_max/2;
Motor_RunWayInit(StepNumber2,0,Motor_DirDown);
Rem_Adjust_step = ToVericalCenter;
break;
case Position_Check:
break;
default:
break;
}
}
void Motor_AdjustPositionCheck(void)
{
switch(Rem_Adjust_step)
{
case ToLeft:
if(POSITION_IS_LEFT)
{
x_value = 0;
Motor1_StartRun = FALSE;
Adjust_step = ToRight;
}
break;
case ToRight:
if(POSITION_IS_RIGHT)
{
x_max = x_value;
Motor1_StartRun = FALSE;
Adjust_step =ToDown;
}
break;
case ToDown:
if(POSITION_IS_DOWN)
{
y_value = 0;
Motor2_StartRun = FALSE;
Adjust_step =Toup;
}
break;
case Toup:
if(POSITION_IS_UP)
{
y_max =y_value;
Motor2_StartRun = FALSE;
Adjust_step =ToHorizonCenter;
}
break;
case ToHorizonCenter:
if(x_value == x_max/2)
{
Motor1_StopFlag = TRUE;
Adjust_step =ToVericalCenter;
}
break;
case ToVericalCenter:
if(y_value == y_max/2)
{
Motor2_StopFlag = TRUE;
CommandType &=~BIT3;
Adjust_step =0;
}
break;
}
}
/*---------------------------------------------------
INPUT: null
OUTPUT: null
FUNCTION: dirve motor, used by timer1 interrupt.
-----------------------------------------------------*/
#pragma RB(1)
void Motor1_Run(void)
{
if(Motor1_StartRun ==FALSE)
return;
if(StepNumber1 ||Motor1_Direct)
{
Motor1_StepRun();
Motor1StepCyc++;
if(Motor1StepCyc>7)
{
if(StepNumber1)
StepNumber1--;
Motor1StepCyc =0;
Xvalue_Count();
}
}
}
#pragma RB(1)
void Motor2_Run(void)
{
if(Motor2_StartRun ==FALSE)
return;
if(StepNumber2 ||Motor2_Direct)
{
Motor2_StepRun();
Motor2StepCyc++;
if(Motor2StepCyc>7)
{
if(StepNumber2)
StepNumber2--;
Motor2StepCyc =0;
Yvalue_Count();
}
}
}
void Xvalue_Count(void)
{
if(Motor1_Dir == Motor_DirRight)
{
x_value++;
}
else
{
if(x_value)
x_value --;
}
}
void Yvalue_Count(void)
{
if(Motor2_Dir == Motor_DirUp)
{
y_value++;
}
else
{
if(y_value)
y_value --;
}
}
void Motor1_StepRun(void)
{
switch(Motor1_step_index)
{
case 1:
MT1_A = 0;
break;
case 2:
MT1_B = 0;
break;
case 3:
MT1_A = 1;
break;
case 4:
MT1_C = 0;
break;
case 5:
MT1_B = 1;
break;
case 6:
MT1_D = 0;
break;
case 7:
MT1_C = 1;
break;
case 8:
MT1_A = 0;
break;
default:
break;
}
Modify_Motor1_Step();
}
void Motor2_StepRun(void)
{
switch(Motor2_step_index)
{
case 1:
MT2_A = 1;
MT2_B = 0;
MT2_C = 0;
MT2_D = 0;
break;
case 2:
MT2_B = 1;
MT2_A = 1;
break;
case 3:
MT2_A = 1;
break;
case 4:
MT2_C = 0;
break;
case 5:
MT2_B = 1;
break;
case 6:
MT2_D = 0;
break;
case 7:
MT2_C = 1;
break;
case 8:
MT2_A = 0;
break;
default:
break;
}
Modify_Motor2_Step();
}
void Modify_Motor1_Step(void)
{
if((Motor1_Dir ==Motor_DirLeft ||Motor1_Dir ==Motor_DirRight))
{
if(Motor1_Dir ==Motor_DirLeft)
{
Motor1_step_index++;
if(Motor1_step_index>8)
Motor1_step_index =1;
}else
{
Motor1_step_index--;
if(Motor1_step_index<1)
Motor1_step_index =8;
}
}
}
void Modify_Motor2_Step(void)
{
if((Motor2_Dir ==Motor_DirUp ||Motor2_Dir ==Motor_DirDown))
{
if(Motor2_Dir ==Motor_DirUp)
{
Motor2_step_index++;
if(Motor2_step_index>8)
Motor2_step_index =1;
}else
{
Motor2_step_index--;
if(Motor2_step_index<1)
Motor2_step_index =8;
}
}
}
void Motor1_VariableReset(void)
{
StepNumber1 = 0;
Motor1StepCyc = 0;
Motor1_StopFlag = FALSE;
Motor1_Dir = Motor_DirIgnore;
Motor1_StartRun = FALSE;
}
void Motor2_VariableReset(void)
{
StepNumber2 = 0;
Motor2StepCyc = 0;
Motor2_StopFlag = FALSE;
Motor2_Dir = Motor_DirIgnore;
Motor2_StartRun = FALSE;
}
void Motor_StepIndex_Init(void)
{
Motor1_step_index = 1;
Motor2_step_index = 1;
RunSpeed = 125;
}
void Motor_VariableReset(void)
{
Motor_StepIndex_Init();
Motor1_VariableReset();
Motor2_VariableReset();
}
void Motor1_Stop(void)
{
MT1_A = 1;
MT1_B = 1;
MT1_C = 1;
MT1_D = 1;
}
void Motor2_Stop(void)
{
MT2_A = 1;
MT2_B = 1;
MT2_C = 1;
MT2_D = 1;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -