CCS/MSP430G2553: Interfacing MSP430G2553 with MPU6050 (accelerometer).

Tool/software: Code Composer Studio

Hello, I've interfaced the msp430 with the mpu6050, but my code stops running when it hits the line with the comment "PROBLEM LINE". If anyone has any ideas as to why the code doesnt continue when it hits that line, that would be very helpful.

#define USE_CLOCK
#define USE_I2C_USCI
#define USE_MPU6050
#define USE_UART

////////////////////////////////////////////////////////////////////////////////


#include <wchar.h>
#include "stdio.h"
//#include "string.h"
#include "Library/mymath.h"
#include <math.h>
#include "msp430g2553.h"
#include "Library/Global.h"
#include "Library/UART.h"
#include "Library/Clock.h"
#include "Library/I2C_USCI.h"
#include "Library/MPU6050.h"


int temp=0,i=0;
int abc=0;
char buff[50];

//Chu y Gyro bi sai so tinh khong dong nhat khi goc quay thay doi
// Khong tinh duoc vi phan cua gyro

void main()
{
	WDTCTL = WDTPW | WDTHOLD;
    Select_Clock(DCO_16MHZ);
	_delay_cycles(100000);

	// Khoi tao Ngat timer
	// Tao timer 0,5 s
	//TACCR0 = 10000;
	//TACCTL0 = CCIE;
	//TACTL = TASSEL_2 + MC_1 + ID_0;             // SMCLK/8, Up mode

	P1DIR|=BIT0;
	UART_Init();
	//UART_Write_Char(10);		//Ky tu xuong dong
	//UART_Write_String("Ket noi thanh cong");
	//UART_Write_Char(10);		//Ky tu xuong dong
	_delay_cycles(100000);
	Init_I2C_USCI(MPU6050_ADDRESS);
	Setup_MPU6050();
	//MPU6050_Test_I2C();
	//MPU6050_Check_Registers();
	//Calibrate_Gyros();
	_delay_cycles(320000);
	Get_Accel_Values();
	Get_Gyro_Values();
	ACCEL_XOUT = 0;
	ACCEL_YOUT = 0;
	ACCEL_ZOUT = 0;
	ACCEL_XOUT_PAST=0;			//
	ACCEL_YOUT_PAST=0;
	ACCEL_ZOUT_PAST=0;
	ACCEL_XA    =0;
	ACCEL_YA    =0;
	ACCEL_ZA    =0;
	ACCEL_XV    = 0;
	ACCEL_YV    = 0;
	ACCEL_ZV    = 0;
	ACCEL_XDECAC=0;
	ACCEL_YDECAC=0;
	ACCEL_ZDECAC=0;
	_delay_cycles(1000000);
	//__bis_SR_register(GIE);			//Cho phep ngat hoat dong
	while(1)
	{
		//Get_Gyro_Values();
		P1OUT^=BIT0;
		//sprintf(buff,"GX:%d GY:%d GZ:%d AX:%d AY:%d AZ:%d",GYRO_XOUT,GYRO_YOUT,GYRO_ZOUT,ACCEL_XOUT,ACCEL_YOUT,ACCEL_ZOUT);
		//sprintf(buff,"1%d 2%d 3%d 4%d 5%d 6%d ",GYRO_XOUT,GYRO_YOUT,GYRO_ZOUT,ACCEL_XOUT,ACCEL_YOUT,ACCEL_ZOUT);
		UART_Write_Char('1');
		sprintf(buff,"%d",ACCEL_XOUT);
		UART_Write_String(buff);
		UART_Write_Char(' ');
		//_delay_cycles(10000);
		UART_Write_Char('2');
		sprintf(buff,"%d",ACCEL_YOUT);
		UART_Write_String(buff);
		UART_Write_Char(' ');
		//_delay_cycles(10000);
		UART_Write_Char('3');
		sprintf(buff,"%d",ACCEL_ZOUT);
		UART_Write_String(buff);
		UART_Write_Char(' ');
		_delay_cycles(10000);
		Get_Accel_Values();
	}
}


// Timer A0 interrupt service routine
#pragma vector=TIMER0_A0_VECTOR
__interrupt void Timer_A0 (void)
{

}


#ifdef USE_I2C_USCI

#define MPU6050_ADDRESS	0x68
#define DS1307_ADDRESS	0x68
#define LM92_ADDRESS	0x48
/******************************************************************************\
*					Prototype (nguyen mau ham)    						       *
\******************************************************************************/

void Init_I2C_USCI(unsigned char addr);
void Set_Address(unsigned char addr);
unsigned char ByteRead_I2C_USCI(unsigned char address);
unsigned char ByteWrite_I2C_USCI(unsigned char address, unsigned char Data);
unsigned char WordRead_I2C_USCI(unsigned char Addr_Data,unsigned char *Data, unsigned char Length);
/******************************************************************************\
*					Function (noi dung ham)	                           *
\******************************************************************************/


void Init_I2C_USCI(unsigned char addr)
{
	P1SEL |= BIT6 + BIT7;                     // Assign I2C pins to USCI_B0
	P1SEL2|= BIT6 + BIT7;                     // Assign I2C pins to USCI_B0
	UCB0CTL1 |= UCSWRST;                      // Enable SW reset
	UCB0CTL0 = UCMST+UCMODE_3+UCSYNC;         // I2C Master, synchronous mode
  	UCB0CTL1 = UCSSEL_2+UCSWRST;              // Use SMCLK, keep SW reset
	UCB0BR0 = 40;                             // fSCL = SMCLK/40 = ~400kHz
	UCB0BR1 = 0;
	UCB0I2CSA = addr;                         // Set slave address
	UCB0CTL1 &= ~UCSWRST;                     // Clear SW reset, resume operation
}

void Set_Address(unsigned char addr)
{
  	UCB0CTL1 |= UCSWRST;    
  	UCB0I2CSA = addr;                     		// Set slave address
  	UCB0CTL1 &= ~UCSWRST;                 		// Clear SW reset, resume operation 	
}

unsigned char ByteRead_I2C_USCI(unsigned char address)
{ 	
	while (UCB0CTL1 & UCTXSTP);             	// Cho tin hieu I2C STT duoc gui di
	UCB0CTL1 |= UCTR + UCTXSTT;             	// I2C TX,START

	while (!(IFG2&UCB0TXIFG));			// PROBLEM LINE
	UCB0TXBUF = address;                      	// Dia chi luu gia tri Seconds

	while (!(IFG2&UCB0TXIFG));					// PROBLEM LINE

	UCB0CTL1 &= ~UCTR;                      // I2C RX
	UCB0CTL1 |= UCTXSTT;                    // I2C RESTART
	IFG2 &= ~UCB0TXIFG;                     // Xoa co ngat USCI_B0 TX

	while (UCB0CTL1 & UCTXSTT);             // Cho den khi I2C STT duoc gui di
	UCB0CTL1 |= UCTXSTP;                    // Gui bit STOP
	return UCB0RXBUF;
}


//doc 1 mang tu DS
unsigned char WordRead_I2C_USCI(unsigned char Addr_Data,unsigned char *Data, unsigned char Length)
{ 	
	unsigned char i=0;
	while (UCB0CTL1 & UCTXSTP);             // Loop until I2C STT is sent
	UCB0CTL1 |= UCTR + UCTXSTT;             // I2C TX, start condition

	while (!(IFG2&UCB0TXIFG));
	IFG2 &= ~UCB0TXIFG;                     // Clear USCI_B0 TX int flag
	if(UCB0STAT & UCNACKIFG) return UCB0STAT;	//Neu bao loi
	UCB0TXBUF = Addr_Data;                      	// Dia chi luu gia tri Seconds

	while (!(IFG2&UCB0TXIFG));
	if(UCB0STAT & UCNACKIFG) return UCB0STAT;	//Neu bao loi

	UCB0CTL1 &= ~UCTR;                      // I2C RX
	UCB0CTL1 |= UCTXSTT;                    // I2C start condition
	IFG2 &= ~UCB0TXIFG;                     // Clear USCI_B0 TX int flag
	while (UCB0CTL1 & UCTXSTT);             // Loop until I2C STT is sent
	for(i=0;i<(Length-1);i++)
	{
		while (!(IFG2&UCB0RXIFG));
		IFG2 &= ~UCB0TXIFG;                     // Clear USCI_B0 TX int flag
		Data[i] = UCB0RXBUF;
	}
	while (!(IFG2&UCB0RXIFG));
	IFG2 &= ~UCB0TXIFG;                     // Clear USCI_B0 TX int flag
	UCB0CTL1 |= UCTXSTP;                    // I2C stop condition after 1st TX
	Data[Length-1] = UCB0RXBUF;
	IFG2 &= ~UCB0TXIFG;                     // Clear USCI_B0 TX int flag
	return 0;
}

unsigned char ByteWrite_I2C_USCI(unsigned char address, unsigned char data)
{
	while (UCB0CTL1 & UCTXSTP);             // Cho den khi tin hieu STT duoc gui xong
	UCB0CTL1 |= UCTR + UCTXSTT;             // I2C TX, Gui bit START

	while (!(IFG2&UCB0TXIFG));				// Cho cho bit START gui xong
	if(UCB0STAT & UCNACKIFG) return UCB0STAT;	//Neu bao loi thì thoat khoi ham
	UCB0TXBUF = address;					// Gui dia chi thanh ghi can ghi


	while (!(IFG2&UCB0TXIFG));			// Cho gui xong
	if(UCB0STAT & UCNACKIFG) return UCB0STAT;	//Neu bao loi thì thoat khoi ham
	UCB0TXBUF = data;						// Gui du lieu

	while (!(IFG2&UCB0TXIFG));				// Cho gui xong
	if(UCB0STAT & UCNACKIFG) return UCB0STAT;	//Neu bao loi thì thoat khoi ham
	UCB0CTL1 |= UCTXSTP;                    // Gui bit STOP
	IFG2 &= ~UCB0TXIFG;                     // Xoa co USCI_B0 TX
	return 0;
}
unsigned int Read_LM92()
{
	unsigned char a;
	while (UCB0CTL1 & UCTXSTP);             // Loop until I2C STT is sent
	UCB0CTL1 |= UCTR + UCTXSTT;             // I2C TX, start condition

	while (!(IFG2&UCB0TXIFG));
	UCB0TXBUF = 0x00;                      	// Dia chi luu gia tri Seconds

	while (!(IFG2&UCB0TXIFG));

	UCB0CTL1 &= ~UCTR;                      // I2C RX
	UCB0CTL1 |= UCTXSTT;                    // I2C start condition
	IFG2 &= ~UCB0TXIFG;                     // Clear USCI_B0 TX int flag

	while (UCB0CTL1 & UCTXSTT);             // Loop until I2C STT is sent
	a = UCB0RXBUF;
	while (UCB0CTL1 & UCTXSTT);             // Loop until I2C STT is sent
	UCB0CTL1 |= UCTXSTP;                    // I2C stop condition after 1st TX
	return ((a<<8)|UCB0RXBUF);	
}

 
#endif