/*
Program ID  : HTGyro-getTrvl.nxc
Create date : 4-30-2010
Author      : CH, Chen (Taiwan)
Description :
Hitechnic Gyro sensor's features testing:
1. Using Ralph Hempel's weighted average method to calculate zero point value
2. Measure angular displacement via Gyro sensor
*/
#define GYRO_PORT            IN_1     //Connect Gyro sensor to this port
#define SCALE                64
#define WEIGHT               128
#define ZERO_SAMPLE_PERIOD   4
#define WATCH_ZERO_PERIOD    256
#define TRAVEL_SAMPLE_PERIOD 32
#define WATCH_TRAVEL_PERIOD  512

/* ********************************************************
 * Calculate Gyro Zero Point value (Weighted average method)
 * ******************************************************** */
 long Get_GyroZero()
 {
    long zeroVal=0, oldVal=0;
    long xtick=0, sampleTick=0, dispTick=0;
    
    ClearScreen();
    TextOut(0, LCD_LINE1, "Watch/Set Gyro's" );
    TextOut(0, LCD_LINE2, "Zero Point Value" );
    TextOut(0, LCD_LINE4, "Weighted average" );
    TextOut(0, LCD_LINE8, "             Set" );
    do
    {
       xtick = CurrentTick();
       if ((xtick - sampleTick) >= ZERO_SAMPLE_PERIOD)
       {
          zeroVal = zeroVal - (zeroVal/WEIGHT) + SensorRaw(GYRO_PORT) * SCALE;
          sampleTick = xtick;
       }
       if ((xtick - dispTick) >= WATCH_ZERO_PERIOD)
       {
          NumOut( 0, LCD_LINE5, zeroVal);
          TextOut(0, LCD_LINE6, "               " );
          NumOut( 0, LCD_LINE6, 1.0000 * zeroVal / (SCALE * WEIGHT));
          dispTick = xtick;
          oldVal   = zeroVal;
       }
    } until (ButtonPressed(BTNRIGHT, true));
    return (zeroVal);
 }

/* *******************************************************
 * sensor initialization
 * ******************************************************* */
 void Sensor_Init()
 {
    SetSensorHTGyro(GYRO_PORT);
 }

/* ********************************************************
 * main()
 * ******************************************************** */
 task main()
 {
     string msg;
     long currTick=0, sampleTick=0, dispTick=0;
     long gyroZero, gyroMove=0, currVal=0, lastVal=0;

     Sensor_Init();
     gyroZero = Get_GyroZero();

     ClearScreen();
     TextOut(0, LCD_LINE1, "Measure rotating" );
     TextOut(0, LCD_LINE2, "degrees via Gyro" );
     TextOut(0, LCD_LINE4, "0-Speed: " );
     NumOut(54, LCD_LINE4, (1.0000 * gyroZero / (SCALE * WEIGHT)));
     TextOut(0, LCD_LINE5, "Degrees: " );
     TextOut(0, LCD_LINE6, "  Speed: " );
     TextOut(0, LCD_LINE8, "Reset Quit      " );
     
     currTick   = CurrentTick();
     sampleTick = currTick;
     dispTick   = currTick;
     do
     {
        currTick = CurrentTick();
        //Reset
        if (ButtonPressed(BTNLEFT, true))
        {
           gyroMove   = 0;
           sampleTick = currTick;
           dispTick   = currTick;
        }
        
        //Calculate and sum total angular displacement
        if ((currTick - sampleTick) >= TRAVEL_SAMPLE_PERIOD)
        {
           currVal    = SensorRaw(GYRO_PORT) * SCALE * WEIGHT - gyroZero;
           gyroMove  += (currTick - sampleTick) * (currVal + lastVal) / 2;
           sampleTick = currTick;
           lastVal    = currVal;
        }
        
        //Display angular displacement and current speed
        if ((currTick - dispTick) >= WATCH_TRAVEL_PERIOD)
        {
           TextOut(54,LCD_LINE5, "       ");
           TextOut(54,LCD_LINE6, "       ");
           NumOut(54, LCD_LINE5, 0.001 * (gyroMove / (SCALE * WEIGHT)));
           NumOut(54, LCD_LINE6, (1.0000 * currVal/(SCALE * WEIGHT)));
           dispTick = currTick;
        }

     } until (ButtonPressed(BTNCENTER, true));
     Stop(true);
 }