/////* ----------------------------------------------------------------------------
//// * Copyright (c) 2017 Semiconductor Components Industries, LLC (d/b/a
//// * ON Semiconductor), All Rights Reserved
//// *
//// * This code is the property of ON Semiconductor and may not be redistributed
//// * in any form without prior written permission from ON Semiconductor.
//// * The terms of use and warranty for this code are covered by contractual
//// * agreements between ON Semiconductor and the licensee.
//// *
//// * This is Reusable Code.
//// *
//// * ----------------------------------------------------------------------------
//// * app.c
//// * - Simple application using a DIO5 input to write and read data throught I2C
//// * - DIO6 is configured as output and connected to a LED
//// * - During communication DIO6 is ON
//// * - If data read from the slave is equal to the written data to the slave the
//// *   LED blinks at the end of transaction
//// * ----------------------------------------------------------------------------
//// * $Revision: 1.11 $
//// * $Date: 2017/12/05 16:19:51 $
//// * ------------------------------------------------------------------------- */
////#include <rsl10.h>
////#include <stdint.h>
////
/////* ----------------------------------------------------------------------------
//// * Define declaration
//// * ------------------------------------------------------------------------- */
////#define CONCAT(x, y)                    x##y
////#define DIO_SRC(x)                      CONCAT(DIO_SRC_DIO_, x)
////#define TX_DATA                         "RSL10 includes an I2C"
////#define BUFFER_SIZE                     (sizeof TX_DATA)
////#define I2C_SCL_DIO                     0
////#define I2C_SDA_DIO                     1
////#define BUTTON_DIO                      5
////#define LED_DIO                         6
////#define SLAVE_ADDRESS                   0b1101010
////
/////* DIO number that is used for easy re-flashing (recovery mode) */
////#define RECOVERY_DIO                    12
////
/////* ----------------------------------------------------------------------------
//// * Global variables declaration
//// * ------------------------------------------------------------------------- */
////enum state
////{
////    I2C_IDLE, I2C_START, I2C_WRITE_FINISH, I2C_READ_FINISH
////};
////volatile enum state i2c_state = I2C_IDLE;
////volatile uint8_t    tx_buffer[BUFFER_SIZE] = TX_DATA;
////volatile uint8_t    tx_buffer_index = 0;
////volatile uint8_t    rx_buffer[BUFFER_SIZE];
////volatile uint8_t    rx_buffer_index = 0;
////
/////* ----------------------------------------------------------------------------
//// * Interrupt handler declaration
//// * ------------------------------------------------------------------------- */
////extern void DIO0_IRQHandler(void);
////extern void I2C_IRQHandler(void);
////
/////* ----------------------------------------------------------------------------
//// * Forward declaration
//// * ------------------------------------------------------------------------- */
////void Initialize(void);
////
/////* ----------------------------------------------------------------------------
//// * Function      : void DIO0_IRQHandler(void)
//// * ----------------------------------------------------------------------------
//// * Description   : Start the transactions
//// * Inputs        : None
//// * Outputs       : None
//// * Assumptions   : None
//// * ------------------------------------------------------------------------- */
////void DIO0_IRQHandler(void)
////{
////    static uint8_t ignore_next_dio_int = 0;
////    if (ignore_next_dio_int == 1)
////    {
////        ignore_next_dio_int = 0;
////    }
////    else if (DIO_DATA->ALIAS[BUTTON_DIO] == 0)
////    {
////        /* Button is pressed: Ignore next interrupt.
////         * This is required to deal with the debounce circuit limitations. */
////        ignore_next_dio_int = 1;
////
////        /* Set the i2c_start status */
////        i2c_state = I2C_START;
////    }
////}
////
/////* ----------------------------------------------------------------------------
//// * Function      : void I2C_IRQHandler(void)
//// * ----------------------------------------------------------------------------
//// * Description   : Handle the interrupts of I2C
//// * Inputs        : None
//// * Outputs       : None
//// * Assumptions   : None
//// * ------------------------------------------------------------------------- */
////void I2C_IRQHandler(void)
////{
////    uint32_t i2c_status = Sys_I2C_Get_Status();
////
////    if ((i2c_status & (1 << I2C_STATUS_STOP_DETECT_Pos)) == I2C_STOP_DETECTED)
////    {
////        /* Stop detected, set the I2C state according to the current mode. */
////        if ((i2c_status & (1 << I2C_STATUS_READ_WRITE_Pos)) == I2C_IS_READ)
////        {
////            if (rx_buffer_index == BUFFER_SIZE)
////            {
////                i2c_state = I2C_READ_FINISH;
////            }
////            else
////            {
////                /* Slave did not respond with ACK to Read request return
////                 * to IDLE state. */
////                i2c_state = I2C_IDLE;
////            }
////        }
////        else
////        {
////            if (tx_buffer_index == BUFFER_SIZE)
////            {
////                i2c_state = I2C_WRITE_FINISH;
////            }
////            else
////            {
////                /* Slave did not respond with ACK to write requests return
////                 * to IDLE state. */
////                i2c_state = I2C_IDLE;
////            }
////        }
////    }
////    else if ((i2c_status & (1 << I2C_STATUS_READ_WRITE_Pos)) == I2C_IS_READ)
////    {
////        /* READ mode, If buffer full put a new data on RX buffer. When receive
////         * the number of byte expected, send NACK and Stop. */
////        if ((i2c_status & (1 << I2C_STATUS_BUFFER_FULL_Pos)) == I2C_BUFFER_FULL)
////        {
////            if (rx_buffer_index < (BUFFER_SIZE - 1))
////            {
////                Sys_I2C_ACK();
////                rx_buffer[rx_buffer_index++] = I2C->DATA;
////            }
////            else
////            {
////                Sys_I2C_NACKAndStop();
////                rx_buffer[rx_buffer_index++] = I2C->DATA;
////            }
////        }
////
////        /* If Data Event is set send an ACK to start the read */
////        else if ((i2c_status & (1 << I2C_STATUS_DATA_EVENT_Pos)) ==
////                 I2C_DATA_EVENT)
////        {
////            Sys_I2C_ACK();
////        }
////    }
////    else if ((i2c_status & (1 << I2C_STATUS_READ_WRITE_Pos)) == I2C_IS_WRITE)
////    {
////        /* If WRITE mode, send the next byte. If all positions of the
////         * buffer are sent set LAST_DATA */
////        if ((i2c_status & (1 << I2C_STATUS_ACK_STATUS_Pos)) == I2C_HAS_ACK)
////        {
////            if (tx_buffer_index < BUFFER_SIZE)
////            {
////                I2C->DATA = tx_buffer[tx_buffer_index++];
////            }
////            else
////            {
////                I2C_CTRL1->LAST_DATA_ALIAS = I2C_LAST_DATA_BITBAND;
////            }
////        }
////    }
////}
////
/////* ----------------------------------------------------------------------------
//// * Function      : void Initialize(void)
//// * ----------------------------------------------------------------------------
//// * Description   : Initialize the system by disabling interrupts, configuring
//// *                 the required DIOs for an I2C interface and enabling the
//// *                 interrupts
//// * Inputs        : None
//// * Outputs       : None
//// * Assumptions   : None
//// * ------------------------------------------------------------------------- */
////void Initialize(void)
////{
////    /* Mask all interrupts */
////    __set_PRIMASK(PRIMASK_DISABLE_INTERRUPTS);
////
////    /* Disable all existing interrupts, clearing all pending source */
////    Sys_NVIC_DisableAllInt();
////    Sys_NVIC_ClearAllPendingInt();
////
////    /* Test DIO12 to pause the program to make it easy to re-flash */
////    DIO->CFG[RECOVERY_DIO] = DIO_MODE_INPUT  | DIO_WEAK_PULL_UP |
////                             DIO_LPF_DISABLE | DIO_6X_DRIVE;
////    while (DIO_DATA->ALIAS[RECOVERY_DIO] == 0);
////
////    /* Setup DIO5 as a GPIO input with interrupts on transitions, DIO6 as a
////     * GPIO output. Use the integrated debounce circuit to ensure that only a
////     * single interrupt event occurs for each push of the pushbutton.
////     * The debounce circuit always has to be used in combination with the
////     * transition mode to deal with the debounce circuit limitations.
////     * A debounce filter time of 50 ms is used. */
////    Sys_DIO_Config(BUTTON_DIO, DIO_MODE_GPIO_IN_0 | DIO_WEAK_PULL_UP |
////                   DIO_LPF_DISABLE);
////    Sys_DIO_Config(LED_DIO, DIO_MODE_GPIO_OUT_0);
////    Sys_DIO_IntConfig(0, DIO_EVENT_TRANSITION | DIO_SRC(BUTTON_DIO) |
////                      DIO_DEBOUNCE_ENABLE,
////                      DIO_DEBOUNCE_SLOWCLK_DIV1024, 49);
////
////    /* Configure I2C as Master */
////    Sys_I2C_Config(I2C_MASTER_SPEED_120  | I2C_CONTROLLER_CM3   |
////                   I2C_STOP_INT_ENABLE   | I2C_AUTO_ACK_DISABLE |
////                   I2C_SAMPLE_CLK_ENABLE | I2C_SLAVE_DISABLE);
////
////    /* Configure the DIOs for I2C operation, strong pull-up used to drive the
////     * line,
////     * if external pull-up is used the DIO_NO_PULL can be used */
////    Sys_I2C_DIOConfig(DIO_6X_DRIVE | DIO_LPF_ENABLE | DIO_STRONG_PULL_UP,
////                      I2C_SCL_DIO,
////                      I2C_SDA_DIO);
////
////    /* Enable interrupts */
////    NVIC_EnableIRQ(DIO0_IRQn);
////    NVIC_EnableIRQ(I2C_IRQn);
////
////    /* Unmask all interrupts */
////    __set_PRIMASK(PRIMASK_ENABLE_INTERRUPTS);
////}
////
/////* ----------------------------------------------------------------------------
//// * Function      : int main(void)
//// * ----------------------------------------------------------------------------
//// * Description   : Initialize the system, then send an I2C frame controlled
//// *                 by DIO5 (press to send).
//// *                 Set the LED 0.5 s when a transmission start
//// * Inputs        : None
//// * Outputs       : None
//// * Assumptions   : None
//// * ------------------------------------------------------------------------- */
////int main(void)
////{
////    /* Initialize the system */
////    Initialize();
////
////    /* Spin loop */
////    while (1)
////    {
////        /* Refresh the watch-dog timer */
////        Sys_Watchdog_Refresh();
////
////        /* The I2C transaction starts when the Button is pressed. It starts
////         * with a write to the slave with the address 5.
////         * The LED is set for 0.5 s. */
////        if (i2c_state == I2C_START)
////        {
////            i2c_state = I2C_IDLE;
////            while ((Sys_I2C_Get_Status() & (1 << I2C_STATUS_BUS_FREE_Pos)) !=
////                   I2C_BUS_FREE);
////            tx_buffer_index = 0;
////            Sys_I2C_StartWrite(SLAVE_ADDRESS);
////            Sys_GPIO_Set_High(LED_DIO);
////            Sys_Delay_ProgramROM(0.5 * SystemCoreClock);
////            Sys_GPIO_Set_Low(LED_DIO);
////        }
////
////        /* I2C write has finished execution. Continue with an I2C Read command.
////         * */
////        else if (i2c_state == I2C_WRITE_FINISH)
////        {
////            i2c_state = I2C_IDLE;
////            while ((Sys_I2C_Get_Status() & (1 << I2C_STATUS_BUS_FREE_Pos)) !=
////                   I2C_BUS_FREE);
////
////            rx_buffer_index = 0;
////            Sys_I2C_StartRead(SLAVE_ADDRESS);
////        }
////
////        /* The I2C Read command has finished execution. Check the data received.
////         * The LED is set for 0.1 s if the data received is equal to the data
////         * that was sent. */
////        else if (i2c_state == I2C_READ_FINISH)
////        {
////            i2c_state = I2C_IDLE;
////            uint8_t error = 0;
////            uint8_t i;
////            for (i = 0; i < BUFFER_SIZE; i++)
////            {
////                if (rx_buffer[i] != tx_buffer[i])
////                {
////                    error = 1;
////                    break;
////                }
////            }
////            if (error == 0)
////            {
////                /* Wait 0.1 s before setting the LED so the user can see
////                 * the blink */
////                Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
////                Sys_GPIO_Set_High(LED_DIO);
////                Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
////                Sys_GPIO_Set_Low(LED_DIO);
////            }
////        }
////    }
////}
//
///* ----------------------------------------------------------------------------
// * Copyright (c) 2017 Semiconductor Components Industries, LLC (d/b/a
// * ON Semiconductor), All Rights Reserved
// *
// * This code is the property of ON Semiconductor and may not be redistributed
// * in any form without prior written permission from ON Semiconductor.
// * The terms of use and warranty for this code are covered by contractual
// * agreements between ON Semiconductor and the licensee.
// *
// * This is Reusable Code.
// *
// * ----------------------------------------------------------------------------
// * app.c
// * - Simple application using a DIO5 input to write and read data throught I2C
// * - DIO6 is configured as output and connected to a LED
// * - During communication DIO6 is ON
// * - If data read from the slave is equal to the written data to the slave the
// *   LED blinks at the end of transaction
// * ----------------------------------------------------------------------------
// * $Revision: 1.11 $
// * $Date: 2017/12/05 16:19:51 $
// * ------------------------------------------------------------------------- */
//#include <rsl10.h>
//#include <stdint.h>
//#include <stdio.h>
//
///* ----------------------------------------------------------------------------
// * Define declaration
// * ------------------------------------------------------------------------- */
//#define CONCAT(x, y)                    x##y
//#define DIO_SRC(x)                      CONCAT(DIO_SRC_DIO_, x)
//#define TX_DATA                         "RSL10 includes an I2C"
//#define BUFFER_SIZE                     (sizeof TX_DATA)
//#define I2C_SCL_DIO                     0x0
//#define I2C_SDA_DIO                     0x1
//#define BUTTON_DIO                      5
//#define LED_DIO                         6
//#define SLAVE_ADDRESS				0b1010011
//
//
////volatile int I2C_ACK = 0;
////volatile int I2C_NACK = 1;
//
///* DIO number that is used for easy re-flashing (recovery mode) */
//#define RECOVERY_DIO                    12
//
///* ----------------------------------------------------------------------------
// * Global variables declaration
// * ------------------------------------------------------------------------- */
//enum state
//{
//    I2C_IDLE, I2C_START, I2C_WRITE_FINISH, I2C_READ_FINISH
//};
//volatile enum state i2c_state = I2C_IDLE;
//volatile uint8_t    tx_buffer[BUFFER_SIZE] = TX_DATA;
//volatile uint8_t    tx_buffer_index = 0;
//volatile uint8_t    rx_buffer[BUFFER_SIZE];
//volatile uint8_t    rx_buffer_index = 0;
//
///* ----------------------------------------------------------------------------
// * Interrupt handler declaration
// * ------------------------------------------------------------------------- */
//extern void DIO0_IRQHandler(void);
//extern void I2C_IRQHandler(void);
//
///* ----------------------------------------------------------------------------
// * Forward declaration
// * ------------------------------------------------------------------------- */
//void Initialize(void);
//
///* ----------------------------------------------------------------------------
// * Function      : void DIO0_IRQHandler(void)
// * ----------------------------------------------------------------------------
// * Description   : Start the transactions
// * Inputs        : None
// * Outputs       : None
// * Assumptions   : None
// * ------------------------------------------------------------------------- */
//void DIO0_IRQHandler(void)
//{
//    static uint8_t ignore_next_dio_int = 0;
//    if (ignore_next_dio_int == 1)
//    {
//        ignore_next_dio_int = 0;
//    }
//    else if (DIO_DATA->ALIAS[BUTTON_DIO] == 0)
//    {
//        /* Button is pressed: Ignore next interrupt.
//         * This is required to deal with the debounce circuit limitations. */
//        ignore_next_dio_int = 1;
//
//        /* Set the i2c_start status */
//        i2c_state = I2C_START;
//    }
//}
//
///* ----------------------------------------------------------------------------
// * Function      : void I2C_IRQHandler(void)
// * ----------------------------------------------------------------------------
// * Description   : Handle the interrupts of I2C
// * Inputs        : None
// * Outputs       : None
// * Assumptions   : None
// * ------------------------------------------------------------------------- */
//void I2C_IRQHandler(void)
//{
//    uint32_t i2c_status = Sys_I2C_Get_Status();
//
//    if ((i2c_status & (1 << I2C_STATUS_STOP_DETECT_Pos)) == I2C_STOP_DETECTED)
//    {
//        /* Stop detected, set the I2C state according to the current mode. */
//        if ((i2c_status & (1 << I2C_STATUS_READ_WRITE_Pos)) == I2C_IS_READ)
//        {
//            if (rx_buffer_index == BUFFER_SIZE)
//            {
//                i2c_state = I2C_READ_FINISH;
//            }
//            else
//            {
//                /* Slave did not respond with ACK to Read request return
//                 * to IDLE state. */
//                i2c_state = I2C_IDLE;
//            }
//        }
//        else
//        {
//            if (tx_buffer_index == BUFFER_SIZE)
//            {
//                i2c_state = I2C_WRITE_FINISH;
//            }
//            else
//            {
//                /* Slave did not respond with ACK to write requests return
//                 * to IDLE state. */
//                i2c_state = I2C_IDLE;
//            }
//        }
//    }
//    else if ((i2c_status & (1 << I2C_STATUS_READ_WRITE_Pos)) == I2C_IS_READ)
//    {
//        /* READ mode, If buffer full put a new data on RX buffer. When receive
//         * the number of byte expected, send NACK and Stop. */
//        if ((i2c_status & (1 << I2C_STATUS_BUFFER_FULL_Pos)) == I2C_BUFFER_FULL)
//        {
//            if (rx_buffer_index < (BUFFER_SIZE - 1))
//            {
//                Sys_I2C_ACK();
//                rx_buffer[rx_buffer_index++] = I2C->DATA;
//            }
//            else
//            {
//               Sys_I2C_NACKAndStop();
//            	//Sys_I2C_ACK();
//                rx_buffer[rx_buffer_index++] = I2C->DATA;
//            	//Sys_I2C_ACK();
//            }
//        }
//
//        /* If Data Event is set send an ACK to start the read */
//        else if ((i2c_status & (1 << I2C_STATUS_DATA_EVENT_Pos)) ==
//                 I2C_DATA_EVENT)
//        {
//            Sys_I2C_ACK();
//        }
//    }
//    else if ((i2c_status & (1 << I2C_STATUS_READ_WRITE_Pos)) == I2C_IS_WRITE)
//    {
//        /* If WRITE mode, send the next byte. If all positions of the
//         * buffer are sent set LAST_DATA */
//        if ((i2c_status & (1 << I2C_STATUS_ACK_STATUS_Pos)) == I2C_HAS_ACK)
//        {
//            if (tx_buffer_index < BUFFER_SIZE)
//            {
//               //I2C->DATA = tx_buffer[tx_buffer_index++];
//            	I2C->DATA = tx_buffer[BUFFER_SIZE];
//            }
//            else
//            {
//                I2C_CTRL1->LAST_DATA_ALIAS = I2C_LAST_DATA_BITBAND;
//            }
//        }
//    }
//}
//
///* ----------------------------------------------------------------------------
// * Function      : void Initialize(void)
// * ----------------------------------------------------------------------------
// * Description   : Initialize the system by disabling interrupts, configuring
// *                 the required DIOs for an I2C interface and enabling the
// *                 interrupts
// * Inputs        : None
// * Outputs       : None
// * Assumptions   : None
// * ------------------------------------------------------------------------- */
//void Initialize(void)
//{
//    /* Mask all interrupts */
//    __set_PRIMASK(PRIMASK_DISABLE_INTERRUPTS);
//
//    /* Disable all existing interrupts, clearing all pending source */
//    Sys_NVIC_DisableAllInt();
//    Sys_NVIC_ClearAllPendingInt();
//
//    /* Test DIO12 to pause the program to make it easy to re-flash */
//    DIO->CFG[RECOVERY_DIO] = DIO_MODE_INPUT  | DIO_WEAK_PULL_UP |
//                             DIO_LPF_DISABLE | DIO_6X_DRIVE;
//    while (DIO_DATA->ALIAS[RECOVERY_DIO] == 0);
//
//    /* Setup DIO5 as a GPIO input with interrupts on transitions, DIO6 as a
//     * GPIO output. Use the integrated debounce circuit to ensure that only a
//     * single interrupt event occurs for each push of the pushbutton.
//     * The debounce circuit always has to be used in combination with the
//     * transition mode to deal with the debounce circuit limitations.
//     * A debounce filter time of 50 ms is used. */
//    Sys_DIO_Config(BUTTON_DIO, DIO_MODE_GPIO_IN_0 | DIO_WEAK_PULL_UP |
//                   DIO_LPF_DISABLE);
//    Sys_DIO_Config(LED_DIO, DIO_MODE_GPIO_OUT_0);
//    Sys_DIO_IntConfig(0, DIO_EVENT_TRANSITION | DIO_SRC(BUTTON_DIO) |
//                      DIO_DEBOUNCE_ENABLE,
//                      DIO_DEBOUNCE_SLOWCLK_DIV1024, 49);
//
//    /* Configure I2C as Master */
//    Sys_I2C_Config(I2C_MASTER_SPEED_120  | I2C_CONTROLLER_CM3   |
//                   I2C_STOP_INT_ENABLE   | I2C_AUTO_ACK_DISABLE |
//                   I2C_SAMPLE_CLK_ENABLE | I2C_SLAVE_DISABLE);
//
//    /* Configure the DIOs for I2C operation, strong pull-up used to drive the
//     * line,
//     * if external pull-up is used the DIO_NO_PULL can be used */
//    Sys_I2C_DIOConfig(DIO_6X_DRIVE | DIO_LPF_ENABLE | DIO_STRONG_PULL_UP,
//                      I2C_SCL_DIO,
//                      I2C_SDA_DIO);
//
//    /* Enable interrupts */
//    NVIC_EnableIRQ(DIO0_IRQn);
//    NVIC_EnableIRQ(I2C_IRQn);
//
//    /* Unmask all interrupts */
//    __set_PRIMASK(PRIMASK_ENABLE_INTERRUPTS);
//}
//
///*void UART_TX_IRQHandler(void)
//{
//    nbr_tx_send++;
//    if (tx_buffer_index < BUFFER_SIZE)
//    {
//        UART->TX_DATA = tx_buffer[tx_buffer_index++];
//    }
//}*/
//
//void writeAccel(uint8_t addr, uint8_t dataW){
//
//	uint32_t statc = 0;
//	uint8_t store = 0;
//	uint8_t SADW = 0xD5;
//	uint8_t SAD = 0b1010011;
//
//	//statc = (Sys_I2C_Get_Status() & 0b0000000000010000);
//	//while(statc == 0b0000000000010000 ){
//		//statc = (Sys_I2C_Get_Status() & 0b0000000000010000);
//	//}
//
//	//Sys_GPIO_Set_Low(I2C_SDA_DIO);
//	Sys_I2C_StartWrite(SLAVE_ADDRESS);
//	//Sys_I2C_StartWrite(0b0010000);
//	while((Sys_I2C_Get_Status() & (1 << I2C_STATUS_ACK_STATUS_Pos)) != I2C_HAS_ACK);
//	tx_buffer[BUFFER_SIZE] = addr;
//	while((Sys_I2C_Get_Status() & (1 << I2C_STATUS_ACK_STATUS_Pos)) != I2C_HAS_ACK);
//	tx_buffer[BUFFER_SIZE] = dataW;
//	while((Sys_I2C_Get_Status() & (1 << I2C_STATUS_ACK_STATUS_Pos)) != I2C_HAS_ACK);
//	I2C_CTRL1->LAST_DATA_ALIAS = I2C_LAST_DATA_BITBAND;
//	//Sys_GPIO_Set_High(I2C_SDA_DIO);
//
//}
//
//uint8_t readAccel(uint8_t addrW){
//
//	uint8_t acc = 0;
//	uint8_t acc2 = 0;
//	uint32_t statc = 0;
//	uint8_t SAD = 0b1010011;
//
//	//statc = (Sys_I2C_Get_Status() & 0b0000000000010000);
//	//while(statc == 0b0000000000010000 ){
//		//statc = (Sys_I2C_Get_Status() & 0b0000000000010000);
////	}
//	//Sys_GPIO_Set_Low(I2C_SDA_DIO);
//	Sys_I2C_StartWrite(SLAVE_ADDRESS);
//	while((Sys_I2C_Get_Status() & (1 << I2C_STATUS_ACK_STATUS_Pos)) != I2C_HAS_ACK);
//	tx_buffer[BUFFER_SIZE] = addrW;
//	//Sys_I2C_DATA_IS(0x00);
//	while((Sys_I2C_Get_Status() & (1 << I2C_STATUS_ACK_STATUS_Pos)) != I2C_HAS_ACK){
//		//I2C->DATA = tx_buffer[BUFFER_SIZE];
//	}
//	Sys_I2C_StartRead(SLAVE_ADDRESS);
//	while((Sys_I2C_Get_Status() & (1 << I2C_STATUS_ACK_STATUS_Pos)) != I2C_HAS_ACK);
//	// catch the data
//	acc = I2C->DATA;
//	Sys_I2C_ACK();
//	acc2 = rx_buffer[BUFFER_SIZE];
//	//Sys_I2C_NACKAndStop();
//	I2C_CTRL1->LAST_DATA_ALIAS = I2C_LAST_DATA_BITBAND;
//	//Sys_GPIO_Set_High(I2C_SDA_DIO);
//
//	return acc;
//
//}
//
//void configSEN(void){
//
//	uint8_t FIFOaddr = 0x2E;
//	uint8_t FIFOset = 0b11000000;
//	uint8_t REG6addr = 0x20;
//	uint8_t REG6set = 0b10010000;
//	uint8_t REG9addr = 0x23;
//	uint8_t REG9set = 0b00000010;
//
//	//Set FIFO-CTRL register for continuous mode operation
//	writeAccel(FIFOaddr, FIFOset);
//	//Set CTRL_REG6_XL to use accelerometer but not use gyroscope
//	writeAccel(REG6addr, REG6set);
//	//CRL_REG9_FIFO_EN to enable FIFO memory
//	writeAccel(REG9addr, REG9set);
//
//}
//
//
//
///* ----------------------------------------------------------------------------
// * Function      : int main(void)
// * ----------------------------------------------------------------------------
// * Description   : Initialize the system, then send an I2C frame controlled
// *                 by DIO5 (press to send).
// *                 Set the LED 0.5 s when a transmission start
// * Inputs        : None
// * Outputs       : None
// * Assumptions   : None
// * ------------------------------------------------------------------------- */
//int main(void)
//{
//    /*while(1){
//    	Sys_GPIO_Set_Low(I2C_SDA_DIO);
//    	Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
//    	Sys_GPIO_Set_High(I2C_SDA_DIO);
//    }*/
//    /* Initialize the system */
//	//Sys_GPIO_Set_High(I2C_SDA_DIO);
//    Initialize();
//   // configSEN();
//    //Sys_GPIO_Set_High(I2C_SDA_DIO);
//    uint8_t status = 0x27;
//    uint8_t xvallo = 0x28;
//    uint8_t xvalhi = 0x29;
//    uint8_t yvallo = 0x2A;
//    uint8_t yvalhi = 0x2B;
//    uint8_t zvallo = 0x2C;
//    uint8_t zvalhi = 0x2D;
//    uint8_t catchxlo = 0;
//    uint8_t catchxhi = 0;
//    uint8_t catchylo = 0;
//    uint8_t catchyhi = 0;
//    uint8_t catchzlo = 0;
//    uint8_t catchzhi = 0;
//	uint8_t REG6addr = 0x20;
//	uint8_t REG6set = 0b10010000;
//    float statcheck = 0;
//
//
////    while(1){
////    	uint8_t hold = 0;
////    	writeAccel(0x05, 0xAA);
////    	hold = readAccel(0x05);
////    	if (hold == 0xAA){
////    		while(1){
////            Sys_GPIO_Set_High(LED_DIO);
////            Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
////            Sys_GPIO_Set_Low(LED_DIO);
////    		}
////    	}else{
////    		hold = 0;
////    	}
////    }
////    /* Spin loop */
////    while (1)
////    {
////    	statcheck = (readAccel(status) & 0b00000001);
////    	if (statcheck == 0b00000001){
////    		//printf("Hello World \n");
////    		//putchar(64);
////    		catchxhi = readAccel(xvalhi);
////    		catchxlo = readAccel(xvallo);
////    		catchyhi = readAccel(yvalhi);
////    		catchylo = readAccel(yvallo);
////    		catchzhi = readAccel(zvalhi);
////    		catchzlo = readAccel(zvallo);
////    		/* Restart indexes */
////    	}else{
////    		statcheck = 0;
////    	}
////    }
//        //Sys_Delay_ProgramROM(0.5 * SystemCoreClock);
//
//        /* Refresh the watch-dog timer */
//    while(1){
//       Sys_Watchdog_Refresh();
//
//        /* The I2C transaction starts when the Button is pressed. It starts
//         * with a write to the slave with the address 5.
//         * The LED is set for 0.5 s. */
//      // if (i2c_state == I2C_START)
//       //{
//           i2c_state = I2C_IDLE;
//           while ((Sys_I2C_Get_Status() & (1 << I2C_STATUS_BUS_FREE_Pos)) !=
//                  I2C_BUS_FREE);
//           tx_buffer_index = 0;
//           readAccel(0x32);
//           //writeAccel(0x38, 0b10000001);
//           Sys_GPIO_Set_High(LED_DIO);
//           Sys_Delay_ProgramROM(0.5 * SystemCoreClock);
//           Sys_GPIO_Set_Low(LED_DIO);
//
//
//        /* I2C write has finished execution. Continue with an I2C Read command.
//         * */
////        else if (i2c_state == I2C_WRITE_FINISH)
////        {
////            i2c_state = I2C_IDLE;
////            while ((Sys_I2C_Get_Status() & (1 << I2C_STATUS_BUS_FREE_Pos)) !=
////                   I2C_BUS_FREE);
////
////            rx_buffer_index = 0;
////            Sys_I2C_StartRead(SLAVE_ADDRESS);
////        }
//
//        /* The I2C Read command has finished execution. Check the data received.
//         * The LED is set for 0.1 s if the data received is equal to the data
//         * that was sent. */
////        else if (i2c_state == I2C_READ_FINISH)
////        {
////            i2c_state = I2C_IDLE;
////            uint8_t error = 0;
////            uint8_t i;
////            for (i = 0; i < BUFFER_SIZE; i++)
////            {
////                if (rx_buffer[i] != tx_buffer[i])
////                {
////                    error = 1;
////                    break;
////                }
////            }
////            if (error == 0)
////            {
////                /* Wait 0.1 s before setting the LED so the user can see
////                 * the blink */
////                Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
////                Sys_GPIO_Set_High(LED_DIO);
////                Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
////                Sys_GPIO_Set_Low(LED_DIO);
////            }
////        }
//        /* Put the new value on UART TX buffer */
//        //tx_buffer[tx_buffer_index++] = catch;
//       // UART->TX_DATA   = tx_buffer[tx_buffer_index];
//        //sprintf(catchmsb, "%.5f",catch);
//        //printf(catchmsb);
//
//        /* Remove flag */
//        //start_tx = 0;
//
//        /* Refresh the watch-dog timer */
//        /*Sys_Watchdog_Refresh();*/
//
//        /* The I2C transaction starts when the Button is pressed. It starts
//         * with a write to the slave with the address 5.
//         * The LED is set for 0.5 s. */
//        /*if (i2c_state == I2C_START)
//        {
//            i2c_state = I2C_IDLE;
//            while ((Sys_I2C_Get_Status() & (1 << I2C_STATUS_BUS_FREE_Pos)) !=
//                   I2C_BUS_FREE);
//            tx_buffer_index = 0;
//            Sys_I2C_StartWrite(SLAVE_ADDRESS);
//            Sys_GPIO_Set_High(LED_DIO);
//            Sys_Delay_ProgramROM(0.5 * SystemCoreClock);
//            Sys_GPIO_Set_Low(LED_DIO);
//        }
//
//        /* I2C write has finished execution. Continue with an I2C Read command.
//         * */
//       /* else if (i2c_state == I2C_WRITE_FINISH)
//        {
//            i2c_state = I2C_IDLE;
//            while ((Sys_I2C_Get_Status() & (1 << I2C_STATUS_BUS_FREE_Pos)) !=
//                   I2C_BUS_FREE);
//
//            rx_buffer_index = 0;
//            Sys_I2C_StartRead(SLAVE_ADDRESS);
//        }
//
//        /* The I2C Read command has finished execution. Check the data received.
//         * The LED is set for 0.1 s if the data received is equal to the data
//         * that was sent. */
//        /*else if (i2c_state == I2C_READ_FINISH)
//        {
//            i2c_state = I2C_IDLE;
//            uint8_t error = 0;
//            uint8_t i;
//            for (i = 0; i < BUFFER_SIZE; i++)
//            {
//                if (rx_buffer[i] != tx_buffer[i])
//                {
//                    error = 1;
//                    break;
//                }
//            }
//            if (error == 0)
//            {
//                /* Wait 0.1 s before setting the LED so the user can see
//                 * the blink */
//               /* Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
//                Sys_GPIO_Set_High(LED_DIO);
//                Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
//                Sys_GPIO_Set_Low(LED_DIO);
//            }
//        }*/
//    }
//}
//
//
//


/* ----------------------------------------------------------------------------
 * Copyright (c) 2017 Semiconductor Components Industries, LLC (d/b/a
 * ON Semiconductor), All Rights Reserved
 *
 * This code is the property of ON Semiconductor and may not be redistributed
 * in any form without prior written permission from ON Semiconductor.
 * The terms of use and warranty for this code are covered by contractual
 * agreements between ON Semiconductor and the licensee.
 *
 * This is Reusable Code.
 *
 * ----------------------------------------------------------------------------
 * app.c
 * - Simple application using a DIO5 input to write and read data throught I2C
 * - DIO6 is configured as output and connected to a LED
 * - During communication DIO6 is ON
 * - If data read from the slave is equal to the written data to the slave the
 *   LED blinks at the end of transaction
 * ----------------------------------------------------------------------------
 * $Revision: 1.11 $
 * $Date: 2017/12/05 16:19:51 $
 * ------------------------------------------------------------------------- */
#include <rsl10.h>
#include <stdint.h>
#include <stdio.h>

/* ----------------------------------------------------------------------------
 * Define declaration
 * ------------------------------------------------------------------------- */
#define CONCAT(x, y)                    x##y
#define DIO_SRC(x)                      CONCAT(DIO_SRC_DIO_, x)
#define TX_DATA                         "RSL10 includes an I2C"
#define BUFFER_START                    (sizeof TX_DATA)
#define I2C_SCL_DIO                     0x0
#define I2C_SDA_DIO                     0x1
#define BUTTON_DIO                      5
#define LED_DIO                         6
#define SLAVE_ADDRESS				0b1010011



/* DIO number that is used for easy re-flashing (recovery mode) */
#define RECOVERY_DIO                    12

/* ----------------------------------------------------------------------------
 * Global variables declaration
 * ------------------------------------------------------------------------- */
enum state
{
    I2C_IDLE, I2C_START, I2C_WRITE_FINISH, I2C_READ_FINISH
};
volatile enum state i2c_state = I2C_IDLE;
volatile int BUFFER_SIZEtx = 0;
volatile int BUFFER_SIZErx = 0;
volatile uint8_t    tx_buffer[BUFFER_START] = TX_DATA;
volatile uint8_t    tx_buffer_index = 0;
volatile uint8_t    rx_buffer[BUFFER_START];
volatile uint8_t    rx_buffer_index = 0;

/* ----------------------------------------------------------------------------
 * Interrupt handler declaration
 * ------------------------------------------------------------------------- */
extern void DIO0_IRQHandler(void);
extern void I2C_IRQHandler(void);

/* ----------------------------------------------------------------------------
 * Forward declaration
 * ------------------------------------------------------------------------- */
void Initialize(void);

/* ----------------------------------------------------------------------------
 * Function      : void DIO0_IRQHandler(void)
 * ----------------------------------------------------------------------------
 * Description   : Start the transactions
 * Inputs        : None
 * Outputs       : None
 * Assumptions   : None
 * ------------------------------------------------------------------------- */
void DIO0_IRQHandler(void)
{
    static uint8_t ignore_next_dio_int = 0;
    if (ignore_next_dio_int == 1)
    {
        ignore_next_dio_int = 0;
    }
    else if (DIO_DATA->ALIAS[BUTTON_DIO] == 0)
    {
        /* Button is pressed: Ignore next interrupt.
         * This is required to deal with the debounce circuit limitations. */
        ignore_next_dio_int = 1;

        /* Set the i2c_start status */
        i2c_state = I2C_START;
    }
}

/* ----------------------------------------------------------------------------
 * Function      : void I2C_IRQHandler(void)
 * ----------------------------------------------------------------------------
 * Description   : Handle the interrupts of I2C
 * Inputs        : None
 * Outputs       : None
 * Assumptions   : None
 * ------------------------------------------------------------------------- */
void I2C_IRQHandler(void)
{
    uint32_t i2c_status = Sys_I2C_Get_Status();

    if ((i2c_status & (1 << I2C_STATUS_STOP_DETECT_Pos)) == I2C_STOP_DETECTED)
    {
        /* Stop detected, set the I2C state according to the current mode. */
        if ((i2c_status & (1 << I2C_STATUS_READ_WRITE_Pos)) == I2C_IS_READ)
        {
            if (rx_buffer_index == BUFFER_SIZErx)
            {
                i2c_state = I2C_READ_FINISH;
            }
            else
            {
                /* Slave did not respond with ACK to Read request return
                 * to IDLE state. */
                i2c_state = I2C_IDLE;
            }
        }
        else
        {
            if (tx_buffer_index == BUFFER_SIZEtx)
            {
                i2c_state = I2C_WRITE_FINISH;
                for(int aa; aa<tx_buffer_index; aa++) {
                	tx_buffer[aa] = 0;
                }

            }
            else
            {
                /* Slave did not respond with ACK to write requests return
                 * to IDLE state. */
                i2c_state = I2C_IDLE;
            }
        }
    }
    else if ((i2c_status & (1 << I2C_STATUS_READ_WRITE_Pos)) == I2C_IS_READ)
    {
        /* READ mode, If buffer full put a new data on RX buffer. When receive
         * the number of byte expected, send NACK and Stop. */
        if ((i2c_status & (1 << I2C_STATUS_BUFFER_FULL_Pos)) == I2C_BUFFER_FULL)
        {
            if (rx_buffer_index < (BUFFER_SIZErx - 1))
            {
                Sys_I2C_ACK();
                rx_buffer[rx_buffer_index++] = I2C->DATA;
            }
            else
            {
               Sys_I2C_NACKAndStop();
               rx_buffer[rx_buffer_index++] = I2C->DATA;
               //I2C_CTRL1->RESET_ALIAS = I2C_RESET_BITBAND;
            }
        }

        /* If Data Event is set send an ACK to start the read */
        else if ((i2c_status & (1 << I2C_STATUS_DATA_EVENT_Pos)) ==
                 I2C_DATA_EVENT)
        {
            Sys_I2C_ACK();
        }
    }
    else if ((i2c_status & (1 << I2C_STATUS_READ_WRITE_Pos)) == I2C_IS_WRITE)
    {
        /* If WRITE mode, send the next byte. If all positions of the
         * buffer are sent set LAST_DATA */
        if ((i2c_status & (1 << I2C_STATUS_ACK_STATUS_Pos)) == I2C_HAS_ACK)
        {
            if (tx_buffer_index < BUFFER_SIZEtx)
            {
                I2C->DATA_M = tx_buffer[tx_buffer_index++];
            }
            else
            {
//            	I2C->DATA_M = 0x00;
                I2C_CTRL1->LAST_DATA_ALIAS = I2C_LAST_DATA_BITBAND;
//                I2C->DATA_M = 0x00;
//                I2C_CTRL1->STOP_ALIAS = I2C_STOP_BITBAND;
                //I2C_CTRL1->RESET_ALIAS = I2C_RESET_BITBAND;
            }
        }
    }
}

/* ----------------------------------------------------------------------------
 * Function      : void Initialize(void)
 * ----------------------------------------------------------------------------
 * Description   : Initialize the system by disabling interrupts, configuring
 *                 the required DIOs for an I2C interface and enabling the
 *                 interrupts
 * Inputs        : None
 * Outputs       : None
 * Assumptions   : None
 * ------------------------------------------------------------------------- */
void Initialize(void)
{
    /* Mask all interrupts */
    __set_PRIMASK(PRIMASK_DISABLE_INTERRUPTS);

    /* Disable all existing interrupts, clearing all pending source */
    Sys_NVIC_DisableAllInt();
    Sys_NVIC_ClearAllPendingInt();

    /* Test DIO12 to pause the program to make it easy to re-flash */
    DIO->CFG[RECOVERY_DIO] = DIO_MODE_INPUT  | DIO_WEAK_PULL_UP |
                             DIO_LPF_DISABLE | DIO_6X_DRIVE;
    while (DIO_DATA->ALIAS[RECOVERY_DIO] == 0);

    /* Setup DIO5 as a GPIO input with interrupts on transitions, DIO6 as a
     * GPIO output. Use the integrated debounce circuit to ensure that only a
     * single interrupt event occurs for each push of the pushbutton.
     * The debounce circuit always has to be used in combination with the
     * transition mode to deal with the debounce circuit limitations.
     * A debounce filter time of 50 ms is used. */
    Sys_DIO_Config(BUTTON_DIO, DIO_MODE_GPIO_IN_0 | DIO_WEAK_PULL_UP |
                   DIO_LPF_DISABLE);
    Sys_DIO_Config(LED_DIO, DIO_MODE_GPIO_OUT_0);
    Sys_DIO_IntConfig(0, DIO_EVENT_TRANSITION | DIO_SRC(BUTTON_DIO) |
                      DIO_DEBOUNCE_ENABLE,
                      DIO_DEBOUNCE_SLOWCLK_DIV1024, 49);

    /* Configure I2C as Master */
    Sys_I2C_Config(I2C_MASTER_SPEED_120  | I2C_CONTROLLER_CM3   |
                   I2C_STOP_INT_ENABLE   | I2C_AUTO_ACK_DISABLE |
                   I2C_SAMPLE_CLK_ENABLE | I2C_SLAVE_DISABLE);

    /* Configure the DIOs for I2C operation, strong pull-up used to drive the
     * line,
     * if external pull-up is used the DIO_NO_PULL can be used */
    Sys_I2C_DIOConfig(DIO_6X_DRIVE | DIO_LPF_ENABLE | DIO_STRONG_PULL_UP,
                      I2C_SCL_DIO,
                      I2C_SDA_DIO);

    /* Enable interrupts */
    NVIC_EnableIRQ(DIO0_IRQn);
    NVIC_EnableIRQ(I2C_IRQn);

    /* Unmask all interrupts */
    __set_PRIMASK(PRIMASK_ENABLE_INTERRUPTS);
}

void writeAccel(uint8_t addr, uint8_t dataW){

	while ((Sys_I2C_Get_Status() & (1 << I2C_STATUS_BUS_FREE_Pos)) !=
			I2C_BUS_FREE);
	tx_buffer_index = 0;
	BUFFER_SIZEtx = 2;
	tx_buffer[BUFFER_SIZEtx-2] = addr;
	tx_buffer[BUFFER_SIZEtx-1] = dataW;
	Sys_I2C_StartWrite(SLAVE_ADDRESS);
	// add in wait for stop, then clear buffers

}

uint8_t readAccel(uint8_t addrW, int bytes2Read){

	while ((Sys_I2C_Get_Status() & (1 << I2C_STATUS_BUS_FREE_Pos)) !=
			I2C_BUS_FREE);
	tx_buffer_index = 0;
	rx_buffer_index = 0;
	uint8_t acc = 0;
	BUFFER_SIZEtx = 1;
	BUFFER_SIZErx = bytes2Read;
	tx_buffer[BUFFER_SIZEtx-1] = addrW;
	//I2C->DATA = addrW;
	Sys_I2C_StartWrite(SLAVE_ADDRESS);
	while ((Sys_I2C_Get_Status() & (1 << I2C_STATUS_BUS_FREE_Pos)) !=
				I2C_BUS_FREE);
	Sys_I2C_StartRead(SLAVE_ADDRESS);
	return acc;

}

void configADXL(void){
	uint8_t BWRateReg = 0x2C;
	uint8_t PwrCtrlReg = 0x2D;
	uint8_t DataFmtReg = 0x31;
	uint8_t FIFOCtrlReg = 0x38;
	uint8_t BWVal = 0x0A;
	uint8_t PowerVal = 0x08;
	uint8_t FormatVal = 0x02;
	uint8_t fifoVal = 0x81;
	// reg 2C, value 0A: bit rate of 100Hz
	writeAccel(BWRateReg,BWVal);
	// reg 2D, value 08: put into measure mode
	writeAccel(PwrCtrlReg,PowerVal);
	// reg 31, value 02: 10 bit resolution, right justified, +/- 8g
	writeAccel(DataFmtReg,FormatVal);
	// reg 38, value 81: stream mode, one sample for watermark
	writeAccel(FIFOCtrlReg, fifoVal);


}



/* ----------------------------------------------------------------------------
 * Function      : int main(void)
 * ----------------------------------------------------------------------------
 * Description   : Initialize the system, then send an I2C frame controlled
 *                 by DIO5 (press to send).
 *                 Set the LED 0.5 s when a transmission start
 * Inputs        : None
 * Outputs       : None
 * Assumptions   : None
 * ------------------------------------------------------------------------- */
int main(void)
{
    /*while(1){
    	Sys_GPIO_Set_Low(I2C_SDA_DIO);
    	Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
    	Sys_GPIO_Set_High(I2C_SDA_DIO);
    }*/
    /* Initialize the system */
	//Sys_GPIO_Set_High(I2C_SDA_DIO);
    Initialize();
    configADXL();
    //Sys_GPIO_Set_High(I2C_SDA_DIO);
    uint8_t catchxlo = 0;
    uint8_t catchxhi = 0;
    uint8_t catchylo = 0;
    uint8_t catchyhi = 0;
    uint8_t catchzlo = 0;
    uint8_t catchzhi = 0;
    float statcheck = 0;


//    while(1){
//    	uint8_t hold = 0;
//    	writeAccel(0x05, 0xAA);
//    	hold = readAccel(0x05);
//    	if (hold == 0xAA){
//    		while(1){
//            Sys_GPIO_Set_High(LED_DIO);
//            Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
//            Sys_GPIO_Set_Low(LED_DIO);
//    		}
//    	}else{
//    		hold = 0;
//    	}
//    }
//    /* Spin loop */
//    while (1)
//    {
//    	statcheck = (readAccel(status) & 0b00000001);
//    	if (statcheck == 0b00000001){
//    		//printf("Hello World \n");
//    		//putchar(64);
//    		catchxhi = readAccel(xvalhi);
//    		catchxlo = readAccel(xvallo);
//    		catchyhi = readAccel(yvalhi);
//    		catchylo = readAccel(yvallo);
//    		catchzhi = readAccel(zvalhi);
//    		catchzlo = readAccel(zvallo);
//    		/* Restart indexes */
//    	}else{
//    		statcheck = 0;
//    	}
//    }
        //Sys_Delay_ProgramROM(0.5 * SystemCoreClock);

        /* Refresh the watch-dog timer */
    while(1){
       Sys_Watchdog_Refresh();



       int Demo6 = 1;


//
//	   int bytesRead = 1;
       uint8_t firstDataReg = 0x32;




       Sys_GPIO_Set_Low(LED_DIO);
       Sys_Delay_ProgramROM(0.5 * SystemCoreClock);
       Sys_GPIO_Set_High(LED_DIO);
//	   readAccel(0x00,bytesRead);

       if (Demo6==1) {
    	   int bytesRead2 = 6;
    	   catchzhi = readAccel(firstDataReg,bytesRead2);
       }
       else {
    	   int bytesRead3 = 2;
    	   catchxhi = readAccel(firstDataReg,bytesRead3);
       }

//	   Sys_GPIO_Set_Low(LED_DIO);
//	   Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
//	   Sys_GPIO_Set_High(LED_DIO);
//       Sys_GPIO_Set_Low(LED_DIO);
//       Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
//       Sys_GPIO_Set_High(LED_DIO);
//	//   Sys_GPIO_Set_Low(LED_DIO);
//	  // Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
//	  // Sys_GPIO_Set_High(LED_DIO);
////	   Sys_GPIO_Set_Low(LED_DIO);
////	   Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
////	   Sys_GPIO_Set_High(LED_DIO);
   //
//	   //readAccel(0x2C,bytesRead2);
//	   Sys_GPIO_Set_Low(LED_DIO);
//	          Sys_Delay_ProgramROM(0.1 * SystemCoreClock);
//	          Sys_GPIO_Set_High(LED_DIO);
//	   	   writeAccel(0x38, 0b10000001);



    }
}

