#include "include\MSP430x14x.h"
#include "include\PLDDriver.h"
#include "include\PWM.h"
#include "include\TrapServo.h"
#define abs(a) (((a)>=0)?(a):(-a))


//This is an exclectic mix of routines and test code to check out
//the first Dwarf Core board.

//This routine sets up the ports reguired to speak to the pld
//P1 is the address and data
//P3.0 AS
//P3.2 WR
//P3.4 RD
//P3.5 INT

static MotorChannel mc1;
long int relative;

void sendbyte(unsigned char tosend)
{
unsigned char temp;
temp = tosend >> 4;
tosend &= 0x0f;

if (temp >9 ) temp += ('A'-10);
else temp += '0';

while ((IFG2 & UTXIFG1) == 0);        // USART0 TX buffer ready?
       TXBUF1 = temp;                    //send it

if (tosend >9 ) tosend += ('A'-10);
else tosend += '0';

while ((IFG2 & UTXIFG1) == 0);        // USART0 TX buffer ready?
       TXBUF1 = tosend;                    //send it

}

unsigned char readbyte(void)
{
unsigned char temp;

while ((IFG2 & URXIFG1) == 0);        
		temp = RXBUF1;  
		 

	if(temp < 64)temp-= 48;
  	else if((temp>64)&(temp<=96))temp-=55;
  	else temp-=87;
                       
    return temp;

}

interrupt [TIMERA1_VECTOR] void timeraov(void)
{
	
  //need to read the interrupt vector to clear the interrupt
   switch (TAIV)  //read TAIV to determine reason for interrupt
    {              //case 10 would be the timer ov.
     case 10:  break;
     default: break; 
    } 
    
    getMotorPos(&mc1);
 	trapServoLoop(&mc1);
   
}
void startmove(long int relative,MotorChannel* mc)
{
  getMotorPos(mc);
  mc->goal_position = mc->position + relative;
  mc->stop_flag=0;
 
}

void main (void)
{
unsigned char tempchar;
unsigned char LB,CB,MB;
int temp,i;
unsigned char comm_bytes[4]={0,0,0,0};
unsigned int pos_holder;

//need the PLD control lines defined as soon as possible
initpldport();

//First Check if the XT2 Osc will run and at 4MHz.  configure the 
//Osc to run the SMclk and also enable the SMclk output on P5.5.

//first set the BCSCTL1 register to enable XT2

BCSCTL1 &=  ((XT2OFF)^0xff);

//osc2 is on, now wait to stabilize
for(temp=0;temp<32767;temp++);

//reset flag
IFG1 &= (unsigned char)(~(OFIFG));
//Then setup sources
BCSCTL2 |= (SELS | SELM_2);

//now drive the pin

P5DIR |= 0x30;
P5SEL |= 0x30;



//now configure the serial port1, assume 4MHz SMclk

UCTL1 = CHAR;                         // 8-bit character
UTCTL1 = SSEL1;                       // UCLK = SMCLK
U1BR0 = 0xa0;                         // 4e6/9600 = 416 = 1a0
U1BR1 = 0x01;                         //
UMCTL1 = 0xB6;                        // modulation
ME2 |= UTXE1 + URXE1;                 // Enable USART0 TXD/RXD
P3SEL |= 0xC0;                        // P3.6,7 = USART0 TXD/RXD
P3DIR |= 0x40;                        // P3.6 output direction
URCTL1 = URXEIE;						//Enable on all characters

//temporary for measuring
 //then setup port2 as output
 // P2DIR = 0xff;
 

PWM();

//Now that the PWM is running let's setup the interrupt!
//first stop timer A
TACTL = (TACLR | TASSEL0);
//Then set what to cont to 32Hz
TACCR0 = 1024;
//now start timer a with interrupts on, and count to CCR0.
TACTL |= (TAIE | MC0);

tempchar = readreg(0x0a);


writereg(0x06, 0x00);

writereg(0x06, 0x01);
P5OUT |= 0x02;
		//clear the interrupt
		writereg(wINTreg, (fBrake1 | 0x80));
		//release the brake
		P5OUT &= 0xfD;

writereg(INTenable, 0xE0);

//gMISCout &= (unsigned char)(~(Motor_pwr));
gMISCout |= (unsigned char)((Motor_pwr));


  //turn encoders on
	gPWRctrl |= Enc51;
	writereg(PWRctrl,gPWRctrl);

//position=0;
//encoderPosition=0;
//Initialize a motor channel
initMotorChannel(&mc1);

//start global interrupts
_EINT();

for(;;)
{

while ((IFG2 & URXIFG1) == 0)
{
tempchar = readreg(INTreg);
if (tempchar & fBrake1) 
  gMISCout &= (unsigned char)(~(M1_error));
else gMISCout |= M1_error;
if (tempchar & fBrake2) 
  gMISCout &= (unsigned char)(~(M2_error));
else gMISCout |= M2_error;
//if (P2IN & 0x80)
//  gMISCout &= (((unsigned char)(~(Activity))) & ((unsigned char)(~(Motor_pwr))));

writereg(MISCout, gMISCout);

};        // USART0 RX buffer ready?

       tempchar = RXBUF1;                    //read character
switch(tempchar)
{
case 'Q':P5SEL |= 0x20;  //turn on SMCLK
		break;
case 'q':P5OUT &= 0xDF;  //set line low
		P5SEL &= 0xDF; //deselt SMCLK
		break;
case 'L':MB = readreg(Limits_Switches);
		sendbyte(MB);
		break;
case 'H':gPWRctrl |= Home51;
		writereg(PWRctrl,gPWRctrl);
		break;
case 'h':gPWRctrl &= ((unsigned char)(~(Home51)));
		writereg(PWRctrl,gPWRctrl);
		break;
case 'Z':gHConfig |= EnableHome1;
		writereg(HConfig,gHConfig);
		break;
case '+':gHConfig |= sense1;
		writereg(HConfig,gHConfig);
		break;
case '-':gHConfig &= ((unsigned char)(~(sense1)));
		writereg(HConfig,gHConfig);
		break;
case 'M':gMISCout |= Motor_pwr;
		break;
case 'm':gMISCout &= ((unsigned char)(~(Motor_pwr)));
		break;	
case 'F':P5OUT |= 0x01;
        P5OUT &= 0xfd;
        break;
case 'R':P5OUT &= 0xfc;
        break;
case 'S':P5OUT |= 0x02;
        break;
case 'C':P5OUT |= 0x02;
		//clear the interrupt
		writereg(wINTreg, (fBrake1 | 0x80));
		//release the brake
		P5OUT &= 0xfD;
		break;
case 'E':gPWRctrl |= Enc51;
		writereg(PWRctrl,gPWRctrl);
		break;
case 'e':gPWRctrl &= ((unsigned char)(~(Enc51)));
		writereg(PWRctrl,gPWRctrl);
		break;
case 'P':LB = readreg(Encode_1_LB);
		CB = readreg(Encode_CB);
		MB = readreg(Encode_MB);
		while ((IFG2 & UTXIFG1) == 0);        // USART0 TX buffer ready?
        TXBUF1 = 0x0d;                    //send LF
		sendbyte(MB);
		sendbyte(CB);
		sendbyte(LB);
		while ((IFG2 & UTXIFG1) == 0);        // USART0 TX buffer ready?
       	TXBUF1 = 0x0a;                //send CR
		while ((IFG2 & UTXIFG1) == 0);        // USART0 TX buffer ready?
       	TXBUF1 = 0x0d;                    //send LF
       	break;
case 'j':	//JOG +dir
		for(i=0;i<4;i++)
		{
	   comm_bytes[i]=readbyte();
	   }
		pos_holder=0;
		pos_holder+=comm_bytes[3];
		pos_holder|=(comm_bytes[2]<<4);
		pos_holder|=(comm_bytes[1]<<8);
		pos_holder|=(comm_bytes[0]<<12);
		relative=0;
		relative=pos_holder;
		startmove(relative,&mc1);
		break;
case 'J':	//JOG -dir
		for(i=0;i<4;i++)
		{
	   comm_bytes[i]=readbyte();
	   }
		pos_holder=0;
		pos_holder+=comm_bytes[3];
		pos_holder|=(comm_bytes[2]<<4);
		pos_holder|=(comm_bytes[1]<<8);
		pos_holder|=(comm_bytes[0]<<12);
		relative=0;
		relative=pos_holder;
		startmove(-relative,&mc1);
		break;
case 'g':	//Go to pos
		for(i=0;i<4;i++)
		{
	   comm_bytes[i]=readbyte();
	   }
		pos_holder=0;
		pos_holder+=comm_bytes[3];
		pos_holder|=(comm_bytes[2]<<4);
		pos_holder|=(comm_bytes[1]<<8);
		pos_holder|=(comm_bytes[0]<<12);
		relative=0;
		relative=pos_holder-mc1.position;
		startmove(relative,&mc1);
		break;

}

while ((IFG2 & UTXIFG1) == 0);        // USART0 TX buffer ready?
       TXBUF1 = tempchar;                    //send it
}

}