/************************************************/
/*												*/
/* routines this module enk_12 - 24/3/13	    */
/* Release version								*/
/* Modified routine polldla and dla9600 value	*/
/*												*/
/* 0) void setup								*/
/* 1) void loop									*/
/* 2) void pollenk - poll encoder				*/
/* 3) int makbcd - convert int to BCD ascii str */
/* 4) int makhex - convert long to Hex ascii str*/
/* 5) void blowc - tx a char from second serial */
/* 6) void polldla - waits 9600 baud bit delay  */
/*												*/
/************************************************/

/************************************************/
/* program enk_6                                */
/*												*/
/* program reads encoder outputs				*/
/*												*/
/* direct Port i/o								*/
/* PORTB bits to arduino pin #:					*/
/*	0 1  2  3  4  5  6 7 BIT#					*/
/*	8 9 10 11 12 13 (xtl6,xtl7) -ino dig pin#	*/
/*												*/
/* PORTB masks:									*/
/* Pin 8 = 0x01									*/
/* Pin 9 = 0x02									*/
/* Pin10 = 0x04									*/
/* Pin11 = 0x08									*/
/* Pin12 = 0x10									*/
/* Pin13 = 0x20									*/
/*												*/
/* PORTD maps to arduino dig pins 0 to 7		*/
/* PORTD masks:									*/
/* Pin7 = 0x80									*/
/*												*/
/************************************************/

/* define digital input channels */
int chan1=9;
int getch1 = 0x02;
int chan2=8;
int getch2 = 0x01;

/* read channel 1/2 i/p patterns */
int ic1;
int ic2;

/* saved channel states */
int ch1state;
int ch2state;

/* chan 1/2 debounce counters */
int ic1up = 0;
int ic1dn = 0;
int ic2up = 0;
int ic2dn = 0;
int icdbnce = 0;

/* define digital o/p ports */
int clkout = 13; /* clock */
int clkpin = 1;
int setclk = 0x20;
int clrclk = 0xff-setclk;

int ch1out = 12; /*chan1 tick */
int setch1 = 0x10;
int clrch1 = 0xff-setch1;

int ch2out = 11; /*chan2 tick */
int setch2= 0x08;
int clrch2=0xff-setch2;

int dirout = 10; /*direction F(hi)/R(lo) */
int setdir=0x04;
int clrdir=0xff-setdir;
int dir1;
int dir2;
int dirf;
int cnt1;
int cnt2;

long tcount=0; /* tick count */

/* define test counts for dir change */
int fwdc=0;
int revc=0;
int dirlim=20;

/* char buffers for serial text */
char sout[10],tout[13];

/* 9600 baud timing delays */
int dla9600 = 95; /* 9600 bit time = 104us delay */
int t1_9600 = 0;  /* # of polling cycles in delay */
int t2_9600 = 0; /* remainder us in delay */

/* define digital serial out port */
int serout = 7;
int setser=0x80;
int clrser=0xff-0x80;

/************************************************/
/*												*/
/************************************************/

void setup() 
{                
/************************************************/
/*												*/
/* setup - initialize analogue pins as input etc*/
/*												*/
/************************************************/
	
	/* define autos */
	unsigned long mus, mus2,diftl;
	int difti;

	pinMode(chan1, INPUT);     
	pinMode(chan2, INPUT);     

	pinMode(clkout,OUTPUT);
	digitalWrite(clkout,HIGH);

	pinMode(ch1out,OUTPUT);
	digitalWrite(ch1out,HIGH);
	ch1state=1;

	pinMode(ch2out,OUTPUT);
	digitalWrite(ch2out,HIGH);
	ch2state=1;

	pinMode(dirout,OUTPUT);
	digitalWrite(dirout,HIGH);
	dir1=1;
	dir2=1;
	dirf=1;

	/* set up serial out */
	pinMode(serout,OUTPUT);
	digitalWrite(serout,HIGH);

	/* now time ticker polling loop */
	mus = micros();
	pollenk();
	mus2=micros();
	diftl=mus2-mus;

	/* check clock did not roll over */
	if(diftl>1000) /* polling loop ca. 25us */
	{
		mus = micros();
		pollenk();
		mus2=micros();
		diftl=mus2-mus;
	} /* end redo pollenk timing */

	/* now see how long serial delay time is */
	difti=int(diftl);
	t1_9600=int(dla9600/difti); /* number of polling cycles in the delay */
	t2_9600 = dla9600 % difti; /* # of remaining us */

	/* debug - set COMx serial */
	/*Serial.begin(9600);*/

	/* wait a bit */
	/*delay(1000);*/
}

/************************************************/
/*												*/
/************************************************/

void loop() 
{
/************************************************/
/*												*/
/* loop - loop reading encoder channels			*/
/*												*/
/************************************************/

	/* declare autos */
	int slen,i,j;
	char *s;

    for (j=1;;)
	{    
		/* poll encoder */
		pollenk();

		/* write out count every 6msec or so */
		if(++j > 500)
		{
			j=0;

			/* now convert the count to a string */
			/*slen=makbcd();*/
			slen=makhex();

			/* now write tick count chars */
			for (s = tout, i=0; i < slen; i++, s++)
			{
				/* write char to serial */
				/* *s=0x55; /* debug bit pattern - 0101 0101 (lsb first) */
				blowc(*s);

				/* debug - write to COMx serial */
				/*Serial.write(*s);*/

			} /* end write data */

			/* terminate COMx serial */
			*s='\n';
			/*Serial.write(*s);*/

		} /* end if a write output cycle */

	} /* end loop forever */

} /* end main */

/************************************************/
/*												*/
/************************************************/

void pollenk()
{

/************************************************/
/*												*/
/* routine reads encoder outputs				*/
/*												*/
/* direct Port i/o								*/
/* PORTB bits to arduino pin #:					*/
/*	0 1  2  3  4  5  6 7 BIT#					*/
/*	8 9 10 11 12 13 (xtl6,xtl7) -ino dig pin#	*/
/*												*/
/* PORTB masks:									*/
/* Pin 8 = 0x01									*/
/* Pin 9 = 0x02									*/
/* Pin10 = 0x04									*/
/* Pin11 = 0x08									*/
/* Pin12 = 0x10									*/
/* Pin13 = 0x20									*/
/*												*/
/************************************************/
	/* define autos */

	/* toggle clock */
	clkpin *=-1;
	cnt1=0;
	cnt2=0;

	/*digitalWrite(clkout,LOW);*/
	PORTB &= clrclk; /* put pin low*/

	/* read data chan 1 and 2*/
	/*ic1=digitalRead(chan1);*/
	ic1 = PINB & getch1;
	ic2 = PINB & getch2;
	if(ic1>0)
	{
		/* debounce */
		ic1up++;
		ic1dn=0;
	}
	else
	{
		/* debounce */
		ic1dn++;
		ic1up=0;
	}
	if(ic1up>icdbnce)
	{
		/* see if A rising edge  */
		if(ch1state<0)
		{
			/* if B hi then REV */
			if(ch2state >0)
			{
					dir1=-1;
			} /* end set chan 1 direction Rev */
			else
			{
				/* if B lo then FWD */
				dir1=1;
			} /* end if B lo */

			/* inc tick count */
			cnt1=1;
		} /* end A rising edge */

		/* put ic1 pin hi */
		//digitalWrite(ch1out,HIGH);
		PORTB |= setch1;
		ch1state=1;
	}
	if(ic1dn>icdbnce)
	{
		/* see if A falling edge  */
		if(ch1state>0)
		{
			/* if B hi then FWD */
			if(ch2state >0)
			{
				dir1=1;
			} /* end set chan 1 direction Fwd */
			else
			{
				/*if B lo then REV */
				dir1=-1;
			} /* end if B lo */

			/* inc tick count */
			cnt1=1;
		} /* end A falling edge */
			
		/* put ic1 pin lo */
		//digitalWrite(ch1out,LOW);
		PORTB &= clrch1;
		ch1state=-1;
	}

	/* read data chan 2 */
	/*ic2=digitalRead(chan2);*/
	/*ic2 = PINB & getch2; */
	if(ic2>0)
	{
		/* debounce */
		ic2up++;
		ic2dn=0;
	}
	else
	{
		/* debounce */
		ic2dn++;
		ic2up=0;
	}
	if(ic2up > icdbnce)
	{
		/* see if B rising edge  */
		if(ch2state<0)
		{
			/* if A hi then FWD */
			if(ch1state >0)
			{
				dir2=1;
			} /* end set chan 1 direction Fwd */
			else
			{
				/* if A lo then REV */
				dir2=-1;
			} /* end if A lo */

			/* inc tick count */
			cnt2=1;
		} /* end B rising edge */

		/* put ic2 pin hi */
		//digitalWrite(ch2out,HIGH);
		PORTB |= setch2;
		ch2state=1;
	}
	if(ic2dn > icdbnce)
	{
		/* see if B falling edge  */
		if(ch2state>0)
		{
			/* if A hi then REV */
			if(ch1state >0)
			{
				dir2=-1;
			} /* end set chan 1 direction Rev */
			else
			{
				/* if A lo then FWD */
				dir2=1;
			} /* end if A lo */

			/* inc tick count */
			cnt2=1;
		} /* end B falling edge */

		/* put ic1 pin lo */
		//digitalWrite(ch2out,LOW);
		PORTB &= clrch2;
		ch2state=-1;
	}

	/* ch2 update - now write direction pin */
	if(dir1>0 && dir2>0) 
	{
		revc=0;
		fwdc++;
		if (fwdc>dirlim)
		{
			//digitalWrite(dirout,HIGH);
			PORTB |= setdir;
			dirf=1;
		}
	}
	if(dir1<0 && dir2<0)
	{
		fwdc=0;
		revc++;
		if(revc>dirlim)
		{
			//digitalWrite(dirout,LOW);
			PORTB &= clrdir;
			dirf=-1;
		}
	}

	/* now add/sub position cnt */
	if(dirf>0)
	{
		tcount += cnt1;
		tcount += cnt2;
	}
	if(dirf<0)
	{
		tcount -= cnt1;
		tcount -= cnt2;
	} /* end inc the counts

	/*digitalWrite(clkout,HIGH);*/
	PORTB |= setclk; /* put pin high */

	return;
} /* end sub pollenk */

/************************************************/
/*												*/
/************************************************/

int makbcd()
{

/************************************************/
/*												*/
/* makbcd - coverts long count to a decimal ascii*/
/* string, returns length of string				*/
/*												*/
/************************************************/
	/* define autos */
	/*long lnum,subn; */
	int lnum,subn;
	int dig,i,dig_cnt;
	char *s,*t;

	/* pick up current value of tick count */
	lnum=tcount;

	/* setup output string */
	t=tout;
	*t='+';
	dig_cnt=0;
	if(lnum<0) 
	{
		*t='-';
		lnum*= -1;
	}
	t++;

	/* test if case zero */
	if(lnum==0)
	{
		*(t++)='0';
        dig_cnt=1;
	} /* end fix case = 0 */

	/*poll encoder */
	pollenk();

	for (s=sout,i=0;lnum>0;i++)
	{
		/* loop extracting lsf digits */
		/*subn=long(lnum/10l);*/
		subn=int(lnum/10);

		/*poll encoder */
		pollenk();

		/*dig=int(lnum-subn*10l);*/
		dig=int(lnum-subn*10);
		*s=dig+0x30;
		s++;
		lnum=subn;

		/*poll encoder */
		pollenk();
	}

	/*poll encoder */
	pollenk();

	/* now put digits msd first */
	dig_cnt +=i;	 /*save # of digits */
	for(s--;i>0;i--)
	{
		*t++=*s--;
	} /*end sort into msd order */

	/*poll encoder */
	pollenk();

	/* finish string */
	*(t++)=',';
	*t='\0';

	/*set length of number string, incl sign and delimiter */
	/*dig=strlen(tout);*/
	dig_cnt +=2;

	/* return # valid chars in string */
	return dig_cnt;

} /* end routine makbcd */


/************************************************/
/*												*/
/************************************************/

int makhex()
{

/************************************************/
/*												*/
/* makhex - coverts long count to a hex ascii   */
/* string, returns length of string				*/
/*												*/
/************************************************/
	/* define autos */
	long lnum;
	int i,dig_cnt;
	char *s,*t, *ovlay,cwsl,cwsr;

	/* pick up current value of tick count */
	lnum=tcount;

	/* setup output string */
	t=tout;
	*t++='0';	
	*t++='x';

	/*poll encoder */
	pollenk();

	/* now print hex byte-wise version - 32 bit int*/
	for (ovlay = (char *) (&lnum),s=sout,i = 0; i<4; i++)
	{
		cwsr =*ovlay;
		cwsl =*(ovlay++);

		/* get high 4 bits */
		cwsl=cwsl >> 4; /*move high bits right */
		cwsl=cwsl & 0x0f;
		cwsr=cwsr & 0x0f;

		/*poll encoder */
		pollenk();

		/* save low(R) nibble as hex */
		*s=cwsr+0x30;
		if(*s > 0x39) 
		{
			*s += 0x27 ; /* letters to lower case */
		} /* end  out out A to F */

		/*poll encoder */
		pollenk();

		/* save high(L) nibble as hex */
		*(++s)=cwsl+0x30;
		if(*s > 0x39) 
		{
			*s += 0x27 ; /* letters to lower case */
		} /* end  out out A to F */

		s++;

		/*poll encoder */
		pollenk();
	} /* end loop over 8 nibbles */

	/*poll encoder */
	pollenk();

	/* now reverse order string */
	for (s--, i=0; i<8; i++)
	{
		*t++=*s--;
	} /* end format string */

	/*poll encoder */
	pollenk();

	/* finish string */
	*(t++)=',';
	*t='\0';
	dig_cnt=11;

	/* return # valid chars in string */
	return dig_cnt;

} /* end routine makhex */

/************************************************/
/*												*/
/************************************************/

void blowc(char s)
{

/************************************************/
/*												*/
/* blowc - puts a char out the hand rolled		*/
/* serial link									*/
/*												*/
/************************************************/
	/* define autos */
	byte mask;

	/* put out startbit */
	PORTD &= clrser; /* send a lo */

	/* wait in polling delay */
	polldla();

	/* shift bits out - lsb first */
	for (mask = 0x01; mask>0; mask <<= 1) 
	{
		/* select bit */
		if (s & mask)
		{
			PORTD |= setser; /* send a hi */
		}
		else
		{
			PORTD &= clrser; /* send a lo */
		}

    	/* wait in polling delay */
		polldla();
	}

	/* send stop bit */
	PORTD |= setser; /* send a hi */

      /* wait in polling delay */
	polldla();

	/* now wait out another byte-time or so */
	for (mask = 0; mask <15; mask++)
	{
		polldla();
	} /* end long pause */

} /* end routine blowc */

/************************************************/
/*												*/
/************************************************/

void polldla()
{

/************************************************/
/*												*/
/* polldla - waits 9600 baud delay by polling	*/
/*												*/
/************************************************/
	/* define autos */
	unsigned long mus, mus2;
	int i;

	/* note start time */
	mus=micros();

	for(i=0;i<=t1_9600;i++)
	{
		pollenk();
	} /* end polling delay */

	/* pick up time now */
	mus2=micros();
	mus2 -= mus;
	i = dla9600-int(mus2);

	/* now remainder us */
	if(i>0) 
	{
		delayMicroseconds(i);
	}

} /* end routine polldla */

/************************************************/
/*												*/
/************************************************/