Delay(100);

    GPIO_SetBits(LED_POWER);

    Delay(500);

    GPIO_ResetBits(LED_POWER);

    init_spi();

    ssd1306_Init();

    ssd1306_block_write();

    ssd1306_DrawString("Hello World!", 0, 10);

  //  ssd1306_block_write();

    while(1)

   {  

        //ssd1306_block_write();

        // Process sensor inputs [TODO: 5hz?]

        process();

        // Run state machine [TODO: 50hz?]

        machine(); 

        // probably just passed the actual port

        // TODO: Grab buttonpresses with interrupts

        uint8_t sw_btn = GPIO_ReadInputDataBit(SW_BTN);

        uint8_t sw_up = GPIO_ReadInputDataBit(SW_UP);

        uint8_t sw_down = GPIO_ReadInputDataBit(SW_DOWN);

        uint8_t sw_left = GPIO_ReadInputDataBit(SW_LEFT);

        uint8_t sw_right = GPIO_ReadInputDataBit(SW_RIGHT);

        if(!sw_btn) {

            GPIO_ToggleBits(LED_STAT);

        }

        GPIO_SetBits(LED_POWER);

        Delay(50);

        GPIO_ResetBits(LED_POWER);

        Delay(50);

    }

}

int32_t temp = 0;

int32_t setpoint = 0;

int32_t p = 1;