#include "HardwareInfo.c"
#include "JMLib.c"
#include <SetLCDBack.h>
#include <SetDisplayString.h>
#include <GetRightButton.h>
#include <SetInBeep.h>
#include <SetAHRS.h>
#include <GetAHRS.h>
#include <SetDisplayVar.h>

int main(void)
{
    EV5_Init();
    int Roll = 0;
    int Pitch = 0;
    int Yaw = 0;
    int ButR = 0;
    SetLCDBack(ON);
    SetDisplayString(1, "P1", YELLOW, BLACK);
    SetDisplayString(2, "EULER", YELLOW, BLACK);
    SetDisplayString(4, "ROLL", YELLOW, BLACK);
    SetDisplayString(5, "PITCH", YELLOW, BLACK);
    SetDisplayString(6, "YAW", YELLOW, BLACK);
    while (1)
    {
        ButR = GetRightButton();
        if ( ButR==1 )
        {
            SetInBeep(ON);
            SetAHRS(_P1_, 1);
        }
        SetInBeep(OFF);
        Roll = GetAHRS(_P1_, 1, 0);
        Pitch = GetAHRS(_P1_, 2, 0);
        Yaw = GetAHRS(_P1_, 3, 0);
        SetDisplayVar(4, Roll, YELLOW, BLACK);
        SetDisplayVar(5, Pitch, YELLOW, BLACK);
        SetDisplayVar(6, Roll, YELLOW, BLACK);
    }
    while(1);
}

