/*
 * Copyright(C) Paul und Scherer (mct.de/mct.net)
 *
 * This example demonstrates how to...
 *
 *  ... decode signals from a quadrature encoder.
 */

#include <stdio.h>
#include <sys/arm7tdmi.h>
#include <target.h>

/*
 * The ADuC7128 contains a "quadrature encoder" - at least Analog Devices
 * says so in the data sheet...
 *
 * A quadrature encoder is something on the shaft of a motor, giving some
 * information about its angular position. So, what they meant to say, is
 * "a quadrature encoder INTERFACE". This happens to be just the contrary
 * - namely a quadrature DECODER. So far for the terminology...
 *
 * Having used QDECs before (e.g TPU based QDECs by Motorola), I wondered
 * about the resolution of the on-chip quadrature encoder interface being
 * four times lower than the resolution of a TPU-QDEC (ran simultaneously
 * and connected to the same input lines).
 *
 * Even more puzzling was the fact, that the two (completely independent)
 * position counters began to show significant differences, after running
 * the motor for some time. By marking the initial motor position I found
 * out that the builtin position counter ran off this zero reference. The
 * effect could be reproduced even with no power applied to the motor and
 * moving it manually instead.
 *
 * The QEN configuration register allows to activate a "filter" function,
 * which I tried without noticing an effect.
 *
 * I then built a soft-QDEC, running on the ADuC7128, using the same port
 * pins as the builtin hard-QDEC. Both, hard and soft-QDEC running at the
 * same time. With the external TPU-QDEC still connected, now THREE QDECs
 * were running parallel, using the very same signal lines!
 *
 * Result: The TPU-QDEC and the soft-QDEC never disagreed.
 *
 * I contacted Analog Devices and although they told me to investigate in
 * this matter, they did not provide me with anything helpful, not even a
 * QEN sample program, I had asked them for...
 *
 * Here are the two parallel running QDECs:
 *
 *  1. Hard-QDEC using the QEN hardware    (position count is QEN_pos)
 *  2. Soft-QDEC using the QEN inputs only (position count is FIQ_pos)
 */

#define QEN_pos	((short)Intern_qenval)		// QEN pos count

extern char _bdata;				// data start
#define FIQ_VEC	(*((long *)&_bdata+15))		// FIQ vector

#define QENC	(Intern_gp4dat&3)		// QENC input

static int old_code;				// last QENC code
static int FIQ_pos;				// FIQ pos count
static int err;					//     err count

/*
 * FIQ service (Quadrature Decoder)
 *
 * Only t0 is enabled for FIQ,
 * no need for polling FIQSTA.
 *
 * Theory of operation:
 *
 * The encoder provides two signals (pulses),
 * which are phase-shifted by 90 degrees, and
 * so can take together 4 different states. A
 * special coding of these states (Gray code)
 * avoids multiple bit changes at transitions
 * (i.e. only ONE bit at most can change from
 * one state to another).
 *
 *  State code
 *  ----- ----
 *    1   00 =0
 *    2   01 =1
 *    3   11 =3
 *    4   10 =2
 *    1   00 =0
 *    .    .
 *    .    .
 *
 * The position counter is updated, according
 * to state transitions (+/- 1). In case both
 * bits change, an error has occurred.
 *
 * In each state there is ONE state, that can
 * not follow this state. In case the current
 * state happens to be an illegal follower of
 * the previous state, this indicates, that a
 * transition has been missed, or the signals
 * are distorted.
 *
 *  Previous code  illegal follower
 *  -------------  ----------------
 *      00 =0         11 =3 =3-0
 *      01 =1         10 =2 =3-1
 *      11 =3         00 =0 =3-3
 *      10 =2         01 =1 =3-2
 *
 * So the current code is an illegal follower
 * of the previous code, when it equals three
 * minus the previous code.
 *
 * The current and previous codes are used as
 * indices into an array, holding the offsets
 * to be added to the position counter (1, -1
 * or 0).
 *
 * Note: The previous code must be read once,
 * before entering the decoder!
 */
static void fiq_ack(void) {Intern_t0iclr = 0;}	// clear int flag
static void __attribute__((interrupt))		// handle as ISR!
fiq_sr(void)
{
	static const int table[][4] =		// delta pos...
	{					//     new ->
		{ 0, -1,  1,  0},		// old 0 1 2 3
		{ 1,  0,  0, -1},		//  |  1
		{-1,  0,  0,  1},		//  v  2
		{ 0,  1, -1,  0}		//     3
	};

	int new_code = QENC;			// get cur code

	if (new_code == 3-old_code) err++;	// count up errors
	FIQ_pos += table[old_code][new_code];	// update pos
	old_code = new_code;			// save cur as old
	fiq_ack();				// acknowledge int
}

/*
 * The QEN(coder) - which is a QDE(coder) in reality - is
 * a position counter, which counts up or down, depending
 * on two signals, usually called A and B from an encoder
 * used in motor control.
 *
 * Connect A, B from the encoder to S1, S2 (=P4.0, P4.1).
 */
int
main(void)
{
	int QEN_old = 0x8000;			// last QEN_pos
	int FIQ_old = 0x8000;			// last FIQ_pos

	FIQ_VEC = (long)fiq_sr;			// set FIQ SR addr
	Intern_fiqen = 4;			// enable t0 for FIQ

	Intern_t0ld  = _CCLK/100000;		// set load to 10us
	Intern_t0con = 0xc0;			// periodic @cclk

	old_code = QENC;			// get last code

	Intern_qencon  = 0x401;			// enable QEN
	Intern_gp4con |= 0x11;			//        S1, S2

	ENABLE_INTERRUPTS;			// enable core ints

	/*
	 * Read the positions and display changes.
	 */
	while (1) if (QEN_pos != QEN_old
		   || FIQ_pos != FIQ_old) printf("QEN_pos: %d, FIQ_pos: %d (Errors: %d)\n",
						  QEN_old = QEN_pos,
						  FIQ_old = FIQ_pos, err);
}