Status Arm::MoveMotor(int motor,int steps)
{
// Eitan chak
// SetOnError(GO);
Status ErrorStatus;
if (motor >6)
{
ErrorStatus.Error= WRONG_MOTOR; // Rong Motor Nomber
return ErrorStatus;
}
int i=0,n=0;
int a=0,d=2;
// int eitan=0;
_outp(PORT_ADRESS_WRITE,128);
for (i=0;i<abs(steps) ; i++)
{
// eitan++;
// if ( Conv(ReadFromPort()).bit4==0) printf ("Eitan is %d ",eitan);
// printf ("(4)%d (5)%d (6)%d (7)%d (8)%d \n",bit.,bit.bit5,bit.bit6,bit.bit7,bit.bit8);
ErrorStatus=CheckAndCount(motor,steps);
if(ErrorStatus.Error<=GO)
{
if (steps >=0) //Chack the movement direction
{
// ****** Just for the ELBOW ***********
if (motor==2) // If the ELBOW is moveing compensation the hand tention
{
LAST.last[5] /=2;
if (LAST.last[5]<8)
LAST.last[5]=64;
_outp(PORT_ADRESS_WRITE,LAST.last[5]+5);
_sleep(Dhand);
_outp(PORT_ADRESS_WRITE,128);
}//*/
LAST.last[motor] *=2;
if (LAST.last[motor] >64)
LAST.last[motor]=8;
}
else if (steps <0) //Chack the movement direction
{
// ****** Just for the ELBOW ***********
if (motor==2) // If the ELBOW is muved compensation the hand tention
{
LAST.last[5] *=2;
if (LAST.last[5] >64)
LAST.last[5]=8;
_outp(PORT_ADRESS_WRITE,LAST.last[5]+5);
_sleep(Dhand);
_outp(PORT_ADRESS_WRITE,128);
}//*/
LAST.last[motor] /=2;
if (LAST.last[motor]<8)
LAST.last[motor]=64;
}
_outp(PORT_ADRESS_WRITE,LAST.last[motor]+motor);
_sleep(0);
_sleep(Dhand);
if (n<2) _sleep(2);
_outp(PORT_ADRESS_WRITE,128);
}
else if (ErrorStatus.Error == EXIT) return ErrorStatus;
// Delay bitween step to step (The motors must have time to step and the program can "shoot" the motors with steps very fast , and the motors wont move so we add a delay
if (a>=20 ) d=0; // Make bigger Delay at thr bigining of the move sow the motor can acelorate
else if (a>=10) d=1;
a++;
_sleep(DELAY+d);
}
return ErrorStatus;
}
משתמשים הגולשים בפורום זה: Bing [Bot] ו 6 אורחים