void setup() {
    ROM_SysCtlClockSet(SYSCTL_SYSDIV_2_5 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_16MHZ);

    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);

    ROM_FPULazyStackingEnable();
    ROM_FPUEnable();

    uart_setup();

    sd_init();
    dac_setup();
    keycontrol_setup();
    initConfig();
    tick_setup();    
    soundoutcontrol_setup();
    setupADC();
    setupUSBStore();
    initSampleBlocks();
    
    
    ROM_TimerIntEnable(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
    ROM_IntMasterEnable();

    DEBUG_PRINT("Setup complete\n", NULL);
}

void init()
{
    ROM_FPUEnable();
    ROM_FPULazyStackingEnable();

    ROM_SysCtlClockSet(SYSCTL_SYSDIV_2_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);

    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
    GPIO_PORTB_DIR_R = 0x00;
    GPIO_PORTB_DEN_R = 0xff;

    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
    ROM_GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, LED_RED|LED_GREEN|LED_BLUE);

    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
    ROM_GPIOPinConfigure(GPIO_PA0_U0RX);
    ROM_GPIOPinConfigure(GPIO_PA1_U0TX);
    ROM_GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
    UARTStdioInit(0);

    ROM_SysTickPeriodSet(ROM_SysCtlClockGet() / 1000000);
    ROM_SysTickEnable();
    ROM_SysTickIntEnable();

    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);
    ROM_TimerConfigure(TIMER0_BASE, TIMER_CFG_32_BIT_PER);

    reset();
}

//*****************************************************************************
//
// Generic configuration is handled in this function.
//
// This function is called by the start up code to perform any configuration
// necessary before calling the update routine.  It is responsible for setting
// the system clock to the expected rate and setting flash programming
// parameters prior to calling ConfigureUSBInterface() to set up the USB
// hardware.
//
// \return None.
//
//*****************************************************************************
void
UpdaterMain(void)
{
    //
    // Make sure NVIC points at the correct vector table.
    //
    HWREG(NVIC_VTABLE) = 0;

    //
    // Run from the PLL at 120 MHz.
    //
    g_ui32SysClock = MAP_SysCtlClockFreqSet((SYSCTL_XTAL_25MHZ |
                                            SYSCTL_OSC_MAIN |
                                            SYSCTL_USE_PLL |
                                            SYSCTL_CFG_VCO_480), 120000000);

    //
    // Enable lazy stacking for interrupt handlers.  This allows floating-point
    // instructions to be used within interrupt handlers, but at the expense of
    // extra stack usage.
    //
    ROM_FPULazyStackingEnable();

    //
    // Configure the USB interface and power the bus.
    //
    ConfigureUSBInterface();

    //
    // Call the updater function.  This will attempt to load a new image
    // into flash from a USB memory stick.
    //
    UpdaterUSB();
}

static void sys_setup(void)
{
  ROM_FPULazyStackingEnable();
  ROM_SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);
  ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
  ROM_GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_2);
}

void Board::init() // initialize the board specifics
{
	//
		// Enable lazy stacking for interrupt handlers.  This allows floating-point
		// instructions to be used within interrupt handlers, but at the expense of
		// extra stack usage.
		//
		ROM_FPULazyStackingEnable();
		//
		// Set the clocking to run from the PLL at 50MHz
		//
		ROM_SysCtlClockSet(
				SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN
						| SYSCTL_XTAL_16MHZ);
		Usb::init();
/*	//
	// Enable lazy stacking for interrupt handlers.  This allows floating-point
	// instructions to be used within interrupt handlers, but at the expense of
	// extra stack usage.
	FPUEnable();
	FPULazyStackingEnable();
	SysCtlClockSet(
			SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_OSC_MAIN
					| SYSCTL_XTAL_16MHZ); // Set the clocking to run directly from the crystal.
	IntMasterEnable(); // Enable interrupts to the processor.*/

	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF); // Enable the GPIO port that is used for the on-board LED.
	GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_2); // Enable the GPIO pins for the LED (PF2).
	GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_1);
	GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_3);

/*	SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0); // Enable the peripherals used by this example.
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
	IntMasterEnable(); // Enable processor interrupts.
	// Set up the period for the SysTick timer for 1 mS.
	SysTickPeriodSet(SysCtlClockGet() / 1000);
	SysTickIntEnable(); // Enable the SysTick Interrupt.
	SysTickEnable(); // Enable SysTick.

	GPIOPinConfigure(GPIO_PA0_U0RX); // Set GPIO A0 and A1 as UART pins.
	GPIOPinConfigure(GPIO_PA1_U0TX);
	GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);

	UARTConfigSetExpClk(UART0_BASE, SysCtlClockGet(), 115200,
			(UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE)); // Configure the UART for 115,200, 8-N-1 operation.
	IntEnable(INT_UART0);
	UARTFIFODisable(UART0_BASE);
//	UARTFIFOLevelSet(UART0_BASE, UART_FIFO_TX1_8, UART_FIFO_RX1_8);
	UARTFlowControlSet(UART0_BASE, UART_FLOWCONTROL_NONE);
	UARTIntDisable(UART0_BASE, UART_INT_RT);
	UARTIntEnable(UART0_BASE, UART_INT_RX | UART_INT_TX); // Enable the UART interrupt.*/
	Board::setLedOn(Board::LED_GREEN, false);
	Board::setLedOn(Board::LED_RED, false);
	Board::setLedOn(Board::LED_BLUE, false);
	ADCInit();
}

void setup()
{
	//--------------------- GENERAL ---------------------

    // Enable lazy stacking for interrupt handlers.  This allows floating-point
    // instructions to be used within interrupt handlers, but at the expense of
    // extra stack usage.
    ROM_FPUEnable();
    ROM_FPULazyStackingEnable();

    // Set the clocking to run directly from the crystal.
    ROM_SysCtlClockSet(SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_OSC_MAIN |
                       SYSCTL_XTAL_16MHZ);

    ROM_IntMasterEnable();

    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);

    ROM_GPIOPinTypeGPIOOutput(GPIO_PORTA_BASE, GPIO_PIN_6);

	//--------------------- UART ---------------------

    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);

    GPIOPinConfigure(GPIO_PA0_U0RX);
    GPIOPinConfigure(GPIO_PA1_U0TX);
    ROM_GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);

    // Configure the UART for 115,200, 8-N-1 operation.
    ROM_UARTConfigSetExpClk(UART0_BASE, ROM_SysCtlClockGet(), 115200,
                            (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));

    // Enable the UART interrupt.
    ROM_IntEnable(INT_UART0);
    ROM_UARTIntEnable(UART0_BASE, UART_INT_RX | UART_INT_RT);


	//--------------------- SSI ---------------------

    SysCtlPeripheralEnable(SYSCTL_PERIPH_SSI0);

    GPIOPinConfigure(GPIO_PA2_SSI0CLK);
    GPIOPinConfigure(GPIO_PA3_SSI0FSS);
    GPIOPinConfigure(GPIO_PA4_SSI0RX);
    GPIOPinConfigure(GPIO_PA5_SSI0TX);

    GPIOPinTypeSSI(GPIO_PORTA_BASE, GPIO_PIN_5 | GPIO_PIN_4 | GPIO_PIN_3 | GPIO_PIN_2);

    SSIConfigSetExpClk(SSI0_BASE, SysCtlClockGet(), SSI_FRF_MOTO_MODE_0, SSI_MODE_MASTER, 10000, 8);

    SSIEnable(SSI0_BASE);

}

void Config_System(void)
{
	ROM_FPUEnable();
	ROM_FPULazyStackingEnable();
	// Config clock
	ROM_SysCtlClockSet(SYSCTL_SYSDIV_2_5 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_16MHZ);
	system_SystickConfig(1);
	//
	ROM_SysCtlPeripheralEnable(BOOST_ENABLE_PREIPHERAL);
	ROM_GPIOPinTypeGPIOOutput(BOOST_ENABLE_PORT, BOOST_ENABLE_PIN);
	ROM_GPIOPadConfigSet(BOOST_ENABLE_PORT, BOOST_ENABLE_PIN, GPIO_STRENGTH_8MA, GPIO_PIN_TYPE_STD_WPU);
	ROM_GPIOPinWrite(BOOST_ENABLE_PORT, BOOST_ENABLE_PIN, 0xff);
}

// Main ----------------------------------------------------------------------------------------------
int main(void){

	// Enable lazy stacking
	ROM_FPULazyStackingEnable();

	// Set the clocking to run directly from the crystal.
	ROM_SysCtlClockSet(SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_OSC_MAIN | SYSCTL_XTAL_16MHZ);

	// Initialize the UART and write status.
	ConfigureUART();

	UARTprintf("Timers example\n");

	// Enable LEDs
	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
	ROM_GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, LED_RED|LED_BLUE|LED_GREEN);


	// Enable the peripherals used by this example.
	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);
	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER1);
	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER2);

	// Enable processor interrupts.
	ROM_IntMasterEnable();

	// Configure the two 32-bit periodic timers.
	ROM_TimerConfigure(TIMER0_BASE, TIMER_CFG_PERIODIC);
	ROM_TimerConfigure(TIMER1_BASE, TIMER_CFG_PERIODIC);
	ROM_TimerConfigure(TIMER2_BASE, TIMER_CFG_PERIODIC);
	ROM_TimerLoadSet(TIMER0_BASE, TIMER_A, ROM_SysCtlClockGet());
	ROM_TimerLoadSet(TIMER1_BASE, TIMER_A, ROM_SysCtlClockGet()*2);	// Blue should blink 2 times as much as red
	ROM_TimerLoadSet(TIMER2_BASE, TIMER_A, ROM_SysCtlClockGet()*3);	// Green should blink 3 times as much as red

	// Setup the interrupts for the timer timeouts.
	ROM_IntEnable(INT_TIMER0A);
	ROM_IntEnable(INT_TIMER1A);
	ROM_IntEnable(INT_TIMER2A);
	ROM_TimerIntEnable(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
	ROM_TimerIntEnable(TIMER1_BASE, TIMER_TIMA_TIMEOUT);
	ROM_TimerIntEnable(TIMER2_BASE, TIMER_TIMA_TIMEOUT);

	// Enable the timers.
	ROM_TimerEnable(TIMER0_BASE, TIMER_A);
	ROM_TimerEnable(TIMER1_BASE, TIMER_A);
	ROM_TimerEnable(TIMER2_BASE, TIMER_A);

	// Loop forever while the timers run.
	while(1){}

}

static void cpu_init(void) {
	g_ulSysTickCount = 0;
		// Enable lazy stacking for interrupt handlers.  This allows floating-point
			// instructions to be used within interrupt handlers, but at the expense of extra stack usage.
	ROM_FPULazyStackingEnable();

  // A safety loop in order to interrupt the MCU before setting the clock (wrongly)
  int i;
  for(i=0; i<1000000; i++);

  // Setup for 16MHZ external crystal, use 200MHz PLL and divide by 4 = 50MHz
  MAP_SysCtlClockSet(SYSCTL_SYSDIV_16 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN |
      SYSCTL_XTAL_16MHZ);
}

// Main ----------------------------------------------------------------------------------------------
int main(void){

	// Enable lazy stacking
	ROM_FPULazyStackingEnable();

	// Set the system clock to run at 40Mhz off PLL with external crystal as reference.
	ROM_SysCtlClockSet(SYSCTL_SYSDIV_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);

	// Initialize the UART and write status.
	ConfigureUART();
	UARTprintf("ISL29023 Example\n");

	// Enable LEDs
	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
	ROM_GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, LED_RED|LED_BLUE|LED_GREEN);

	// Enable I2C3
	ConfigureI2C3();

	// Create struct
	tISL29023 islSensHub;

	// Create print variables
	uint32_t printValue[2];

	ISL29023ChangeSettings(ISL29023_COMMANDII_RANGE64k, ISL29023_COMMANDII_RES16, &islSensHub);

	while(1){
		// Get ALS
		ISL29023GetALS(&islSensHub);
		FloatToPrint(islSensHub.alsVal, printValue);
		UARTprintf("ALS: %d.%03d |.| ",printValue[0],printValue[1]);

		// Get IR
		ISL29023GetIR(&islSensHub);
		FloatToPrint(islSensHub.irVal, printValue);
		UARTprintf("IR: %d.%03d\n",printValue[0],printValue[1]);

		// Blink LED
		ROM_GPIOPinWrite(GPIO_PORTF_BASE, LED_RED|LED_GREEN|LED_BLUE, LED_GREEN);
		ROM_SysCtlDelay(ROM_SysCtlClockGet()/3/10);	// Delay for 100ms (1/10s) :: ClockGet()/3 = 1second
		ROM_GPIOPinWrite(GPIO_PORTF_BASE, LED_RED|LED_GREEN|LED_BLUE, 0);

		// Delay for second
		ROM_SysCtlDelay(ROM_SysCtlClockGet()/3);
	}

}

// Initializes C and starts program
void ResetISR(void)
{
    void (**p)() = (void (**)())0x200009ac;

    // Set system clock to 80 MHz
    ROM_SysCtlClockSet(SYSCTL_SYSDIV_2_5|SYSCTL_USE_PLL|SYSCTL_XTAL_16MHZ|SYSCTL_OSC_MAIN);

    // Enable FPU and allow floating point operations in interrupts
    ROM_FPUEnable();
    ROM_FPULazyStackingEnable();

    // Call all the global constructors
    (*p)();

    main();
}

void initTimer0(int interval, gyro *G){
    //
    // Enable lazy stacking for interrupt handlers.  This allows floating-point
    // instructions to be used within interrupt handlers, but at the expense of
    // extra stack usage.
    //
    ROM_FPULazyStackingEnable();
    volatile int tick = 0;

    tick = (ROM_SysCtlClockGet() / 1000) * interval;

    //
    // Enable the GPIO port that is used for the on-board LED.
    //
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);

    //
    // Enable the GPIO pins for the LED (PF1 & PF2).
    //
    ROM_GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_2 | GPIO_PIN_1);


    //
    // Enable the peripherals used by this example.
    //
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);

    //
    // Configure the two 32-bit periodic timers.
    //
    ROM_TimerConfigure(TIMER0_BASE, TIMER_CFG_PERIODIC);
    /// imposta il time_out
    ROM_TimerLoadSet(TIMER0_BASE, TIMER_A, tick);
    G->tick = (float) interval / 1000;
    //
    // Setup the interrupts for the timer timeouts.
    //
    ROM_IntEnable(INT_TIMER0A);
    //ROM_IntEnable(INT_TIMER1A);
    ROM_TimerIntEnable(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
    //ROM_TimerIntEnable(TIMER1_BASE, TIMER_TIMA_TIMEOUT);

    //
    // Enable the timers.
    //
    ROM_TimerEnable(TIMER0_BASE, TIMER_A);
}

//*****************************************************************************
// This example demonstrates how to use serial command parser
//*****************************************************************************
int main(void)
{
    // Enable lazy stacking for interrupt handlers.  This allows floating-point
    // instructions to be used within interrupt handlers, but at the expense of
    // extra stack usage.
    ROM_FPUEnable();
    ROM_FPULazyStackingEnable();

    // Set the clocking to run directly from the crystal.
    ROM_SysCtlClockSet(SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_OSC_MAIN |
                       SYSCTL_XTAL_16MHZ);

    // Enable the GPIO port that is used for the on-board LED.
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
    // Enable the GPIO pins for the LED (PF2).
    ROM_GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_2);
    // Enable the peripherals used by this example.
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
    
    // configure UART
    initUART();
    
    // Enable processor interrupts.
    ROM_IntMasterEnable();

    // Prompt for text to be entered.
    UARTSend((uint8_t *)"\033[2JEnter text: ", 16);

    // Loop forever echoing data through the UART.
    while(1)
    {
      //if(RingBufUsed(&g_tBuffRx) > 4) 
      //{
        //for(int i=4; i>0; i--) 
        //{
          //ROM_UARTCharPutNonBlocking(UART0_BASE,RingBufReadOne(&g_tBuffRx));
        //}
      //}
      UARTParse();
    }
}

int main(void)
{
	  //
	// Enable lazy stacking for interrupt handlers.  This allows floating-point
	// instructions to be used within interrupt handlers, but at the expense of
	// extra stack usage.
	//
	ROM_FPULazyStackingEnable();
    //
    // Setup the system clock to run at 50 Mhz from PLL with crystal reference
    //
    SysCtlClockSet(SYSCTL_SYSDIV_4|SYSCTL_USE_PLL|SYSCTL_XTAL_16MHZ|
                    SYSCTL_OSC_MAIN);
    //
    SetupUART();
    SetupI2C();
    //
    UARTprintf("Starting....\n");
    if(!L3G4200D_CheckDevice())
    {
    	UARTprintf("L3G4200D not detected\n");
    	while(1);
    }
    SetupL3G4200D();
    while(1)
    {
    	short x,y,z;
    	char buff[20];
    	L3G4200D_ReadRaw(&x,&y,&z);
    	ftoa(x,buff);
    	UARTprintf("X: %s,",buff);
    	ftoa(y,buff);
    	UARTprintf("Y: %s,",buff);
    	ftoa(z,buff);
    	UARTprintf("Z: %s\n",buff);
    	DELAY_MS(20);
    }
}

/// \brief Initialize the RTOS Kernel for creating objects.
/// \details Enable double-word stack alignment and initialize the Idle thread.
/// \return Status code that indicates the execution status of the function.
/// \note MUST REMAIN UNCHANGED: \b osKernelInitialize shall be consistent in every CMSIS-RTOS.
osStatus osKernelInitialize (void)
{
	os_KernelEnterCriticalSection();
	
	// Enable double-word stack alignment
	SCB->CCR |= SCB_CCR_STKALIGN_Msk; // Set STKALIGN bit (bit 9) of CCR
	
	// Enable lazy stacking for interrupt handlers.  This allows floating-point
	// instructions to be used within interrupt handlers, but at the expense of
	// extra stack usage.
	//
	ROM_FPULazyStackingEnable();	
	
	os_KernelExitCriticalSection();	
	
	// Initialize the Idle thread
	// Needs SVC exceptions not masked
	if (Init_threadIdle() != 0)
	{
		stop_cpu;
	}
	
	return osOK;
}

//*****************************************************************************
//
// Print "Hello World!" to the display.
//
//*****************************************************************************
int
main(void)
{
    tContext sContext;
    tRectangle sRect;

    //
    // Enable lazy stacking for interrupt handlers.  This allows floating-point
    // instructions to be used within interrupt handlers, but at the expense of
    // extra stack usage.
    //
    ROM_FPULazyStackingEnable();

    //
    // Set the clocking to run directly from the crystal.
    //
    ROM_SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ |
                       SYSCTL_OSC_MAIN);

    //
    // Initialize the UART.
    //
    ConfigureUART();

    UARTprintf("Hello, world!\n");

    //
    // Initialize the display driver.
    //
    CFAL96x64x16Init();

    //
    // Initialize the graphics context.
    //
    GrContextInit(&sContext, &g_sCFAL96x64x16);

    //
    // Fill the top 24 rows of the screen with blue to create the banner.
    //
    sRect.i16XMin = 0;
    sRect.i16YMin = 0;
    sRect.i16XMax = GrContextDpyWidthGet(&sContext) - 1;
    sRect.i16YMax = 23;
    GrContextForegroundSet(&sContext, ClrDarkBlue);
    GrRectFill(&sContext, &sRect);

    //
    // Put a white box around the banner.
    //
    GrContextForegroundSet(&sContext, ClrWhite);
    GrRectDraw(&sContext, &sRect);

    //
    // Put the application name in the middle of the banner.
    //
    GrContextFontSet(&sContext, g_psFontCm12);
    GrStringDrawCentered(&sContext, "hello", -1,
                         GrContextDpyWidthGet(&sContext) / 2, 10, 0);

    //
    // Say hello using the Computer Modern 40 point font.
    //
    GrContextFontSet(&sContext, g_psFontCm12/*g_psFontFixed6x8*/);
    GrStringDrawCentered(&sContext, "Hello World!", -1,
                         GrContextDpyWidthGet(&sContext) / 2,
                         ((GrContextDpyHeightGet(&sContext) - 24) / 2) + 24,
                         0);

    //
    // Flush any cached drawing operations.
    //
    GrFlush(&sContext);

    //
    // We are finished. Hang around doing nothing.
    //
    while(1) {
    }
}

//*****************************************************************************
//
// main() -- Execution Starts Here
//
//*****************************************************************************
int main(void)
{
    // Enable lazy stacking for interrupt handlers.  This allows floating-point
    // instructions to be used within interrupt handlers, but at the expense of
    // extra stack usage.
    ROM_FPULazyStackingEnable ();

    // Set the clocking to run directly from the crystal.
    SysCtlClockSet(
        SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_OSC_MAIN
        | SYSCTL_XTAL_16MHZ);

    // Initialize the I/O Ports (pins)
    PortFunctionInit();

    // Initialize the UART.
    UART_Configure();

    // Initialize the I2C3 port as a slave to the cRIO
    Slave_Configure();

    // Initialize the I2C1 port as master to Gyro
    Gyro_Configure();

    // Say Hello on debug port
    UARTprintf("\nReady.\n");

    int16_t     i16Data = 0;               // holds data read from a gyro register
    uint8_t     ui8Status = 0;             // holds data read from gyro status register
    uint32_t    ui32Iteration = 0;         // iterations counter used to average first N samples
    float       fAvgDrift = 0.0;           // average of 1st N samples, used as an offset to subtract
    int16_t     i16MaxDrift = 0.0;
    int16_t     i16MinDrift = 0.0;
    float       fRate = 0.0;               // rate of change in Z in degs/sec
    float       fAngleDegrees = 0.0;       // running Z orientation in degrees
    uint16_t    ui16AngleScaled = 0.0;     // running Z orientation scaled to LSB = 0.01 degrees (i.e. value = degrees * 100)

    // Configure the L3G4200D Gyro chip
    Gyro_RegWrite(GYRO_CTRL_REG5, 0x80);     //BOOT:1 (reboot memory)
    SysCtlDelay(SysCtlClockGet() / 200);     //delay ~1/200 sec
    Gyro_RegWrite(GYRO_CTRL_REG1, 0x0C);     //DR:00 (100 Hz) PD:1 (enabled) XYZen:100 (Z only enabled)
    SysCtlDelay(SysCtlClockGet() / 200);     //delay ~1/200 sec
    Gyro_RegWrite(GYRO_CTRL_REG4, 0x10);     //FS:01 (500dps)
    SysCtlDelay(SysCtlClockGet() / 200);     //delay ~1/200 sec
    Gyro_RegWrite(GYRO_CTRL_REG5, 0x42);     //FIFO_EN:1 (enable FIFO) OUT_SEL:10 (LPF1+LPF2)
    SysCtlDelay(SysCtlClockGet() / 200);     //delay ~1/200 sec
    Gyro_RegWrite(GYRO_FIFO_CTRL_REG, 0x4F); //FM:010 (Stream Mode), WTM:01111
    SysCtlDelay(SysCtlClockGet() / 200);     //delay ~1/200 sec


    //Main Gyro processing loop
    while (1)
    {
        ui8Status = Gyro_RegRead(GYRO_FIFO_SRC_REG);
        //UARTprintf("FIFO_SRC_REG: 0x%x\n", ui8Status);

        // if Gyro's FIFO is not empty
        if (!(ui8Status & 0x20))
        {
            //report the depth of FIFO:
            //if (DEBUG_MODE) UARTprintf("N:0x%02x : ", ui8Status & 0x1F);

            // Read X,Y, and Z values.
            // NOTE: Found that the FIFo does not empty unless all 3 are read
            // TODO: Create a Gyro_RegRead6() to read all 3 values at once
            // Cast from unsigned16 to signed16
            i16Data = (int16_t) Gyro_RegRead2(GYRO_OUT_X_L);  // read and throw away value
            i16Data = (int16_t) Gyro_RegRead2(GYRO_OUT_Y_L);  // read and throw away value
            i16Data = (int16_t) Gyro_RegRead2(GYRO_OUT_Z_L);  // read and keep value

            //Average the first 100 iterations
            if (ui32Iteration < 500)
            {

                //Note min/max
                if (i16Data < i16MinDrift) i16MinDrift = i16Data;
                if (i16Data > i16MaxDrift) i16MaxDrift = i16Data;

                // compute the running average by taking (average * iteration), which is the sum of all prior iterations,
                // adding this iteration's value, and dividing by (iteration+1).
                fAvgDrift = ( (fAvgDrift * (float)ui32Iteration) + (float)i16Data ) / (float)(ui32Iteration + 1);
                ui32Iteration++;

                if (DEBUG_MODE)
                {
                    UARTprintf("Z: %d :", i16Data);
                    UARTprintf("A: %d :", (int16_t)(fAvgDrift * 100.0));
                    UARTprintf("m: %d :", i16MinDrift);
                    UARTprintf("M: %d\n", i16MaxDrift);
                }
            }
            else
            {
                //remove static drift
                fRate = (float)i16Data - fAvgDrift;
//.........这里部分代码省略.........

//*****************************************************************************
//
// This is the main application entry function.
//
//*****************************************************************************
int
main(void)
{
    uint32_t ui32TxCount;
    uint32_t ui32RxCount;
    //uint32_t ui32Loop;

    //
    // Enable lazy stacking for interrupt handlers.  This allows floating-point
    // instructions to be used within interrupt handlers, but at the expense of
    // extra stack usage.
    //
    ROM_FPULazyStackingEnable();

    //
    // Set the clocking to run from the PLL at 50MHz
    //
#if 1
    ROM_SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN |
                       SYSCTL_XTAL_16MHZ);

    //
    // Configure the required pins for USB operation.
    //
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
    ROM_GPIOPinTypeUSBAnalog(GPIO_PORTD_BASE, GPIO_PIN_5 | GPIO_PIN_4);
       //ROM_SysTickPeriodSet(ROM_SysCtlClockGet() / 100);

    /* This code taken from: http://e2e.ti.com/support/microcontrollers/tiva_arm/f/908/t/311237.aspx
    */
#else       
#include "hw_nvic.h"
       FlashErase(0x00000000);
       ROM_IntMasterDisable();
       ROM_SysTickIntDisable();
       ROM_SysTickDisable();
       uint32_t ui32SysClock;
       ROM_SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_16MHZ);
       ui32SysClock = ROM_SysCtlClockGet();
       ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
       ROM_GPIOPinTypeUSBAnalog(GPIO_PORTD_BASE, GPIO_PIN_4 | GPIO_PIN_5);
       ROM_SysTickPeriodSet(ROM_SysCtlClockGet() / 100);
       HWREG(NVIC_DIS0) = 0xffffffff;
       HWREG(NVIC_DIS1) = 0xffffffff;
       HWREG(NVIC_DIS2) = 0xffffffff;
       HWREG(NVIC_DIS3) = 0xffffffff;
       HWREG(NVIC_DIS4) = 0xffffffff;
       int ui32Addr;
       for(ui32Addr = NVIC_PRI0; ui32Addr <= NVIC_PRI34; ui32Addr+=4)
       {
          HWREG(ui32Addr) = 0;
       }
       HWREG(NVIC_SYS_PRI1) = 0;
       HWREG(NVIC_SYS_PRI2) = 0;
       HWREG(NVIC_SYS_PRI3) = 0;
       ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_USB0);
       ROM_SysCtlPeripheralReset(SYSCTL_PERIPH_USB0);
       ROM_SysCtlUSBPLLEnable();
       ROM_SysCtlDelay(ui32SysClock*2 / 3);
       ROM_IntMasterEnable();
       ROM_UpdateUSB(0);
       while(1)
           {
           }
#endif
#define BOOTLOADER_TEST 0
#if BOOTLOADER_TEST
#include "hw_nvic.h"
    
    //ROM_UpdateUART();
    // May need to do the following here:
    //  0. See if this will cause bootloader to start
    //ROM_FlashErase(0); 
    //ROM_UpdateUSB(0);
    
#define SYSTICKS_PER_SECOND 100
    uint32_t ui32SysClock = ROM_SysCtlClockGet();
    ROM_SysTickPeriodSet(ROM_SysCtlClockGet() / SYSTICKS_PER_SECOND);
    ROM_SysTickIntEnable();
    ROM_SysTickEnable();
    
    //USBDCDTerm(0);
    
    // Disable all interrupts
    ROM_IntMasterDisable();
    ROM_SysTickIntDisable();
    ROM_SysTickDisable();
    HWREG(NVIC_DIS0) = 0xffffffff;
    HWREG(NVIC_DIS1) = 0xffffffff;
    HWREG(NVIC_DIS2) = 0xffffffff;
    HWREG(NVIC_DIS3) = 0xffffffff;
    HWREG(NVIC_DIS4) = 0xffffffff;
       int ui32Addr;
       for(ui32Addr = NVIC_PRI0; ui32Addr <= NVIC_PRI34; ui32Addr+=4)
       {
//.........这里部分代码省略.........

//*****************************************************************************
//
// This is the main example program.  It checks to see that the interrupts are
// processed in the correct order when they have identical priorities,
// increasing priorities, and decreasing priorities.  This exercises interrupt
// preemption and tail chaining.
//
//*****************************************************************************
int
main(void)
{
    uint32_t ui32Error;

    //
    // Enable lazy stacking for interrupt handlers.  This allows floating-point
    // instructions to be used within interrupt handlers, but at the expense of
    // extra stack usage.
    //
    ROM_FPULazyStackingEnable();

    //
    // Set the clocking to run directly from the crystal.
    //
    ROM_SysCtlClockSet(SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_OSC_MAIN |
                       SYSCTL_XTAL_16MHZ);

    //
    // Enable the peripherals used by this example.
    //
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);

    //
    // Initialize the UART.
    //
    ConfigureUART();

    UARTprintf("\033[2JInterrupts\n");

    //
    // Configure the PB0-PB2 to be outputs to indicate entry/exit of one
    // of the interrupt handlers.
    //
    ROM_GPIOPinTypeGPIOOutput(GPIO_PORTE_BASE, GPIO_PIN_1 | GPIO_PIN_2 |
                                               GPIO_PIN_3);
    ROM_GPIOPinWrite(GPIO_PORTE_BASE, GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3, 0);

    //
    // Set up and enable the SysTick timer.  It will be used as a reference
    // for delay loops in the interrupt handlers.  The SysTick timer period
    // will be set up for one second.
    //
    ROM_SysTickPeriodSet(ROM_SysCtlClockGet());
    ROM_SysTickEnable();

    //
    // Reset the error indicator.
    //
    ui32Error = 0;

    //
    // Enable interrupts to the processor.
    //
    ROM_IntMasterEnable();

    //
    // Enable the interrupts.
    //
    ROM_IntEnable(INT_GPIOA);
    ROM_IntEnable(INT_GPIOB);
    ROM_IntEnable(INT_GPIOC);

    //
    // Indicate that the equal interrupt priority test is beginning.
    //
    UARTprintf("\nEqual Priority\n");

    //
    // Set the interrupt priorities so they are all equal.
    //
    ROM_IntPrioritySet(INT_GPIOA, 0x00);
    ROM_IntPrioritySet(INT_GPIOB, 0x00);
    ROM_IntPrioritySet(INT_GPIOC, 0x00);

    //
    // Reset the interrupt flags.
    //
    g_ui32GPIOa = 0;
    g_ui32GPIOb = 0;
    g_ui32GPIOc = 0;
    g_ui32Index = 1;

    //
    // Trigger the interrupt for GPIO C.
    //
    HWREG(NVIC_SW_TRIG) = INT_GPIOC - 16;

    //
    // Put the current interrupt state on the LCD.
    //
    DisplayIntStatus();
//.........这里部分代码省略.........

//*****************************************************************************
//
// Demonstrate the use of the USB stick update example.
//
//*****************************************************************************
int
main(void)
{
    unsigned long ulCount;
    tContext sContext;
    tRectangle sRect;

    //
    // Enable lazy stacking for interrupt handlers.  This allows floating-point
    // instructions to be used within interrupt handlers, but at the expense of
    // extra stack usage.
    //
    ROM_FPULazyStackingEnable();

    //
    // Set the system clock to run at 50MHz from the PLL.
    //
    ROM_SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN |
                       SYSCTL_XTAL_16MHZ);

    //
    // Initialize the display driver.
    //
    CFAL96x64x16Init();

    //
    // Initialize the graphics context.
    //
    GrContextInit(&sContext, &g_sCFAL96x64x16);

    //
    // Fill the top 24 rows of the screen with blue to create the banner.
    //
    sRect.sXMin = 0;
    sRect.sYMin = 0;
    sRect.sXMax = GrContextDpyWidthGet(&sContext) - 1;
    sRect.sYMax = 9;
    GrContextForegroundSet(&sContext, ClrDarkBlue);
    GrRectFill(&sContext, &sRect);

    //
    // Put a white box around the banner.
    //
    GrContextForegroundSet(&sContext, ClrWhite);
    GrRectDraw(&sContext, &sRect);

    //
    // Put the application name in the middle of the banner.
    //
    GrContextFontSet(&sContext, g_pFontFixed6x8);
    GrStringDrawCentered(&sContext, "usb-stick-demo", -1,
                         GrContextDpyWidthGet(&sContext) / 2, 4, 0);

    //
    // Indicate what is happening.
    //
    GrStringDrawCentered(&sContext, "Press the", -1,
                         GrContextDpyWidthGet(&sContext) / 2, 20, 0);
    GrStringDrawCentered(&sContext, "select button to", -1,
                         GrContextDpyWidthGet(&sContext) / 2, 30, 0);
    GrStringDrawCentered(&sContext, "start the USB", -1,
                         GrContextDpyWidthGet(&sContext) / 2, 40, 0);
    GrStringDrawCentered(&sContext, "stick updater.", -1,
                         GrContextDpyWidthGet(&sContext) / 2, 50, 0);

    //
    // Flush any cached drawing operations.
    //
    GrFlush(&sContext);

    //
    // Enable the GPIO module which the select button is attached to.
    //
    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOM);

    //
    // Enable the GPIO pin to read the user button.
    //
    ROM_GPIODirModeSet(GPIO_PORTM_BASE, GPIO_PIN_4, GPIO_DIR_MODE_IN);
    ROM_GPIOPadConfigSet(GPIO_PORTM_BASE, GPIO_PIN_4, GPIO_STRENGTH_2MA,
                         GPIO_PIN_TYPE_STD_WPU);

    //
    // Wait for the pullup to take effect or the next loop will exist too soon.
    //
    SysCtlDelay(1000);

    //
    // Wait until the select button has been pressed for ~40ms (in order to
    // debounce the press).
    //
    ulCount = 0;
    while(1)
    {
        //
//.........这里部分代码省略.........