/*Firmware for TMC428/TMC288 evaluation board.
  
  (c) 2000 by Trinamic Microchips GmbH
  www.trinamic.com

  History: 03-07-2000 OK Creation
		   03-11-2000 OK Added most features
		   15-11-2000 OK Slight changes
		   13-02-2001 OK 8MHz Version
		   15-03-2001 OK Changed DIP switches

  Compile with GCC for AVR (Windows version)

  Set #define CRYSTAL_4MHZ to generate a 4MHz version.
*/

#include <io.h>
#include <progmem.h>
#include <interrupt.h>
#include <signal.h>
#include <wdt.h>
#include <eeprom.h>

#define FALSE 0
#define TRUE 1

typedef unsigned char byte;


/*PORT definitions*/
#define SPI_OUTPORT PORTB
#define SPI_INPORT  PINB
#define SPI_SCK 0
#define SPI_MOSI 1
#define SPI_CS 2
#define SPI_MISO 3
#define SPI_DEVSEL 4

#define LED_PORT PORTD
#define LED1	5
#define LED2	6

#define SW_PORT PIND
#define SW1 2
#define SW2 3
#define SW3 4

#define DIP_PORT PINB
#define DIP0 5
#define DIP1 7

#define MODE_PORT PORTB
#define MODE_BIT 4

/*State and command definitions*/
#define STATE_IDLE		0
#define STATE_SPI_START	1
#define STATE_SPI_SEND	2
#define STATE_WAIT_FOR_TXD 3
#define STATE_PRG_EEP_START 4
#define STATE_PRG_EEP 5

#define CMD_SPI4	35
#define CMD_ID	   255
#define CMD_EEP		69

#define ACK 6
#define NAK 21


/*The following configuration data for the TMC428 has to be placed in the EEPROM of the AT90S2313
  (using the programmer or the EEPROM loading program). It is copied to the TMC428 configuration RAM
  after each reset.
  It contains the driver configuration and the mircostepping sine wave table.

  byte  
    TMC428Cfg[]={0x11, 0x11, 0x11, 0x11, 0x11, 0x10, 0x10, 0x0F, 0x07, 0x0E, 0x06, 0x0D, 0x0C, 0x0B, 0x0A, 0x09,
				 0x08, 0x05, 0x04, 0x03, 0x02, 0x01, 0x00, 0x30, 0x11, 0x11, 0x10, 0x11, 0x06, 0x10, 0x11, 0x2E,
				 0x11, 0x11, 0x10, 0x11, 0x06, 0x10, 0x11, 0x2E, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
				 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
				 0x00, 0x01, 0x03, 0x04, 0x06, 0x07, 0x09, 0x0A, 0x0C, 0x0E, 0x0F, 0x11, 0x12, 0x14, 0x15, 0x17,
				 0x18, 0x19, 0x1B, 0x1C, 0x1E, 0x1F, 0x20, 0x22, 0x23, 0x24, 0x26, 0x27, 0x28, 0x29, 0x2A, 0x2C,
				 0x2D, 0x2E, 0x2F, 0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x36, 0x37, 0x38, 0x39, 0x39, 0x3A,
				 0x3B, 0x3B, 0x3C, 0x3C, 0x3D, 0x3D, 0x3E, 0x3E, 0x3E, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F};
*/


#define FIFO_SIZE 10
volatile byte RecvFIFO[FIFO_SIZE];
volatile byte RecvWritePtr, RecvReadPtr;
volatile byte SendCount;
volatile byte *SendPtr;
byte incoming[4], outgoing[4];


/*UART interrupt handling*/
SIGNAL(SIG_UART_RECV)	/*byte completely received*/
{
	int i;				/*put it into receive FIFO*/

	i=RecvWritePtr+1;
	if(i==FIFO_SIZE) i=0;
	
	if(i!=RecvReadPtr)
	{
		RecvFIFO[RecvWritePtr]=inp(UDR);
		RecvWritePtr=i;
	}
}

SIGNAL(SIG_UART_TRANS)	/*byte completely transmitted*/
{
	if(SendCount>0)		/*send the next byte if there is one*/
	{
		outb(*SendPtr, UDR);
		SendPtr++;
		SendCount--;
	}
}


/*Read a byte from the receive FIFO
  *abyte: the byte
  Return value:  TRUE: a byte has been successfully read
				 FASLE: the FIFO is empty
*/
byte ReadFromFIFO(byte *abyte)
{
	if(RecvReadPtr==RecvWritePtr) return FALSE;

	*abyte=RecvFIFO[RecvReadPtr++];
	if(RecvReadPtr==FIFO_SIZE) RecvReadPtr=0;
	return TRUE;
}

/*Send and receive 4 byte SPI telegrammes.
  incoming: 4 byte array of incoming bytes	
  outgoing: 4 byte array of outgoing bytes	
*/
void spi4(byte *incoming, byte *outgoing)
{
	byte a;
	int i, j;

	/*Set CS Low*/
	cbi(SPI_OUTPORT, SPI_CS);

	for(i=0; i<4; i++)
	{
		a=outgoing[i];

		for(j=0; j<8; j++)
		{
			/*Falling edge of SCK*/
			cbi(SPI_OUTPORT, SPI_SCK);

			/*output the MSB to MOSI*/
			if(a & 0x80)	sbi(SPI_OUTPORT, SPI_MOSI);
			else			cbi(SPI_OUTPORT, SPI_MOSI);

			/*Shift out the MSB*/
			a<<=1;

			/*Rising edge of SCK*/
			sbi(SPI_OUTPORT, SPI_SCK);

			/*Read a bit from MISO*/
			if(bit_is_set(SPI_INPORT, SPI_MISO)) a|=1;
		}

		incoming[i]=a;
	}

	/*Set CS High again*/
	sbi(SPI_OUTPORT, SPI_CS);
}

void spi_telegramme(byte a, byte b, byte c, byte d)
{
	outgoing[0]=a;
	outgoing[1]=b;
	outgoing[2]=c;
	outgoing[3]=d;

	spi4(incoming, outgoing);
}


int main(void)
{
	byte InputByte, CurrentState, Count, Motor;
	byte spi_in[4], spi_out[4], eep[3], StopFlag;
	unsigned int addr, Sw1, Sw2, Sw3;
	char *IdString="EV428 V1.1";

	/*Initialization*/
	RecvWritePtr=RecvReadPtr=SendCount=Sw1=Sw2=Sw3=StopFlag=0;

	/*Data direction bits and pull-up resistors*/
	outb(0x17, DDRB);
	outb(0xe0, PORTB);
	outb(0x62, DDRD);
	outb(0x9c, PORTD);

	/*Inital output values*/
	/*Turn LEDs off*/
	sbi(LED_PORT, LED1);
	sbi(LED_PORT, LED2);

	/*SPI*/
	sbi(SPI_OUTPORT, SPI_CS);
	sbi(SPI_OUTPORT, SPI_SCK);
	cbi(SPI_OUTPORT, SPI_MOSI);
	cbi(SPI_OUTPORT, SPI_DEVSEL);


	/*UART*/
	outb(0xd8, UCR);  /*TX/RX interrupt enable, 8 Bits*/
#ifndef CRYSTAL_4MHZ
	outb(25,  UBRR);  /*19200 bps @ 8 MHz*/
#else
	outb(12,  UBRR);  /*19200 bps @ 4 MHz*/
#endif

	/*Initialize the TMC428
	  Driver SPI configuration
	  DIP Switches: 00: 1 motor driver
	  				01:	2 motor drivers
	  				10: 3 motor drivers
					11: also 3 motor drivers and maybe something special
	*/

	/*Some delay after reset, flash LEDs*/
	cbi(LED_PORT, LED1);
	cbi(LED_PORT, LED2);
#ifndef CRYSTAL_4MHZ
	for(addr=0; addr<65535; addr++);
#else
	for(addr=0; addr<32767; addr++);
#endif
	sbi(LED_PORT, LED1);
	sbi(LED_PORT, LED2);

	/*SMGP: Polarity Bits: PoFD inverted, LSMD set according to DIP switches*/
	spi_telegramme(0x7e,0x11, 0x07, bit_is_clear(DIP_PORT, DIP0) ? 0x22 : (bit_is_clear(DIP_PORT, DIP1) ? 0x21 : 0x20));

	/*i_s_agtat / i_s_aleat / i_s_v0 / a_threshold for A3972 (SMDA 00) */
	spi_telegramme(0x10, 0x00, 0x10, 0x40);

#ifndef CRYSTAL_4MHZ
	/*Pulsdiv/Rampdiv/µStep-Resolution for A3972 (SMDA 00)*/
	spi_telegramme(0x18, 0x00, 0x2a, 0x07);

	/*Pulsdiv/Rampdiv/µStep-Resolution for L9935 (SMDA 01 & SMDA 10)*/
	spi_telegramme(0x38, 0x00, 0x8a, 0x00);
	spi_telegramme(0x58, 0x00, 0x8a, 0x00);
#else
	/*Pulsdiv/Rampdiv/µStep-Resolution for A3972 (SMDA 00)*/
	spi_telegramme(0x18, 0x00, 0x19, 0x07);

	/*Pulsdiv/Rampdiv/µStep-Resolution for L9935 (SMDA 01 & SMDA 10)*/
	spi_telegramme(0x38, 0x00, 0x79, 0x00);
	spi_telegramme(0x58, 0x00, 0x79, 0x00);
#endif

	/*Driver configuration and microstepping table*/
	spi_out[1]=0;
	for(addr=128; addr<256; addr+=2)
	{
		spi_out[0]=(byte) addr;

		while(!eeprom_is_ready());
		spi_out[2]=eeprom_rb(addr-127);
		
		while(!eeprom_is_ready());
		spi_out[3]=eeprom_rb(addr-128);

		spi4(spi_in, spi_out);
	}

	/*Activate the A3972*/
	spi_telegramme(0x64, 0x00, 0x05, 0x13);
	spi_telegramme(0x66, 0x04, 0x04, 0x83);

	/*Enable Watchdog*/
	wdt_enable(7);

	/*All initialization tasks done, so we can enable the interrupts and enter the main loop*/
	sei();  /*enable interrupts*/

	/*The Main Loop*/
	Count=0;
	CurrentState=STATE_IDLE;
	for(;;)
	{
		if(CurrentState==STATE_IDLE) 
		{
			if(ReadFromFIFO(&InputByte))
			{
				if(InputByte==CMD_SPI4)		//PC sends SPI telegramme
				{
					CurrentState=STATE_SPI_START;
					Count=0;
				} 
				else if(InputByte==CMD_ID)
				{
					CurrentState=STATE_WAIT_FOR_TXD;
					SendPtr=IdString+1;
					SendCount=9;
					outb(IdString[0], UDR);
				}
				else if(InputByte==CMD_EEP) 
				{
					Count=0;
					CurrentState=STATE_PRG_EEP_START;
				}
			} 
		}
		else if(CurrentState==STATE_SPI_START && ReadFromFIFO(&InputByte))
		{
			//wait until the four SPI bytes have arrived from the PC
			spi_out[Count++]=InputByte;
			if(Count==4) CurrentState=STATE_SPI_SEND;
		}
		else if(CurrentState==STATE_SPI_SEND)				//send telegramme to TMC428, then send reply to PC
		{
			spi4(spi_in, spi_out);
			
			SendPtr=spi_in+1;
			SendCount=3;
			outb(spi_in[0], UDR);

			CurrentState=STATE_WAIT_FOR_TXD;
		}
		else if(CurrentState==STATE_WAIT_FOR_TXD)			//wait until all reply bytes have been sent
		{
			if(SendCount==0) CurrentState=STATE_IDLE;
		}
		else if(CurrentState==STATE_PRG_EEP_START && ReadFromFIFO(&InputByte))  //get EEPROM programming bytes
		{																		//address, data, checksum	
			eep[Count++]=InputByte;
			if(Count==3) CurrentState=STATE_PRG_EEP;
		}
		else if(CurrentState==STATE_PRG_EEP)
		{
			SendCount=0;

			if(eep[2]==(eep[0]+eep[1]) % 255)
			{
				eeprom_wb(eep[0], eep[1]);
				while(!eeprom_is_ready());

				outb(ACK, UDR);
			} else outb(NAK, UDR);
			
			CurrentState=STATE_IDLE;
		}

		//See if a key has been pressed
		//SW1: Run all motors in "positive" direction
		if(bit_is_clear(SW_PORT, SW1))
		{
			if(Sw1<5000) Sw1++;
		}
		else Sw1=0;

		if(Sw1==5000)
		{
			StopFlag=FALSE;

			for(Motor=0; Motor<65; Motor+=32)  //set Ramp_Mode to VELOCITY, disable reference switches
				spi_telegramme(0x14 | Motor, 0, 0, 2);

			spi_telegramme(0x0c, 0, 0x01, 0x00);  //a_max
			spi_telegramme(0x06, 0, 0x07, 0xff);  //v_max
			spi_telegramme(0x08, 0, 0x07, 0xff);  //v_target
			spi_telegramme(0x2c, 0, 0x03, 0xff);
			spi_telegramme(0x26, 0, 0x03, 0xff);
			spi_telegramme(0x28, 0, 0x0c, 0x01);
			spi_telegramme(0x4c, 0, 0x00, 0x64);
			spi_telegramme(0x46, 0, 0x04, 0x00);
			spi_telegramme(0x48, 0, 0x04, 0x00);

			sbi(LED_PORT, LED1);
			cbi(LED_PORT, LED2);
			
			Sw1++;
		}

		//SW2: Stop all motors (smoothly after pressing once, immediately after pressing twice)
		if(bit_is_clear(SW_PORT, SW2))
		{
			if(Sw2<5000) Sw2++;
		}
		else Sw2=0;

		if(Sw2==5000)
		{
			for(Motor=0; Motor<65; Motor+=32)
			{
				if(StopFlag)
				{
					spi_telegramme(0x01 | Motor, 0, 0, 0);  //read XTarget
					spi_telegramme(0x02 | Motor, incoming[1], incoming[2], incoming[3]);  //set XActual=XTarget
				}
				spi_telegramme(0x08 | Motor, 0, 0, 0);  //set VTarget=0
				if(StopFlag) spi_telegramme(0x0a | Motor, 0, 0, 0);  //set VActual=0
			}

			if(!StopFlag) StopFlag=TRUE;

			sbi(LED_PORT, LED1);
			sbi(LED_PORT, LED2);
			
			Sw2++;
		}

		//SW3: Run all motors in "negative" direction
		if(bit_is_clear(SW_PORT, SW3)) 
		{
			if(Sw3<5000) Sw3++;
		}
		else Sw3=0;

		if(Sw3==5000)
		{
			StopFlag=FALSE;

			for(Motor=0; Motor<65; Motor+=32)  //set Ramp_Mode to VELOCITY, disable reference switches
				spi_telegramme(0x14 | Motor, 0, 0, 2);

			spi_telegramme(0x0c, 0, 0x01, 0x00);  //a_max
			spi_telegramme(0x06, 0, 0x07, 0xff);  //v_max
			spi_telegramme(0x08, 0, 0x08, 0x01);  //v_target
			spi_telegramme(0x2c, 0, 0x03, 0xff);
			spi_telegramme(0x26, 0, 0x03, 0xff);
			spi_telegramme(0x28, 0, 0x03, 0xff);
			spi_telegramme(0x4c, 0, 0x00, 0x64);
			spi_telegramme(0x46, 0, 0x04, 0x00);
			spi_telegramme(0x48, 0, 0x0c, 0x00);

			sbi(LED_PORT, LED2);
			cbi(LED_PORT, LED1);
			
			Sw3++;
		}

			
		/*Reset the watchdog*/
		wdt_reset();
	}
}