Stepper motor controller for precise movement

Amit Karna March 2, 2013 Coded in C++ for the x86

TASK: Follow the pre-determined path using a robocar (stepper motor driven) interfaced through Parallel port to personal computer.

DESCRIPTION: Using simple mathematical relations, a methodology is implemented after quite a few practical and theoretical experiments for the different problems faced such accurate forward and backward movements, precise angle turns, etc to control stepper motor of robocar through Standard Parallel Port (SPP) of computer.

This code lead us to win BEST ROBO DESIGN for pre-determined path follower task during Technozion'06 @ NIT Warangal and a paper presentation during Engineer'06 @ NIT Surathkal. [Refer: http://amitalks.blogspot.in/2006/02/frontier-robocar.html]

/*
 * IBM-PC Parallel Printer Port Data & Status Registers
 * ====================================================
 *        7   6   5   4   3   2   1   0   I/O Port
 *      +---+---+---+---+---+---+---+---+ ========
 * Data | C8| C7| C6| C5| C4| C3| C2| C1| Base = 278/378/3BC Hex
 *      +---+---+---+---+---+---+---+---+           +---+
 */
#include<sys/io.h>
#include<unistd.h>
#include<stdlib.h>
#include<math.h>	//for floor()
#include<iostream>
using namespace std;

#define BASEPORT 0x378 //SPP - Standard Parallel port base address

class stepper
{
	private:
		long delay;	//delay betn each step
		float pi;	//constant
		float acf, lcf;	//angle and length correction factors in percentage
		float r, c;	// radius and circumerence of wheel
		float ns;		
		int nfsc, nhs;	
		float residue;
		// number of total steps, full step cycles and number of half steps
		float step_rating;	//number of steps per revolution
		float speed;	//speed of stepper in cm/sec
		float l,w;	//length and width of robocar	
	public:
		stepper(float spd);
	
		void specification();
		
		void length2steps(float len, int& nfsc, int& nhs);//conversion
		void angle2steps(float angle, int& nfsc, int& nhs);//conversion
		void move(int nfsc, int nhs, int leftw, int rightw); //move
				
		void fwd(float len);	//length in cm
		void bkwd(float len);	//length in cm
			
		void righturn(float degree, int degree_of_freedom);	//number of degree turns
		void lefturn(float degree, int degree_of_freedom);	//  -- do --
};

stepper:: stepper(float spd=7)
{	
	speed=spd;
	l=18;		//length of car
	w=17-2;		//width of car
	acf=-2;		//angle correction factor in percentage
	lcf=-4;		//angle correction factor in percentage
	pi=3.14159;	//costant
	step_rating=200.5;	//step rating
	r=3.448;		//radius of wheel in cm
	c=2*pi*r;	//circumference
	residue=0;
	//speed being in cm/s
	delay=long(1/(step_rating/c*speed)*1000*1000);	//in microsecond
}

void stepper::specification()
{	
	cout<<"\n\n\n\t\tF R O N T I E R\n";
	cout<<"\t~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~";
	cout<<"\n\n\nDesigned by: Amit Kumar Karna";
	cout<<"\n\n\nMission: Vitrubio, Technozion'06 @ NIT Warangal";
	cout<<"\n\n\nROBOCAR specifications...";
	cout<<"\nDimensions = "<<l<<"cm x"<<w<<"cm";
	cout<<"\nWheel Dia = "<<2*r<<"cm";
	cout<<"\nStepper motor:  12V-0.33A \t"<<step_rating<<" steps/revolution";
	cout<<"\nLength n Angle correction factors: "<<lcf<<"% & "<<acf<<"% respectively";
	cout<<"\nSpeed selected : "<<speed<<"cm/sec";
	cout<<"\ndelay between each step = "<<delay/1000.0<<"ms";
	cout<<endl<<endl<<endl;
}

void stepper::length2steps(float len, int& nfsc, int& nhs)//conversion
{
	ns=step_rating/c*len+residue;
	nfsc=int(ns/4);
	nhs=int(floor((ns-nfsc*4)/0.5));	//rounding
	if(nhs==8)	//may result after rounding
		nhs=7;
	residue=ns-(nfsc*4+nhs*0.5);
}

void stepper::angle2steps(float angle, int& nfsc, int& nhs)//conversion
{
	float arclen=(pi*w)/360.0*angle;
	length2steps(arclen, nfsc, nhs);
}

void stepper::move(int nfsc, int nhs, int leftw, int rightw) //move	
{
   int i;
   int cnt;
   outb(0x00, BASEPORT);
   if(leftw==1 && rightw==1)
   {	
	for(i=0; i<nfsc; i++)
	{
		outb(0x59, BASEPORT);
		usleep(delay);
		outb(0x6A, BASEPORT);
		usleep(delay);
		outb(0xA6, BASEPORT);
		usleep(delay);
		outb(0x95, BASEPORT);
		usleep(delay);
		if(i%50==0 && i!=0)
			outb(0x81, BASEPORT);	//one half step - as a compensation
	}
	//sequence after 5 i.e. 0100 is 1 for half step
	//sequence after 9 i.e. 1001 is 8 for half step
       for(i=0; i<1; i++)	//dummy loop to use break;
       {
	cnt=0;
	if(nhs==0)
		break;	
	outb(0x81, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0xA9, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x28, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x6A, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x42, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x56, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x14, BASEPORT);
	if(++cnt==nhs)
		break;	
	//max of 7 half steps only possible
       }
   }
   else if(leftw==-1 && rightw==-1)
   {
	for(i=0; i<nfsc; i++)
	{
		outb(0x95, BASEPORT);
		usleep(delay);
		outb(0xA6, BASEPORT);
		usleep(delay);
		outb(0x6A, BASEPORT);
		usleep(delay);
		outb(0x59, BASEPORT);
		usleep(delay);
		if(i%50==0 && i!=0)
			outb(0x81, BASEPORT);	//one half step - as a compensation
	}
	//sequence after 5 i.e. 0100 is 1 for half step
	//sequence after 9 i.e. 1001 is 8 for half step
       for(i=0; i<1; i++)	//dummy loop
       {
	cnt=0;
	if(nhs==0)
		break;	
	outb(0x81, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0xA9, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x28, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x6A, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x42, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x56, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x14, BASEPORT);
	if(++cnt==nhs)
		break;	
       }
   }   
   else if(leftw==-1 && rightw==1)	//left turn 2 wheels
   {
	
	for(i=0;i<nfsc;i++)
	{
		outb(0x99, BASEPORT);
		usleep(delay);
		outb(0xAA, BASEPORT);
		usleep(delay);
		outb(0x66, BASEPORT);
		usleep(delay);
		outb(0x55, BASEPORT);
		usleep(delay);
	}
	//sequence after 5 i.e. 0100 is 1 for half step
	cnt=0;
      for(i=0; i<1; i++)
      {
	if(nhs==0)
		break;	
	outb(0x11, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x99, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x88, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0xAA, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x22, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x66, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x44, BASEPORT);
	if(++cnt==nhs)
		break;	
	//max of 7 half steps only possible
      }
   }
   else if(leftw==1 && rightw==-1)	//right turn with 2 wheels
   {
	for(i=0;i<nfsc;i++)
	{
		outb(0x55, BASEPORT);
		usleep(delay);
		outb(0x66, BASEPORT);
		usleep(delay);
		outb(0xAA, BASEPORT);
		usleep(delay);
		outb(0x99, BASEPORT);
		usleep(delay);
	}
	//sequence after 5 i.e. 0100 is 1 for half step
	cnt=0;
      for(i=0; i<1; i++)
      {
	if(nhs==0)
		break;	
	outb(0x44, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x66, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x22, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0xAA, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x88, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x99, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x11, BASEPORT);
	if(++cnt==nhs)
		break;	
	//max of 7 half steps only possible
      }
   }
   else if(leftw==0 && rightw==1) //left turn with one wheel
   {
	
	for(i=0;i<nfsc;i++)
	{
		outb(0x09, BASEPORT);
		usleep(delay);
		outb(0x0A, BASEPORT);
		usleep(delay);
		outb(0x06, BASEPORT);
		usleep(delay);
		outb(0x05, BASEPORT);
		usleep(delay);
	}
	//sequence after 5 i.e. 0100 is 1 for half step
	cnt=0;
      for(i=0; i<1; i++)
      {
	if(nhs==0)
		break;	
	outb(0x01, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x09, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x08, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x0A, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x02, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x06, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x04, BASEPORT);
	if(++cnt==nhs)
		break;	
	//max of 7 half steps only possible
      }
   }
   else if(leftw==1 && rightw==0) //right turn with one wheel
   {
	for(i=0;i<nfsc;i++)
	{
		outb(0x50, BASEPORT);
		usleep(delay);
		outb(0x60, BASEPORT);
		usleep(delay);
		outb(0xA0, BASEPORT);
		usleep(delay);
		outb(0x90, BASEPORT);
		usleep(delay);
	}
	//sequence after 5 i.e. 0100 is 1 for half step
	cnt=0;
      for(i=0; i<1; i++)
      {
	if(nhs==0)
		break;	
	outb(0x40, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x60, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x20, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0xA0, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x80, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x90, BASEPORT);
	if(++cnt==nhs)
		break;
	outb(0x10, BASEPORT);
	if(++cnt==nhs)
		break;	
	//max of 7 half steps only possible
      }
   }
   outb(0x00, BASEPORT);
}		

void stepper::fwd(float len=0)
{
	float tlen=len;
	if(len==0)
		return;
	tlen=tlen+tlen*lcf/100;
	length2steps(tlen, nfsc, nhs);
	cout<<"\nMoving forward by "<<len<<"cms... \t[\t"<<nfsc<<"  "<<nhs<<"  ]"<<endl;
	move(nfsc, nhs, 1, 1);//leftw=1 & rightw=1
}

void stepper::bkwd(float len=0)
{
	float tlen=len;
	if(len==0)
		return;
	tlen=tlen+tlen*lcf/100;
	length2steps(tlen, nfsc, nhs);
	cout<<"\nMoving backward by "<<len<<"cms... \t[\t"<<nfsc<<"  "<<nhs<<"  ]"<<endl;
	move(nfsc, nhs, -1, -1);//leftw=-1 & rightw=-1
}

void stepper::lefturn(float angle, int dof=2)	//degree of freedom
{
	float tangle=angle;
	tangle=tangle*2/dof;
	tangle=tangle+tangle*acf/100;
	angle2steps(tangle, nfsc, nhs);
	cout<<"\nTaking left turn by "<<angle<<"degrees... [\t"<<nfsc<<"  "<<nhs<<"  ]"<<endl;
	if(dof==1)
		move(nfsc, nhs, 0, 1);	//leftwheel=off, rightwheel=on
	else
		move(nfsc, nhs, -1, 1);
}

void stepper::righturn(float angle, int dof=2)
{
	float tangle=angle;
	tangle=tangle*2/dof;
	tangle=tangle+tangle*acf/100;
	angle2steps(tangle, nfsc, nhs);
	cout<<"\nTaking right turn by "<<angle<<"degrees... [\t"<<nfsc<<"  "<<nhs<<"  ]"<<endl;
	if(dof==1)
		move(nfsc, nhs, 1, 0); //leftwheel=on, rightwheel=off
	else
		move(nfsc, nhs, 1, -1);
}

main()
{
	system("clear");
	if(ioperm(BASEPORT,3,1))
	{
		cout<<"\nThe parallel port accessing error!";
		exit(1);
	}
	float speed; 		//in cm/s
	float len, la, ra;	//in cm
	float angle;
	long wait=10000;
	
	//cout<<"\nEnter speed in cm/s (eg. 10cm/s) : ";
	//cin>>speed;	

	speed=10;
	stepper sm(speed);	 

	sm.specification();
	cout<<"\nScanning the problem...";usleep(wait);
	cout<<"...";usleep(wait);cout<<"...";usleep(wait);cout<<"...";cout<<"Done!";
	cout<<"\n\n\nStarting the voyage...\n\n\n";
	
	float l1=20, l2=28.284;
	float a1=45, a2=90, a3=135;

	int i;
	
	for(i=0; i<3; i++)
	{
		sm.fwd(l1);
		usleep(wait);
		sm.lefturn(a3);
		usleep(wait);
		sm.fwd(l2);
		usleep(wait);
		sm.righturn(a3);
	}
}

Comments:

There are no comments yet!


Sorry, you need javascript enabled to post any comments.