Simple C Based routines for robot control

By Youssef Edward
October 18, 2019


The following is a simple routines that control robot motion in C language. The robots is assumed to be three axes. The joint is rotated by a stepper motor that controlled with TB5560 driver.

void move1_pos(int p)
{ unsigned int h=0;
portd.f5=1;
for(h=0;h<p;h++)
{
portd.f7=1;
delay_us(5);
portd.f7=0;
if(speed==2) delay_us(700);
else if(speed==1) delay_us(2000);
else if(speed==0) delay_us(3000);

}
}

The above routine is use to send pulsed to the stepper driver with indicated speeds. the p variable is the pulse count to the stepper motor. There are three speeds indicated defined in global variable called speed. If speed equal 2 it will give the highest speed while speed equal 0 will give the lowest speed. The direction of movement is determined by a single control pin.

void move1_neg(int p)
{ unsigned int h=0;
portd.f5=0;//
for(h=0;h<p;h++)
{
portd.f7=1;
delay_us(5);
portd.f7=0;
if(speed==2) delay_us(700);
else if(speed==1) delay_us(2000);
else if(speed==0) delay_us(3000);

}
}

The above routine is similar except it is used to move the motor in the negative direction. Note that positive and negative direction is determined based on sense. There are no rules to determine if the  direction of movement is positive or negative.

 

 

void ref1()
{
if(!portc.f5)
{
while(1)
{ move1_neg(5);
if(portc.f5)
break ;

}
move1_pos(1000);
}
else
{
while(1)
{ move1_neg(5);
if(!portc.f5)
break ;

}
move1_neg(2100);
}
}

 

The above routine is used to reference the first axis. it is included in the system infrared sensor. The control send pulses to the motor to move in the indicated direction. If the sensor is triggered, the motor stopped at once, and exit the loop. Referencing is a basic operation in robotics to be done at startup. This is very important to set a  reference point to measure any motion from it.

 

 

void program4()
{ int h=0;
unsigned short value=0; char result=0;

if(isreferenced==0)
{
ref1();
ref2();
ref3();
}
move2_neg(1600);
while(1)
{

for(h=0;h<area_hor;h++)
{
move3_neg(2200);
}
result= checkstop(); if(result==1) break;;
for(h=0;h<area_hor;h++)
{
move3_pos(2200);
}

result= checkstop(); if(result==1) break;;
move1_pos(area_rotary);

result= checkstop(); if(result==1) break;;
for(h=0;h<area_hor;h++)
{
move3_neg(2200);
}
result= checkstop(); if(result==1) break;;
for(h=0;h<area_hor;h++)
{
move3_pos(2200);
}
result= checkstop(); if(result==1) break;;
move1_neg(area_rotary);

result= checkstop(); if(result==1) break;

}
move2_pos(1600);
isreferenced=0;

}

 

The above routine is used to send a series of movements to the robot. This is used in demo mode  to introduce the robot. The following routines are similar but make different moves. The reader could inspect them in easy manner.

void program1()
{ int h=0;
unsigned short value=0; char result=0;

if(isreferenced==0)
{
ref1();
ref2();
ref3();
}
move2_neg(1600);
while(1)
{
h=0;
while(1)
{
move3_neg(70);

portd.f5=1;
portd.f7=1;
delay_us(5);
portd.f7=0;

if(h%200==0)
{
move2_neg(2);
}
h++;
if(h==area_hor) break;
}
result= checkstop(); if(result==1) break;;
h=0;
while(1)
{
move3_pos(70);

portd.f5=0;
portd.f7=1;
delay_us(5);
portd.f7=0;

if(h%200==0)
{
move2_pos(2);
}
h++;
if(h==area_hor) break;
}
result= checkstop(); if(result==1) break;;
}
isreferenced=0;
move2_pos(1600);
}

void program2()
{ int h=0; char value=0; int vv=0; char result=0;

if(isreferenced==0)
{
ref1();
ref2();
ref3();
}
vv= area_rotary/10; move2_neg(1600);
while(1)
{

for(h=0;h<area_hor;h++)
{
move3_neg(2200);

}
move1_pos(vv);

result= checkstop(); if(result==1) break;
for(h=0;h<area_hor;h++)
{
move3_pos(2200);

}
move1_pos(vv); result= checkstop(); if(result==1) break;

for(h=0;h<area_hor;h++)
{
move3_neg(2200);

}
move1_pos(vv); result= checkstop(); if(result==1) break;

for(h=0;h<area_hor;h++)
{
move3_pos(2200);

}
move1_pos(vv); result= checkstop(); if(result==1) break;
for(h=0;h<area_hor;h++)
{
move3_neg(2200);

}
move1_pos(vv); result= checkstop(); if(result==1) break;

for(h=0;h<area_hor;h++)
{
move3_pos(2200);

}
move1_pos(vv); result= checkstop(); if(result==1) break;
for(h=0;h<area_hor;h++)
{
move3_neg(2200);

}
move1_pos(vv); result= checkstop(); if(result==1) break;

for(h=0;h<area_hor;h++)
{
move3_pos(2200);

}
move1_pos(vv); result= checkstop(); if(result==1) break;
for(h=0;h<area_hor;h++)
{
move3_neg(2200);

}
move1_pos(vv); result= checkstop(); if(result==1) break;

for(h=0;h<area_hor;h++)
{
move3_pos(2200);

}
move1_pos(vv); result= checkstop(); if(result==1) break;

if(endflag==1)
{
endflag=0;
write_ram(1,minute); delay_us(100);
write_ram(2,0); delay_us(100);
buzzer();
break;
}
result= checkstop(); if(result==1) break;

for(h=0;h<area_hor;h++)
{
move3_neg(2200);

}
move1_neg(vv); result= checkstop(); if(result==1) break;

for(h=0;h<area_hor;h++)
{
move3_pos(2200);

}
move1_neg(vv); result= checkstop(); if(result==1) break;

for(h=0;h<area_hor;h++)
{
move3_neg(2200);

}
move1_neg(vv); result= checkstop(); if(result==1) break;

for(h=0;h<area_hor;h++)
{
move3_pos(2200);

}
move1_neg(vv); result= checkstop(); if(result==1) break;
for(h=0;h<area_hor;h++)
{
move3_neg(2200);

}
move1_neg(vv); result= checkstop(); if(result==1) break;

for(h=0;h<area_hor;h++)
{
move3_pos(2200);

}
move1_neg(vv); result= checkstop(); if(result==1) break;
for(h=0;h<area_hor;h++)
{
move3_neg(2200);

}
move1_neg(vv); result= checkstop(); if(result==1) break;

for(h=0;h<area_hor;h++)
{
move3_pos(2200);

}
move1_neg(vv); result= checkstop(); if(result==1) break;; ;
for(h=0;h<area_hor;h++)
{
move3_neg(2200);

}
move1_neg(vv); result= checkstop(); if(result==1) break;;

for(h=0;h<area_hor;h++)
{
move3_pos(2200);

}
move1_neg(vv); result= checkstop(); if(result==1) break;;

if(endflag==1)
{
endflag=0;
write_ram(1,minute); delay_us(100);
write_ram(2,0); delay_us(100);
buzzer();
break;
}

result= checkstop(); if(result==1) break;;;
}
move2_pos(1600);
isreferenced=0;

}

 

 

Leave a Reply

Your email address will not be published. Required fields are marked *