Servo motor control using msp 430 launchpad

#include <stdint.h>
#include <stdio.h>
#include "msp430G2231.h"

#define SERVO_0 650
#define SERVO_180 2350
#define SERVO_COUNT 2
#define SETSERVO(servo, pos) periods[servo].cycle = servopos(pos);

typedef struct period_t_ {
    uint16_t cycle;
    uint8_t pins;
} period_t;

period_t periods[1 + SERVO_COUNT];
uint8_t period = 0;

void mysleep(uint16_t i);
uint16_t servopos(uint8_t pos);
void initperiod(uint8_t period, uint16_t cycle, uint8_t pins);

unsigned int ShaftAngle;
unsigned int AngleChange;
unsigned int OutputPin;
unsigned int Delay;
unsigned int MaxSteps;
unsigned int StepCounter;

int main(void)
{
    
  WDTCTL = WDTPW + WDTHOLD; /* Stop watchdog timer */
    
    initperiod(0, 20000, 0x00);
    initperiod(1, servopos(90), BIT4);
    
    P1OUT = 0x00;
    P1DIR |= BIT4;                        // P1.4 
    CCTL0 = CCIE;
    CCR0 = 65535;                        // Initial clock interupt
    TACTL = MC_1 + TASSEL_2 + TACLR;    // Set clock / timing source

    _BIS_SR(GIE);                        // Activate interupts
    
    ShaftAngle    = 0;                    // Whats the current angle of the shaft
    OutputPin    = 1;                    // What pin on the IC to raise high
    Delay         = 50;                    // How long to wait between each step change
    MaxSteps     = 10;                    // How many steps 
    AngleChange    = 10;                    // Degrees to change shaft position each iteration
    SETSERVO(OutputPin, ShaftAngle);    // reset to zero

    while (1) // Endless Looop
    {
                        // Just wait a moment so we can physically check the servo position (only for testing)
        mysleep(5);
        for (StepCounter=0; StepCounter <= MaxSteps; StepCounter++) 
        {
            SETSERVO(OutputPin, ShaftAngle);
            ShaftAngle+=AngleChange;
            mysleep(5);
        }
             
        mysleep(5);
          
        
        // Just wait a moment so we can physically check the servo position (only for testing)
          
        for (StepCounter=0; StepCounter <= MaxSteps; StepCounter++) 
        {
            ShaftAngle-=AngleChange;
            SETSERVO(OutputPin, ShaftAngle);
            mysleep(5);                            // Slowly move back to zero position to reduce G force
        }
    }
}

#pragma vector = TIMERA0_VECTOR
__interrupt void Timer_A_0(void)
{
    P1OUT = periods[period].pins;
    CCR0 = periods[period].cycle;
    if (period ++ >= SERVO_COUNT) period = 0;
}

void mysleep(uint16_t i)
{
    uint16_t y, x;
    y = i;
    while (--y != 0) {
        for (x = 0; x < 2400; x ++);
    }
}

uint16_t servopos(uint8_t pos)
{
        return (uint16_t) ((SERVO_180 - SERVO_0) * (float) pos / 180) + SERVO_0;
}

void initperiod(uint8_t period, uint16_t cycle, uint8_t pins)
{
    periods[period].cycle = cycle;
    periods[period].pins = pins;
}