#include <msp430.h> 


#define STEP1   BIT1        // P2.1 TA1.1
#define STEP2   BIT4        // P2.4 TA1.2
#define DIR1    BIT2        // P2.2
#define DIR2    BIT3        // P2.3
#define ADC1    BIT4        // P1.4
#define ADC2    BIT5        // P1.5
#define HYST    10          // hysteresis for pot comparator value


#define PERIOD   1000               // timer ticks for 5 steps/s (200 ms)
#define PULSE    200                 // timer ticks for minimum step time


int potPos1;                   //Declare variable to take ADC value
int motorPos1=0;
int delta1;
int potPos2;                   //Declare variable to take ADC value
int motorPos2=0;
int delta2;


void main(void)
{
WDTCTL = WDTPW | WDTHOLD; // stop watchdog timer


    //Set clock speed to be sure it is 1MHz
    BCSCTL1 = CALBC1_1MHZ;              //Basic Clock System CTL (1,8,12,16_MHZ available)
    DCOCTL = CALDCO_1MHZ;               //Digitally-Controlled Oscillator CTL


    // motor pins
    P2DIR |= STEP1+STEP2;
    P2OUT &= ~(STEP1+STEP2);
    P2SEL &= ~(STEP1+STEP2);       // initially off--select timer when stepping is needed


    P2DIR |= DIR1+DIR2;
    P2OUT |= DIR1+DIR2;          // initially + direction=1


    // Initialize P1.4 as the ADC input
    ADC10CTL0 = ADC10SHT_2 + ADC10ON + ADC10IE;     // ADC10 control register
                                        // ADC10SHT_2: ADC10 sample-and-hold time = 16 x ADC10CLKs
                                        // ADC10ON: ADC10 On/Enable
                                        // ADC10IE: ADC10 Interrupt Enable (flag set when ADC10MEM ready)
    ADC10CTL1 = INCH_4;                 // Selects Channel (initially INCH_4)
    ADC10AE0 |= ADC1;                   // Analog Input Enable (initially ADC1=BIT4)


    // set up timer for the stepper rate
    TA1CTL = TASSEL_2 + MC_1 + ID_3;    // SMCLK, up mode, divide by 8
    TA1CCR0 = PERIOD;                     // timer step period
    TA1CCR1 = PULSE;
    TA1CCTL1 = OUTMOD_7;                // PWM timer for motor 1
    TA1CCTL2 = OUTMOD_7;                // PWM timer for motor 2
    TA1CCTL0 = CCIE;


    // main while(1) infinite loop with state machine, sleep & wake, or just sleep (LPM)
    while(1){
        if(ADC10AE0 & ADC1){
            ADC10CTL1 = (ADC10CTL1 & ~INCH_15)|INCH_5;  // Selects Channel 5
        }
        else{
            ADC10CTL1 = (ADC10CTL1 & ~INCH_15)|INCH_4;  // Selects Channel 4
        }
        __bis_SR_register(LPM0_bits + GIE);
    }
}


// ADC10 interrupt service routine
#pragma vector=ADC10_VECTOR
__interrupt void ADC10_ISR (void)
{
    if(ADC10AE0 & ADC1){
        potPos1 = ADC10MEM;
        if((potPos1-HYST) > motorPos1){
            P2OUT |= DIR1;      // + direction
            delta1 = potPos1 - motorPos1;
            P2SEL |= STEP1;
        }
        else if((potPos1+HYST) < motorPos1){
            P2OUT &= ~DIR1;      // - direction
            delta1 = motorPos1 - potPos1;
            P2SEL |= STEP1;
        }
        else{
            P2SEL &= ~STEP1;
            delta1 = 0;
        }
        ADC10AE0 = (ADC10AE0 & ~ADC1)|ADC2;     // Analog Input Enable
        __no_operation();
    }
    else{
        potPos2 = ADC10MEM;
        if((potPos2-HYST) > motorPos2){
            P2OUT |= DIR2;      // + direction
            delta1 = potPos2 - motorPos2;
            P2SEL |= STEP2;
        }
        else if((potPos2+HYST) < motorPos2){
            P2OUT &= ~DIR2;      // - direction
            delta2 = motorPos1 - potPos1;
            P2SEL |= STEP2;
        }
        else{
            P2SEL &= ~STEP2;
            delta2 = 0;
        }
        ADC10AE0 = (ADC10AE0 & ~ADC2)|ADC1;     // Analog Input Enable
    }
    __bic_SR_register_on_exit(LPM0_bits);          // return on exit
}


// Timer ISR for TA1CCR0 interrupt
#pragma vector = TIMER1_A0_VECTOR
__interrupt void TA1_ISR(void) {
    // Read the analog voltage at the ADC
    ADC10CTL0 |= ENC + ADC10SC;         // ADC10 Enable Conversion
    if(P2SEL & STEP1){
        if(P2OUT & DIR1) motorPos1++;
        else             motorPos1--;
        delta1--;
        if(!delta1) P2SEL &= ~STEP1;
    }
    if(P2SEL & STEP2){
        if(P2OUT & DIR2) motorPos2++;
        else             motorPos2--;
        delta2--;
        if(!delta2) P2SEL &= ~STEP2;
    }
}