/*
 * Camera_Motor_EVM.c
 *
 *  Created on: May 6, 2020
 *      Author: a0273119
 */
#include <msp430.h>
#include <Camera_Motor_EVM.h>



//void Start_Motors(void){
//    //Check enable flags
//    if(FOCUS_MOTOR_ENABLE){ FOCUS_ENABLE; }     // set EN pin high on Focus Motor Driver
//    if(TILT_MOTOR_ENABLE){  TILT_ENABLE; }      // set EN pin high on Tilt Motor Driver
//    if(IRIS_MOTOR_ENABLE){                      // set nSleep pin high on IRIS Motor Driver
//        IRIS_WAKEUP;
//        TB1CTL |= MC__UP|TBCLR;         // Start PWM
//        TB3CTL |= MC__UP|TBCLR;         // Start timer for timeout
//    }
//
//    // Wait 1 ms wake-up time from enabling stepper drivers to starting step signals
//    __delay_cycles(SMCLK_FREQ/1000);
//
//    //Start PWM Timers
//    if(FOCUS_MOTOR_ENABLE){ TB0CTL |= MC__UP|TBCLR; }       //Start Timer for Focus step PWM
//    if(TILT_MOTOR_ENABLE){  TB2CTL |= MC__UP|TBCLR; }       //Start Timer for Tilt step PWM
//}

void Start_IRIS_Motor(void){
    //Check enable flags
    if(IRIS_MOTOR_ENABLE){                      // set nSleep pin high on IRIS Motor Driver
        IRIS_WAKEUP;
        TB1CTL |= MC__UP|TBCLR;         // Start PWM
        TB3CTL |= MC__UP|TBCLR;         // Start timer for timeout
    }
}

void Start_Focus_Motor(void){
    //Check enable flags
    if(FOCUS_MOTOR_ENABLE){ FOCUS_ENABLE; }     // set EN pin high on Focus Motor Driver

    // Wait 1 ms wake-up time from enabling stepper drivers to starting step signals
    __delay_cycles(SMCLK_FREQ/1000);

    //Start PWM Timers
    if(FOCUS_MOTOR_ENABLE){ TB0CTL |= TBSSEL__SMCLK | MC__UP| ID__8  | TBCLR | TBIE; }       //Start Timer for Focus step PWM
}

void Focus_Motor_Update_Direction(void){

    //Config Direction pin
    if(FocusMotorDirection == Clockwise){
        P3OUT |= FOCUS_DIR;
    }else{
        P3OUT &= ~FOCUS_DIR;
    }
    P3DIR |= FOCUS_DIR;

}

void Start_Tilt_Motor(void){
    //Check enable flags
    if(TILT_MOTOR_ENABLE){  TILT_ENABLE; }      // set EN pin high on Tilt Motor Driver

    // Wait 1 ms wake-up time from enabling stepper drivers to starting step signals
    __delay_cycles(SMCLK_FREQ/1000);

    //Start PWM Timers
    if(TILT_MOTOR_ENABLE){  TB2CTL |= TBSSEL__SMCLK | MC__UP | ID__8  | TBCLR | TBIE; }       //Start Timer for Tilt step PWM
}

void Tilt_Motor_Update_Direction(void){

    //Config Direction pin
    if(TiltMotorDirection == Clockwise){
        P4OUT |= TILT_DIR;
    }else{
        P4OUT &= ~TILT_DIR;
    }
    P4DIR |= TILT_DIR;

}

void Init_Iris_Motor(void){
    unsigned int PWM_Period, PWM_Duty_Period;

    // Put Driver in Sleep mode
    IRIS_SLEEP;
    P3DIR |= IRIS_nSLEEP;                     // Set pin as output

    // config IN GPIOs for "coast" during setup
    P2OUT &= ~(IRIS_IN1 + IRIS_IN2);
    P2DIR |= IRIS_IN1 + IRIS_IN2;

    //Calculate PWM period and duty cycle
    PWM_Period = SMCLK_FREQ / IRIS_MOTOR_PWM_FREQ_HZ -1;
    PWM_Duty_Period = (unsigned int)( ((float)IRIS_MOTOR_VOLTAGE / MOTOR_DRIVE_INPUT_VOLTAGE) * PWM_Period );

    //TB1 used for IRIS Motor
    TB1CCR0 = PWM_Period;                       // PWM Period
    TB1CCR1 = PWM_Duty_Period;                  // CCR1 PWM duty cycle
    TB1CCR2 = PWM_Duty_Period;                  // CCR2 PWM duty cycle
    TB1CTL = TBSSEL__SMCLK | TBCLR;    // SMCLK, up mode, clear TBR

    //TB3 used for 1ms tick for timeout function
    TB3CCR0 = SMCLK_FREQ/1000;                  // 1MS Period
    TB3CTL = TBSSEL__SMCLK | TBCLR | TBIE;    // SMCLK, up mode, clear TBR

    //Configure motor direction
    if(IrisMotorDirection == Clockwise){    //IN1=PWM, IN2=1
        P2SEL0 |= IRIS_IN2;
        P2OUT  |= IRIS_IN1;
        TB1CCTL1 = OUTMOD_3;                // CCR1 set/reset - IN2 PWM output
    }
    else{  // counterclockwise              //IN1=1, IN2=PWM
        P2SEL0 |= IRIS_IN1;
        P2OUT  |= IRIS_IN2;
        TB1CCTL2 = OUTMOD_3;                // CCR2 set/reset - IN1 PWM output
    }
}




void Init_Focus_Motor(void){
    unsigned int PWM_Period = 0;

    //Disable Motor driver for configuration
    FOCUS_DISABLE;
    P3DIR |= FOCUS_EN;
//    P3DIR &= ~FOCUS_nFAULT;                           //Not supported in demo

    //Config Sleep pin -- Sleep not supported in Demo
    P3OUT |= FOCUS_nSLEEP;
    P3DIR |= FOCUS_nSLEEP;

    //Config Direction pin
    if(FocusMotorDirection == Clockwise){
        P3OUT |= FOCUS_DIR;
    }else{
        P3OUT &= ~FOCUS_DIR;
    }
    P3DIR |= FOCUS_DIR;

    //Config GPIO and Timer for Step PWM
    FOCUS_STEP_PIN_LOW;
    P1DIR |= FOCUS_STEP;      // Set pin as output

    //Photo Interupt
    P1DIR &= ~PI_INPUT;
    P1REN |= PI_INPUT;      // internal pull up resistor
    P1IES |= PI_INPUT;                          // P1.1 Hi/Low edge
    P1IE  |= PI_INPUT;                          // P1.1 interrupt enabled
    P1IFG &= ~PI_INPUT;                         // P1.1 IFG cleared

    PWM_Period = (SMCLK_FREQ /8 /FOCUS_MOTOR_SPEED_PPS) -1;                         // PWM Period
    TB0CCR0 = PWM_Period;
    TB0CCR1 = PWM_Period/2;                   // CCR1 PWM duty cycle
    TB0CCTL1 = CCIE;                          // CCR1 ISR enabled
    TB0CTL = TBSSEL__SMCLK | ID__8 | TBCLR | TBIE;  // SMCLK, clock divide by 8 (24MHz/8= 3MHz), clear TBR

    //Configure M0 and M1 GPIO for correct Step Size
    switch(FocusMotorStepSetting){
        case Step_Full:             // M0=0, M1=0
            P1DIR |= FOCUS_M0 + FOCUS_M1;
            P1OUT &= ~(FOCUS_M0 + FOCUS_M1);
            break;

        case Step_1_2_Noncircular:  // M0=1, M1=0
            P1DIR |= FOCUS_M0 + FOCUS_M1;
            P1OUT |= FOCUS_M0;
            P1OUT &= ~(FOCUS_M1);
            break;

        case Step_1_2:              // M0=Hi-Z, M1=0
            P1DIR &= ~FOCUS_M0;
            P1DIR |= FOCUS_M1;
            P1OUT &= ~FOCUS_M1;
            break;

        case Step_1_4:              // M0=0, M1=1
            P1DIR |= FOCUS_M0 + FOCUS_M1;
            P1OUT &= ~(FOCUS_M0);
            P1OUT |= FOCUS_M1;
            break;

        case Step_1_8:              // M0=1, M1=1
            P1DIR |= FOCUS_M0 + FOCUS_M1;
            P1OUT |= FOCUS_M0 + FOCUS_M1;
            break;

        case Step_1_16:             // M0=Hi-Z, M1=1

            P1DIR &= ~FOCUS_M0;
            P1DIR |= FOCUS_M1;
            P1OUT |= FOCUS_M1;
            break;

        case Step_1_32:             // M0=0, M1=Hi-Z
            P1DIR |= FOCUS_M0;
            P1DIR &= ~FOCUS_M1;
            P1OUT &= ~FOCUS_M0;
            break;

        case Step_1_128:             // M0=Hi-Z, M1=Hi-Z
            P1DIR &= ~(FOCUS_M0 + FOCUS_M1);
            break;

        case Step_1_256:            // M0=1, M1=Hi-Z
            P1DIR |= FOCUS_M0;
            P1DIR &= ~FOCUS_M1;
            P1OUT |= FOCUS_M0;
            break;

        default:
            break;
    }
}




void Init_Tilt_Motor(void){
    unsigned int PWM_Period = 0;

    //Disable Motor driver for configuration
    TILT_DISABLE;
    P4DIR |= TILT_EN;
//    P4DIR &= ~TILT_nFAULT;                    //Not supported in demo

    //Config Sleep pin -- Sleep not supported in Demo
    P4OUT |= TILT_nSLEEP;
    P4DIR |= TILT_nSLEEP;

    //Config Direction pin
    if(TiltMotorDirection == Clockwise){
        P4OUT |= TILT_DIR;
    }else{
        P4OUT &= ~TILT_DIR;
    }
    P4DIR |= TILT_DIR;

    //Photo Interupt
    P3DIR &= ~PI_EN;
    P3REN |= PI_EN;      // internal pull up resistor
    P3IES |= PI_EN;                          // P1.1 Hi/Low edge
    P3IE  |= PI_EN;                          // P1.1 interrupt enabled
    P3IFG &= ~PI_EN;                         // P1.1 IFG cleared


    //Config GPIO and Timer for Step PWM  - GPIO manually toggled in timer ISR
    TILT_STEP_PIN_LOW;
    P1DIR |= TILT_STEP;

    PWM_Period = (SMCLK_FREQ /8 /TILT_MOTOR_SPEED_PPS) -1;                         // PWM Period
    TB2CCR0 = PWM_Period;
    TB2CCR1 = PWM_Period/2;                             // CCR1 PWM duty cycle
    TB2CCTL1 = CCIE;                                    //  CCR1 ISR enabled
    TB2CTL = TBSSEL__SMCLK | ID__8 | TBCLR | TBIE;      // SMCLK, clock divide by 8 (24MHz/8= 3MHz), up mode, clear TBR

    //Configure M0 and M1 GPIO for correct Step Size
    switch(TiltMotorStepSetting){
        case Step_Full:             // M0=0, M1=0
            P2DIR |= TILT_M0 + TILT_M1;
            P2OUT &= ~(TILT_M0 + TILT_M1);
            break;

        case Step_1_2_Noncircular:  // M0=1, M1=0
            P2DIR |= TILT_M0 + TILT_M1;
            P2OUT |= TILT_M0;
            P2OUT &= ~(TILT_M1);
            break;

        case Step_1_2:              // M0=Hi-Z, M1=0
            P2DIR &= ~TILT_M0;
            P2DIR |= TILT_M1;
            P2OUT &= ~TILT_M1;
            break;

        case Step_1_4:              // M0=0, M1=1
            P2DIR |= TILT_M0 + TILT_M1;
            P2OUT &= ~(TILT_M0);
            P2OUT |= TILT_M1;
            break;

        case Step_1_8:             // M0=1, M1=1
            P2DIR |= TILT_M0 + TILT_M1;
            P2OUT |= TILT_M0 + TILT_M1;
            break;

        case Step_1_16:             // M0=Hi-Z, M1=1
            P2DIR &= ~TILT_M0;
            P2DIR |= TILT_M1;
            P2OUT |= TILT_M1;
            break;

        case Step_1_32:             // M0=0, M1=Hi-Z
            P2OUT &= ~TILT_M0;
            P2DIR |= TILT_M0;
            P2DIR &= ~TILT_M1;
            break;

        case Step_1_128:             // M0=Hi-Z, M1=Hi-Z
            P2DIR &= ~(TILT_M0 + TILT_M1);
            break;

        case Step_1_256:            // M0=1, M1=Hi-Z
            P2DIR |= TILT_M0;
            P2DIR &= ~TILT_M1;
            P2OUT |= TILT_M0;
            break;

        default:
            break;
    }
}














