Using accelerometer to calculate position with TM4C123BH6PM

#include <stdbool.h>
#include <stdint.h>

#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"

#include "driverlib/systick.h"
#include "driverlib/fpu.h"
#include "driverlib/gpio.h"
#include "driverlib/pin_map.h"
#include "driverlib/ssi.h"
#include "driverlib/sysctl.h"
#include "driverlib/uart.h"
#include "driverlib/pwm.h"
#include "driverlib/adc.h"
#include "driverlib/timer.h"

#include "utils/uartstdio.h"

#define ACC_WHO_AM_I       0x0F
#define ACC_CTRL_REG1       0X20
#define ACC_CTRL_REG2       0X21
#define ACC_CTRL_REG3       0X22
#define ACC_CTRL_REG4       0X23
#define ACC_CTRL_REG5       0X24
#define ACC_HP_FILTER_RESET 0X25
#define ACC_REFERENCE       0X26
#define ACC_STATUS_REG       0X27
#define ACC_OUT_X_L       0X28
#define ACC_OUT_X_H       0X29
#define ACC_OUT_Y_L       0X2A
#define ACC_OUT_Y_H       0X2B
#define ACC_OUT_Z_L       0X2C
#define ACC_OUT_Z_H       0X2D
#define ACC_INT1_CFG          0X30
#define ACC_INT1_SRC          0X31
#define ACC_INT1_THS          0X31
#define ACC_INT1_DURATION       0X31
#define ACC_INT2_CFG          0X31
#define ACC_INT2_SRC          0X31
#define ACC_INT2_THS          0X31
#define ACC_INT2_DURATION       0X31

#define XYRO_WHO_AM_I       0x0F
#define XYRO_CTRL_REG1       0X20
#define XYRO_CTRL_REG2       0X21
#define XYRO_CTRL_REG3       0X22
#define XYRO_CTRL_REG4       0X23
#define XYRO_CTRL_REG5       0X24
#define XYRO_REFERENCE       0X25
#define OUT_TEMP       		0X26
#define STATUS_REG			0x27
#define XYRO_OUT_X_L       	0X28
#define XYRO_OUT_X_H       	0X29
#define XYRO_OUT_Y_L       	0X2A
#define XYRO_OUT_Y_H       	0X2B
#define XYRO_OUT_Z_L       	0X2C
#define XYRO_OUT_Z_H       	0X2D
#define FIFO_CTRL_REG       0X30

#define NUM_SSI_DATA 6
#define N_AVG 100
#define D_SIZE 100

float Vx=0, Vy=0, Vz=0;
float Vx_prev=0, Vy_prev=0, Vz_prev=0;

float X=0, Y=0, Z=0;
//volatile float X_prev=0, Y_prev=0, Z_prev=0;

float dt = 0;
float freq = 0;

float Ax=0, Ay=0, Az=0;
float Ax_prev=0, Ay_prev=0, Az_prev=0;

volatile float DataX[D_SIZE] ={0,};
volatile float DataY[D_SIZE] ={0,};
volatile float DataZ[D_SIZE] ={0,};

volatile float Rx=0, Ry=0, Rz=0;
volatile float DataRX[D_SIZE] ={0,};
volatile float DataRY[D_SIZE] ={0,};
volatile float DataRZ[D_SIZE] ={0,};

volatile int16_t ax=0,ay=0,az=0;
float ix=0,iy=0,iz=0;

volatile uint32_t del_t=0;

uint32_t pui32DataTx[NUM_SSI_DATA];
uint32_t pui32DataRx[NUM_SSI_DATA];

uint32_t pui32ADC0Value[1];

void InitTimerA0(){
  
  SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);

  TimerConfigure(TIMER0_BASE, TIMER_CFG_PERIODIC_UP);

  // Full-width periodic up-count timer

  TimerLoadSet(TIMER0_BASE, TIMER_A, SysCtlClockGet());

  TimerEnable(TIMER0_BASE, TIMER_A);
}

void InitSSI1(void){
	SysCtlPeripheralEnable(SYSCTL_PERIPH_SSI1);

	    //
	    // For this example SSI0 is used with PortF[3:0].  The actual port and pins
	    // used may be different on your part, consult the data sheet for more
	    // information.  GPIO port A needs to be enabled so these pins can be used.
	    // TODO: change this to whichever GPIO port you are using.
	    //
	    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
		SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);

	    HWREG(GPIO_PORTF_BASE+GPIO_O_LOCK) = GPIO_LOCK_KEY;
	    //Unlock the Port to write to commit register

	    HWREG(GPIO_PORTF_BASE+GPIO_O_CR) = 0xFF;
	    // Set the pins of the commit register so that AFSEL and other registers can be modified
	    //
	    //
	    // This step is not necessary if your part does not support pin muxing.
	    // TODO: change this to select the port/pin you are using.
	    //
	    GPIOPinConfigure(GPIO_PF2_SSI1CLK);
	    GPIOPinConfigure(GPIO_PF0_SSI1RX);
	    GPIOPinConfigure(GPIO_PF1_SSI1TX);

	    HWREG(GPIO_PORTF_BASE+GPIO_O_LOCK) = ~(GPIO_LOCK_KEY);

	    //
	    // Configure the GPIO settings for the SSI pins.  This function also gives
	    // control of these pins to the SSI hardware.  Consult the data sheet to
	    // see which functions are allocated per pin.
	    // The pins are assigned as follows:
	    //      PF0 - SSI1Rx
	    //		PF1 - SSI1Tx
	    //     	PF2 - SSI1CLK
	    //		PF3 - SSI1Fss
	    //
	    //
	    // TODO: change this to select the port/pin you are using.
	    //
	    GPIOPinTypeSSI(GPIO_PORTF_BASE,GPIO_PIN_2 | GPIO_PIN_1 | GPIO_PIN_0);

		GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_3);//ACC FSS pull-up
		GPIOPinWrite(GPIO_PORTF_BASE,GPIO_PIN_3,GPIO_PIN_3);
		GPIOPinTypeGPIOOutput(GPIO_PORTE_BASE, GPIO_PIN_5);//Gyro FSS pull-up
		GPIOPinWrite(GPIO_PORTE_BASE,GPIO_PIN_5,GPIO_PIN_5);

	    //
	    // Configure and enable the SSI port for SPI master mode.  Use SSI0,
	    // system clock supply, idle clock level low and active low clock in
	    // freescale SPI mode, master mode, 1MHz SSI frequency, and 8-bit data.
	    // For SPI mode, you can set the polarity of the SSI clock when the SSI
	    // unit is idle.  You can also configure what clock edge you want to
	    // capture data on.  Please reference the datasheet for more information on
	    // the different SPI modes.
	    //
	    SSIConfigSetExpClk(SSI1_BASE, SysCtlClockGet(), SSI_FRF_MOTO_MODE_3,
	                       SSI_MODE_MASTER, 5000000, 16);

	    //
	    // Enable the SSI0 module.
	    //
	    SSIEnable(SSI1_BASE);
}

void InitConsole(void)
{
    //
    // Enable GPIO port A which is used for UART0 pins.
    // TODO: change this to whichever GPIO port you are using.
    //
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);

    //
    // Configure the pin muxing for UART0 functions on port A0 and A1.
    // This step is not necessary if your part does not support pin muxing.
    // TODO: change this to select the port/pin you are using.
    //
    GPIOPinConfigure(GPIO_PA0_U0RX);
    GPIOPinConfigure(GPIO_PA1_U0TX);

    //
    // Enable UART0 so that we can configure the clock.
    //
    SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);

    //
    // Use the internal 16MHz oscillator as the UART clock source.
    //
    UARTClockSourceSet(UART0_BASE, UART_CLOCK_PIOSC);

    //
    // Select the alternate (UART) function for these pins.
    // TODO: change this to select the port/pin you are using.
    //
    GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);

    //
    // Initialize the UART for console I/O.
    //
    UARTStdioConfig(0, 115200, 16000000);
}

void InitPWM(){
	SysCtlPWMClockSet(SYSCTL_PWMDIV_1);

	SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM0);
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);

	// DC Motor
	GPIOPinConfigure(GPIO_PB6_M0PWM0);
	GPIOPinConfigure(GPIO_PB7_M0PWM1);

	//Rotation Motor
	GPIOPinConfigure(GPIO_PB4_M0PWM2); //Right
	GPIOPinConfigure(GPIO_PB5_M0PWM3); //Left

	GPIOPinTypeGPIOOutput(GPIO_PORTB_BASE, GPIO_PIN_6);
	GPIOPinTypeGPIOOutput(GPIO_PORTB_BASE, GPIO_PIN_7);

	GPIOPinTypeGPIOOutput(GPIO_PORTB_BASE, GPIO_PIN_4);
	GPIOPinTypeGPIOOutput(GPIO_PORTB_BASE, GPIO_PIN_5);
	//GPIOPinWrite(GPIO_PORTB_BASE,GPIO_PIN_6,0);
	//GPIOPinWrite(GPIO_PORTB_BASE,GPIO_PIN_7,0);

	GPIOPinTypePWM(GPIO_PORTB_BASE, (GPIO_PIN_4 | GPIO_PIN_5 | GPIO_PIN_6 | GPIO_PIN_7));

	PWMGenConfigure(PWM0_BASE, PWM_GEN_0, PWM_GEN_MODE_UP_DOWN |
	                        PWM_GEN_MODE_NO_SYNC);

	PWMGenConfigure(PWM0_BASE, PWM_GEN_1, PWM_GEN_MODE_UP_DOWN |
		                        PWM_GEN_MODE_NO_SYNC);

	    // use the following equation: N = (1 / f) * SysClk.  Where N is the
	    // Set the PWM period to 250Hz.  To calculate the appropriate parameter
	    // function parameter, f is the desired frequency, and SysClk is the
	    // system clock frequency.
	    // In this case you get: (1 / 250Hz) * 16MHz = 64000 cycles.  Note that
	    // the maximum period you can set is 2^16.
	    // TODO: modify this calculation to use the clock frequency that you are
	    // using.
	    //
	//DC Motor Freq Setting
	PWMGenPeriodSet(PWM0_BASE, PWM_GEN_0, 15000000);

	//Rotation Motor Freq Setting
	PWMGenPeriodSet(PWM0_BASE, PWM_GEN_1, 10000);

	    // Set PWM0 to a duty cycle of 25%.  You set the duty cycle as a function
	    // of the period.  Since the period was set above, you can use the
	    // PWMGenPeriodGet() function.  For this example the PWM will be high for
	    // 25% of the time or 16000 clock ticks (64000 / 4).
	    //

	// DC Motor duty setting
	PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0,
	                         PWMGenPeriodGet(PWM0_BASE, PWM_OUT_0) / 5);


	PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1,
	                             PWMGenPeriodGet(PWM0_BASE, PWM_OUT_0) / 100000);



	// Rotation Motor duty setting
	PWMPulseWidthSet(PWM0_BASE, PWM_OUT_2,
			10000 / 2);

	PWMPulseWidthSet(PWM0_BASE, PWM_OUT_3,
			10000 / 5000);


	PWMOutputState(PWM0_BASE, PWM_OUT_0_BIT, true);
	PWMOutputState(PWM0_BASE, PWM_OUT_1_BIT, true);

	PWMOutputState(PWM0_BASE, PWM_OUT_2_BIT, true);
	PWMOutputState(PWM0_BASE, PWM_OUT_3_BIT, true);

	PWMGenEnable(PWM0_BASE, PWM_GEN_0);
	PWMGenEnable(PWM0_BASE, PWM_GEN_1);

	UARTprintf("PWM ->\n");
	UARTprintf("  Module: PWM0\n");
	UARTprintf("  Pin: PB6, PB7(DC Motor) , PB4, PB5(Rotation Motor)\n");
	   // Display the setup on the console.
	    //
   UARTprintf("SSI1 ->\n");
   UARTprintf("  Mode: SPI\n");
   UARTprintf("  Data: 16-bit\n\n");

	    //
    // The SSI1 peripheral must be enabled for use.
    //
}

void InitACCEL(){
	   uint32_t ui32Index;

	   while(SSIDataGetNonBlocking(SSI1_BASE, &pui32DataRx[0]));

	       //
	       // Initialize the data to send.
	       //
	       pui32DataTx[0] = ((ACC_CTRL_REG1<<8)|0x3F);//write control reg1
	       pui32DataTx[1] = ((ACC_CTRL_REG2<<8)|0x33);//write control reg2
	       //pui32DataTx[2] = ((ACC_CTRL_REG4<<8)|0x80);//write control reg4, 2g
	       pui32DataTx[2] = ((ACC_CTRL_REG4<<8)|0xB0);//write control reg4, 8g

	       pui32DataTx[3] = ((ACC_REFERENCE<<8)|0x00);

	       pui32DataTx[4] = (ACC_WHO_AM_I|0x80)<<8;//read status reg
	       pui32DataTx[5] = (ACC_CTRL_REG1|0x80)<<8;//read control reg1

	       //
	       // Display indication that the SSI is transmitting data.
	       //
	       UARTprintf("Sent:\n  ");

	       //
	       // Send 3 bytes of data.
	       //
	       for(ui32Index = 0; ui32Index < NUM_SSI_DATA; ui32Index++)
	       {
	           //
	           // Display the data that SSI is transferring.
	           //
	           UARTprintf("%02x ", pui32DataTx[ui32Index]);

	           //
	           // Send the data using the "blocking" put function.  This function
	           // will wait until there is room in the send FIFO before returning.
	           // This allows you to assure that all the data you send makes it into
	           // the send FIFO.
	           //
			  GPIOPinWrite(GPIO_PORTF_BASE,GPIO_PIN_3,0);
			  SSIDataPut(SSI1_BASE, pui32DataTx[ui32Index]);

			  SysCtlDelay(100);
			  GPIOPinWrite(GPIO_PORTF_BASE,GPIO_PIN_3,GPIO_PIN_3);
	       }

	       //
	       // Wait until SSI0 is done transferring all the data in the transmit FIFO.
	       //
	       while(SSIBusy(SSI1_BASE))
	       {
	       }

	       //
	       // Display indication that the SSI is receiving data.
	       //
	       UARTprintf("\nReceived:\n  ");

	       //
	       // Receive 3 bytes of data.
	       //
	       for(ui32Index = 0; ui32Index < NUM_SSI_DATA; ui32Index++)
	       {
	           //
	           // Receive the data using the "blocking" Get function. This function
	           // will wait until there is data in the receive FIFO before returning.
	           //
	           SSIDataGet(SSI1_BASE, &pui32DataRx[ui32Index]);

	           //
	           // 8-bit data, mask off the MSB.
	           //
	           pui32DataRx[ui32Index] &= 0x00FF;

	           //
	           // Display the data that SSI1 received.
	           //
	           UARTprintf("%02x ", pui32DataRx[ui32Index]);
	       }

	       UARTprintf("\n\n\n\n");
}

void GetACCEL(){

	   uint32_t ui32Index;
	   uint16_t i=0;
	   uint16_t j=0;

	   for(i=0;i<D_SIZE;i++){

	   for(j=0;j<N_AVG;j++){
		 
		 if(i==0 && j ==0){
		   del_t = TimerValueGet(TIMER0_BASE,TIMER_A);
		 }

	       while(SSIDataGetNonBlocking(SSI1_BASE, &pui32DataRx[0]));

	       //
	       // Initialize the data to send.
	       //

	       pui32DataTx[0] = (ACC_OUT_X_H|0x80)<<8;//read X high
	       pui32DataTx[1] = (ACC_OUT_X_L|0x80)<<8;//read X Low

	       pui32DataTx[2] = (ACC_OUT_Y_H|0x80)<<8;//read Y high
	       pui32DataTx[3] = (ACC_OUT_Y_L|0x80)<<8;//read Y Low

	       pui32DataTx[4] = (ACC_OUT_Z_H|0x80)<<8;//read Z high
	       pui32DataTx[5] = (ACC_OUT_Z_L|0x80)<<8;//read Z Low
	       //
	       // Display indication that the SSI is transmitting data.
	       //

	       //UARTprintf("Sent: ");

	       //
	       //
	       for(ui32Index = 0; ui32Index < NUM_SSI_DATA; ui32Index++)
	       {
	           //
	           // Display the data that SSI is transferring.
	           //
	           //UARTprintf("%02x ", pui32DataTx[ui32Index]);


	           //
	           // Send the data using the "blocking" put function.  This function
	           // will wait until there is room in the send FIFO before returning.
	           // This allows you to assure that all the data you send makes it into
	           // the send FIFO.
	           //
	          GPIOPinWrite(GPIO_PORTF_BASE,GPIO_PIN_3,0);
			  SSIDataPut(SSI1_BASE, pui32DataTx[ui32Index]);

			  SysCtlDelay(40);
			  GPIOPinWrite(GPIO_PORTF_BASE,GPIO_PIN_3,GPIO_PIN_3);
	       }

	       //
	       // Wait until SSI1 is done transferring all the data in the transmit FIFO.
	       //
	       while(SSIBusy(SSI1_BASE))
	       {
	       }

	       //
	       // Display indication that the SSI is receiving data.
	       //
	       //UARTprintf(" : ");

	       for(ui32Index = 0; ui32Index < NUM_SSI_DATA; ui32Index++)
	       {
	           //
	           // Receive the data using the "blocking" Get function. This function
	           // will wait until there is data in the receive FIFO before returning.
	           //
	           SSIDataGet(SSI1_BASE, &pui32DataRx[ui32Index]);

	           //
	           // Since we are using 8-bit data, mask off the MSB.
	           //
	           pui32DataRx[ui32Index] &= 0x00FF;

	           //
	           // Display the data that SSI0 received.
	           //
	           //UARTprintf("%02x ", pui32DataRx[ui32Index]);
	           SysCtlDelay(35);
	       }

	       ax = (int16_t) ((((pui32DataRx[0]<<8) | pui32DataRx[1]))) >>4;
	       ay = (int16_t) ((((pui32DataRx[2]<<8) | pui32DataRx[3]))) >>4;
	       az = (int16_t) ((((pui32DataRx[4]<<8) | pui32DataRx[5]))) >>4;
	       //12bit representation

	       ix = (float)((ax)*(3.9 * 9.8));
	       iy = (float)((ay)*(3.9 * 9.8));
	       iz = (float)((az)*(3.9 * 9.8));

	       //lx =;

	       // * 49 * 9.8

	       //UARTprintf(" / %4d %4d %4d ", ax,ay,az);
	       //UARTprintf(" / %6d %6d %6d ", ix,iy,iz);
	       //UARTprintf("\n");
	       //UARTprintf("\r");

	       Ax += (float)(ix/1000);
	       Ay += (float)(iy/1000);
	       Az += (float)(iz/1000);

	       }

	       Ax = Ax/N_AVG;
	       Ay = Ay/N_AVG;
	       Az = Az/N_AVG;
		   
		    if(i==0){
			  
			  del_t  = TimerValueGet(TIMER0_BASE,TIMER_A) - del_t;
			  dt = (float)( del_t / freq );			  
			
			}

	       //if(i>=0 && i<D_SIZE){
	          //DataX[i] = Ax;
	          //DataY[i] = Ay;
	          //DataZ[i] = Az;
	       //}
			
			//////integration
			
			Vx = Vx_prev + 0.5*(Ax + Ax_prev) * dt ;
			Vy = Vy_prev + 0.5*(Ay + Ay_prev) * dt ;
			Vz = Vz_prev + 0.5*(Az + Az_prev) * dt ;
			
			Ax_prev = Ax;
			Ay_prev = Ay;
			Az_prev = Az;
			
			X = X + 0.5*(Vx + Vx_prev) * dt ;
			Y = Y + 0.5*(Vy + Vy_prev) * dt ;
			Z = Z + 0.5*(Vz + Vz_prev) * dt ;
			
			Vx_prev = Vx;
			Vy_prev = Vy;
			Vz_prev = Vz;
			
	       UARTprintf("ax ay az z : ");
	       UARTprintf("%2d %2d %2d %d \n",(int)Ax,(int)Ay,(int)Az,(int)Z);
	       //SysCtlDelay(10000000);
	   }

	   //UARTprintf("ax ay az  : ");
	   //UARTprintf("%2d %2d %2d \r",(int)Ax,(int)Ay,(int)Az);
}

void InitGYRO(){

	uint32_t ui32Index;

	while(SSIDataGetNonBlocking(SSI1_BASE, &pui32DataRx[0]));

	    //
	    // Initialize the data to send.
	    //
	    pui32DataTx[0] = ((XYRO_CTRL_REG1<<8)|0x0F);//write control reg1
	    pui32DataTx[1] = ((XYRO_CTRL_REG2<<8)|0x20);//write control reg2
	    pui32DataTx[2] = ((XYRO_CTRL_REG4<<8)|0x80);//write control reg4

	    pui32DataTx[3] = ((XYRO_REFERENCE<<8)|0x00);
		pui32DataTx[4] = ((XYRO_CTRL_REG5<<8)|0x01);//read control reg1
	    pui32DataTx[5] = (XYRO_WHO_AM_I|0x80)<<8;//read status reg

	    //pui32DataTx[5] = (ACC_CTRL_REG2|0x80)<<8;//read control reg2

	    //
	    // Display indication that the SSI is transmitting data.
	    //
	    UARTprintf("Sent:\n  ");

	    //
	    // Send 3 bytes of data.
	    //
	    for(ui32Index = 0; ui32Index < NUM_SSI_DATA; ui32Index++)
	    {
	        //
	        // Display the data that SSI is transferring.
	        //
	        UARTprintf("%02x ", pui32DataTx[ui32Index]);

	        //
	        // Send the data using the "blocking" put function.  This function
	        // will wait until there is room in the send FIFO before returning.
	        // This allows you to assure that all the data you send makes it into
	        // the send FIFO.
	        //
	        GPIOPinWrite(GPIO_PORTE_BASE,GPIO_PIN_5,0);
        	SSIDataPut(SSI1_BASE, pui32DataTx[ui32Index]);

			SysCtlDelay(100);
			GPIOPinWrite(GPIO_PORTE_BASE,GPIO_PIN_5,GPIO_PIN_5);
	    }

	    //
	    // Wait until SSI0 is done transferring all the data in the transmit FIFO.
	    //
	    while(SSIBusy(SSI1_BASE))
	    {
	    }

	    //
	    // Display indication that the SSI is receiving data.
	    //
	    UARTprintf("\nReceived:\n  ");

	    //
	    // Receive 3 bytes of data.
	    //
	    for(ui32Index = 0; ui32Index < NUM_SSI_DATA; ui32Index++)
	    {
	        //
	        // Receive the data using the "blocking" Get function. This function
	        // will wait until there is data in the receive FIFO before returning.
	        //
	        SSIDataGet(SSI1_BASE, &pui32DataRx[ui32Index]);

	        //
	        // 8-bit data, mask off the MSB.
	        //
	        pui32DataRx[ui32Index] &= 0x00FF;

	        //
	        // Display the data that SSI1 received.
	        //
	        UARTprintf("%02x ", pui32DataRx[ui32Index]);
	    }

	    UARTprintf("\n\n\n\n");
}

void GetGyro(){

  uint32_t ui32Index;
  uint16_t i=0;
  uint16_t j=0;

  SysCtlDelay(50);

  for(i=0;i<D_SIZE;i++){

    for(j=0;j<N_AVG;j++){

    while(SSIDataGetNonBlocking(SSI1_BASE, &pui32DataRx[0]));

    //
    // Initialize the data to send.
    //

    pui32DataTx[0] = (XYRO_OUT_X_H|0x80)<<8;//read X high
    pui32DataTx[1] = (XYRO_OUT_X_L|0x80)<<8;//read X Low

    pui32DataTx[2] = (XYRO_OUT_Y_H|0x80)<<8;//read Y high
    pui32DataTx[3] = (XYRO_OUT_Y_L|0x80)<<8;//read Y Low

    pui32DataTx[4] = (XYRO_OUT_Z_H|0x80)<<8;//read Z high
    pui32DataTx[5] = (XYRO_OUT_Z_L|0x80)<<8;//read Z Low
    //
    // Display indication that the SSI is transmitting data.
    //

    //UARTprintf("Sent: ");

    //
    //
    for(ui32Index = 0; ui32Index < NUM_SSI_DATA; ui32Index++)
    {
        //
        // Display the data that SSI is transferring.
        //
        //UARTprintf("%02x ", pui32DataTx[ui32Index]);
    	//SysCtlDelay(50);

        //
        // Send the data using the "blocking" put function.  This function
        // will wait until there is room in the send FIFO before returning.
        // This allows you to assure that all the data you send makes it into
        // the send FIFO.
        //
		GPIOPinWrite(GPIO_PORTE_BASE,GPIO_PIN_5,0);
        SSIDataPut(SSI1_BASE, pui32DataTx[ui32Index]);

		SysCtlDelay(100);
		GPIOPinWrite(GPIO_PORTE_BASE,GPIO_PIN_5,GPIO_PIN_5);
    }

    //
    // Wait until SSI1 is done transferring all the data in the transmit FIFO.
    //
    while(SSIBusy(SSI1_BASE))
    {
    }

    //
    // Display indication that the SSI is receiving data.
    //
    //UARTprintf("\nRecieve : ");

    for(ui32Index = 0; ui32Index < NUM_SSI_DATA; ui32Index++)
    {
        //
        // Receive the data using the "blocking" Get function. This function
        // will wait until there is data in the receive FIFO before returning.
        //
        SSIDataGet(SSI1_BASE, &pui32DataRx[ui32Index]);

        //
        // Since we are using 8-bit data, mask off the MSB.
        //
        pui32DataRx[ui32Index] &= 0x00FF;

        //
        // Display the data that SSI0 received.
        //
        //UARTprintf("%02x ", pui32DataRx[ui32Index]);
        //SysCtlDelay(50);
    }

    ax = (int16_t) ((((pui32DataRx[0]<<8) | pui32DataRx[1])));
    ay = (int16_t) ((((pui32DataRx[2]<<8) | pui32DataRx[3])));
    az = (int16_t) ((((pui32DataRx[4]<<8) | pui32DataRx[5])));
    //12bit representation

    ix = (float)((ax)*(8.75));
    iy = (float)((ay)*(8.75));
    iz = (float)((az)*(8.75));

    //lx =;

    // * 49 * 9.8

    //UARTprintf(" / %4d %4d %4d ", ax,ay,az);
    //UARTprintf(" / %6d %6d %6d ", ix,iy,iz);
    //UARTprintf("\n");
    //UARTprintf("\r");

    Rx += (float)(ix/1000);
    Ry += (float)(iy/1000);
    Rz += (float)(iz/1000);

    }

    Rx = Rx/N_AVG;
    Ry = Ry/N_AVG;
    Rz = Rz/N_AVG;

	DataRX[i] = Rx;
	DataRY[i] = Ry;
	DataRZ[i] = Rz;

    UARTprintf("Rx Ry Rz  : ");
    UARTprintf("%3d %3d %3d\n",(int)Rx,(int)Ry,(int)Rz);
    //SysCtlDelay(10000000);
  }

  UARTprintf("///////////////////////\n\n");
}

void InitADC(){

    SysCtlPeripheralEnable(SYSCTL_PERIPH_ADC0);
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);
    GPIOPinTypeADC(GPIO_PORTE_BASE, GPIO_PIN_3);

    // Enable sample sequence 3 with a processor signal trigger.  Sequence 3
        // will do a single sample when the processor sends a signal to start the
        // conversion.  Each ADC module has 4 programmable sequences, sequence 0
        // to sequence 3.  This example is arbitrarily using sequence 3.
        //
    ADCSequenceConfigure(ADC0_BASE, 3, ADC_TRIGGER_PROCESSOR, 0);

        //
        // Configure step 0 on sequence 3.  Sample channel 0 (ADC_CTL_CH0) in
        // single-ended mode (default) and configure the interrupt flag
        // (ADC_CTL_IE) to be set when the sample is done.  Tell the ADC logic
        // that this is the last conversion on sequence 3 (ADC_CTL_END).  Sequence
        // 3 has only one programmable step.  Sequence 1 and 2 have 4 steps, and
        // sequence 0 has 8 programmable steps.  Since we are only doing a single
        // conversion using sequence 3 we will only configure step 0.  For more
        // information on the ADC sequences and steps, reference the datasheet.
        //
    ADCSequenceStepConfigure(ADC0_BASE, 3, 0, ADC_CTL_CH0 | ADC_CTL_IE |
                                 ADC_CTL_END);

        //
        // Since sample sequence 3 is now configured, it must be enabled.
        //
    ADCSequenceEnable(ADC0_BASE, 3);
    ADCIntClear(ADC0_BASE, 3);

	UARTprintf("ADC ->\n");
	UARTprintf("  Type: Single Ended\n");
	UARTprintf("  Samples: One\n");
	UARTprintf("  Update Rate: 250ms\n");
	UARTprintf("  Input Pin: AIN0/PE3\n\n");
}

void GetADC(){

	ADCProcessorTrigger(ADC0_BASE, 3);

	// Wait for conversion to be completed.
	while(!ADCIntStatus(ADC0_BASE, 3, false)){}

	// Clear the ADC interrupt flag.
	ADCIntClear(ADC0_BASE, 3);

	// Read ADC Value.
	ADCSequenceDataGet(ADC0_BASE, 3, pui32ADC0Value);

	UARTprintf("AIN0 = %4d\r", pui32ADC0Value[0]);
}

int main(void)
{

    // Set the clocking to run directly from the external crystal/oscillator.
    // TODO: The SYSCTL_XTAL_ value must be changed to match the value of the
    // crystal on your board.
    //
    SysCtlClockSet(SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_OSC_MAIN |
                   SYSCTL_XTAL_16MHZ);

    FPUEnable();
    FPULazyStackingEnable();

    InitSSI1();

    InitConsole();

    InitACCEL();

    InitGYRO();
	
	InitTimerA0();
	
	freq = SysCtlClockGet();

    InitPWM();
	
	//UARTprintf("\n\n\n\n\ Start \n\n\n\n");

    while(1){

    	GetACCEL();
    	//GetADC();

    	//GetGyro();

    }

}