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

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

#define PWMP	1000				// PWM period
#define QENC	(Intern_fio0pin3>>3&3)		// QENC input

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

static long qp;					// QDEC pos counter

/*
 * Timer0 interrupt service (FIQ)
 *
 * Quadrature DECoder
 *
 * 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).
 *
 * 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).
 */
static void t0_ack(void)
{
	Intern_t0ir	   = 1;			// clear int flag
	Intern_vicvectaddr = 0;			// reset VIC priority logic
}
static void __attribute__((interrupt))		// handle as ISR!
t0_isr(void)
{
	static const int qtbl[][4] =		// delta pos...
	{
		{ 0, -1,  1,  0},
		{ 1,  0,  0, -1},
		{-1,  0,  0,  1},
		{ 0,  1, -1,  0}
	};

	static int qc_old;			// QENC last code

	int qc = QENC;				// QENC cur code

	qp += qtbl[qc_old][qc];			// update pos
	qc_old = qc;				// save cur as old

	t0_ack();				// acknowledge int
}

/*
 * Power adjust (set PWM high pulse width)
 */
static inline void
Padjust(int P)
{
	P = PWMP-P;

	Intern_t2mr1 = P;
	Intern_t2mr2 = P;
	Intern_t3mr0 = P;
	Intern_t3mr3 = P;
}

int
main(void)
{
	FIQ_VEC		    = (long)t0_isr;	// set t0 ISR addr
	Intern_vicintenable = 0x10;		// enable t0 int
	Intern_vicintselect = 0x10;		// select t0 FIQ

	Intern_t0mr0  = _PCLK/100000-1;		// set MR0 to 10us
	Intern_t0mcr |= 3;			// int on MR0, reset
	Intern_t0tcr  = 1;			// go...

	Intern_t2pwmcon = 6;			// init PWMs...
	Intern_t3pwmcon = 9;
	Intern_t2pr	= 1;
	Intern_t3pr	= 1;
	Intern_t2mcr	= 2;
	Intern_t3mcr	= 0x10;
	Intern_t2mr0	= PWMP-1;
	Intern_t3mr1	= PWMP-1;

	Padjust(0);				// zero power

	Intern_t2tcr = 1;			// PWM
	Intern_t3tcr = 1;			//  go...

	Intern_pinsel0 |= 0x000a0000;		// select pin
	Intern_pinsel1 |= 0x20000800;		//  functions

	Intern_scs	=  1;			// use fast I/O
	Intern_fio0dir |=  0x000f0000;		// port direction
	Intern_fio0mask = ~0x180f0000;		//  and active mask

	ENABLE_INTERRUPTS;			// enable core ints

	Intern_pcon = 1;			// sleep mode (wait for FIQ)
	qp = 0;					// zero pos

	/*
	 * Command interface
	 *
	 * The QDEC position (=qp) is continuously displayed.
	 * To set the motor direction and power enter values
	 * from -1000 to 1000 (per mille, sign is direction).
	 */
	while (1) {
		printf("Pos: %ld\n", qp);
		if (kbhit()) {
			char line[10];		// input buffer
			int new;		// new run value
		
			fgets(line, sizeof(line), stdin);
			if (sscanf(line, "%d", &new) == 1 && abs(new) <= 1000) {

				/*
				 * Set direction and adjust power.
				 */
				if (new < 0) Intern_fio0pin2 = 0x5, Padjust(-new);
				else	     Intern_fio0pin2 = 0xa, Padjust( new);
			}
		}
	}
}