#include <msp430.h>
#include "servo_timer.h"
#include "uart_control.h"

volatile int motor_code,pulsenum;
volatile int M1[]={0,32000,32000,16000,16000};
volatile int M2[]={0,16000,32000,16000,32000};

void parse_string(void);
void main(void){
	volatile int pcount=0,k;

	WDTCTL = WDTPW | WDTHOLD;
	pwm_init();
	uart_init(4);
	pulsenum=0;
	motor_code=0;
	P1OUT|=BIT3;
	P1DIR|=(BIT0+BIT6);
	P1OUT|=BIT0;
	P1REN|=BIT3;
	//	motor[0]=12000;
	//	motor[1]=12000;
	TA1CCR0 = 50000; // Cargamos el periodo PWM
	TA1CCTL0 = CCIE; //CCR1 Interrupt enabled
	TA1CTL = TASSEL_2 + MC_2+ID_3; // Timer SMCLK Modo UP
	_BIS_SR( GIE); // Bajo consumo LPM0
	while(1){
		if ((P1IN&BIT3)==0){		//Button Pressed on Pin 1.3
			while (!(IFG2&UCA0TXIFG));
			UCA0TXBUF='x';
		}
		if (eos_flag){
			eos_flag=0;
			parse_string();
			rx_flag=0;
		}



	}

}

void parse_string(void){
	volatile int veclen=7,incode,intime,k;
	if (rx_data_str[0]!='#')
		return;
	if (rx_data_str[rx_flag-1]=='*')
		return;
	if (rx_flag<veclen)
		return;
	if (rx_flag>veclen)
		return;
	incode=rx_data_str[1]-0x30;
	intime=0;
	for(k=0;k<4;k++){
		intime*=10;
		intime+=rx_data_str[k+2]-0x30;
	}
	pulsenum=intime;
	motor_code=incode;

}

// Timer A0 interrupt service routine
#if defined(__TI_COMPILER_VERSION__) || defined(__IAR_SYSTEMS_ICC__)
#pragma vector=TIMER1_A0_VECTOR
__interrupt void Timer_A1 (void)
#elif defined(__GNUC__)
void __attribute__ ((interrupt(TIMER0_A0_VECTOR))) Timer_A (void)
#else
#error Compiler not supported!
#endif
{
	P1OUT ^= 0x01;                            // Toggle P1.0
	if (pulsenum){
		motor[0]=M1[motor_code];
		motor[1]=M2[motor_code];

		pulsenum--;

	}
	else if (pulsenum<0)
		pulsenum=0;
	else{
		motor[0]=M1[0];
		motor[1]=M2[0];
	}

	TA1CCR0 += 50000;                            // Add Offset to CCR0
}