/*
 * servo_timer.c
 *
 *  Created on: Jul 28, 2014
 *      Author: BHill
 */
#include  "msp430.h"
#define ti_quarter 30000
unsigned int motor[4],tcounter=0,ti_run=0;
//,PWM_SET[]={0x0F,0x0F,0x0F,0x0F,0x00,0x00,0x00,0x00,0x00,0x00},PWM_CLR[]={0x00,0x00,0x00,0x00,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F},PWM_CTR[]={8500,35000,8500,8500,60800,60800,60800,60800,31800};
//void calc_PWM(void);
void pwm_init(void){
	BCSCTL1 = CALBC1_16MHZ;                    // Set DCO
	DCOCTL = CALDCO_16MHZ;
	CCTL0 = CCIE;                           // TACCR0 interrupt enabled
	CCR0 = 50000;
	TACTL = TASSEL_2 + MC_2;                  // SMCLK, contmode

	P2DIR |=0x0F;
	motor[0]=0;
	motor[1]=0;
	motor[2]=0;
	motor[3]=0;
	__bis_SR_register(GIE);       			// interrupts enabled


	//		P2DIR |= 0x18;                            // P2.3 and P2.4 output
	//		  P2SEL |= 0x18;                            // P2.3 and P2.4 TA1/2 otions
	//		  TACCR0 = 24000;                         // PWM Period
	//		  TACCTL1 = OUTMOD_7;                       // TACCR1 reset/set
	//		  TACCR1 = 500;                             // TACCR1 PWM duty cycle
	//		  TACCTL2 = OUTMOD_7;                       // TACCR2 reset/set
	//		  TACCR2 = 0000;                             // TACCR2 PWM duty cycle
	//		  TACTL = TASSEL_2 + MC_1;                  // SMCLK, up mode

}




#pragma vector=TIMER0_A0_VECTOR
__interrupt void Timer_A (void)
{
	volatile int temp;
	if ((ti_run==0)&&(motor[tcounter]>85)){
		CCR0+=motor[tcounter];
		P2OUT|=(1<<tcounter);
	}
	else if(ti_run==0)
		ti_run++;
	if (ti_run==1){
		P2OUT&=~(1<<tcounter);
		if(motor[tcounter]<=85){
			CCR0+=ti_quarter;
		}
		else
			CCR0+=(ti_quarter-motor[temp]);
	}
	if (ti_run>1){
		tcounter++;
		CCR0+=ti_quarter;
	}
	ti_run++;
	if (ti_run>2)
		ti_run=0;
	if(tcounter>3){
		tcounter=0;
	}

}