EmbeddedRelated.com
Code Snippets
   Code Snippets    Submitted by danfe

Stepper motor controller for precise movement

Amit Karna ● March 2, 2013●1 comment ● Coded in C++ for the x86
/*
 * 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);
	}
}