#include  <msp430.h>

int main(void) {
	WDTCTL = WDTPW + WDTHOLD;

	P1DIR |= BIT0;							// Internal LEDs P1.0 of Launchpad is output

	P1DIR |= BIT6;							// P1.6 is used for PWM -> servo 1
	P2DIR |= BIT2;							// P2.2 is used for PWM -> servo 2

	P1OUT = 0; 									// Clear all outputs P1
	P2OUT = 0; 									// Clear all outputs P2

	P1SEL |= BIT6;							// P1.6 select TA0.1 option
	P2SEL |= BIT2;							// P2.2 select TA1.1 option

	// if SMCLK is about 1MHz (or 1000000Hz),
	// and 1000ms are the equivalent of 1 Hz,
	// then, by setting CCR0 to 20000 (1000000 / 1000 * 20)
	// we get a period of 20ms
	TA0CCR0 = 25000-1;					// PWM Period TA0.1


	// setting 1600 is 1.5ms is 0deg. servo pos
	TA0CCR1 = 1600;							// CCR1 PWM duty cycle


	TA0CCTL1 = OUTMOD_7;				// CCR1 reset/set
	TA0CTL   = TASSEL_2 + MC_1;	// SMCLK, up mode

	unsigned int i=0;

	for (;;) {
		TA0CCR1 = 600;
		__delay_cycles(1000000);
		TA0CCR1 = 2600;
		__delay_cycles(1000000);

		TA0CCR1 = 1000 + (i*20);	// testing out stepping
		i++;
		__delay_cycles(200000);
		if(i>98) {
			i=0;
		}
	}
}
