/*
Program ID  : HTGyroCmpsTrvl.nxc
Create date : 5-1-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
3. Compare with the Compass sensor
*/
#define GYRO_PORT            IN_1     //Connect Gyro sensor to this port
#define COMPASS_PORT         IN_2     //Connect Compass sensor to this port
#define COMPASS              0x02
#define COMPASS_2DEG         0x42

#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);
 }
 
/* ********************************************************
 * Get Compass value (0-360)
 * ******************************************************** */
 int Get_Compass()
 {
    byte I2C_req_buf[];
    ArrayBuild(I2C_req_buf, COMPASS, COMPASS_2DEG);

    byte I2C_get_buf[];
    ArrayInit(I2C_get_buf,0,2);

    byte x_status, nbytes=2;
    int x_deg=0;

    while(LowspeedCheckStatus(COMPASS_PORT) == STAT_COMM_PENDING);
    x_status = LowspeedWrite(COMPASS_PORT, 2, I2C_req_buf);
    if (x_status >= 0)
    {
       while(LowspeedStatus(COMPASS_PORT, nbytes) == STAT_COMM_PENDING);
       x_status = LowspeedRead(COMPASS_PORT, nbytes, I2C_get_buf);
       if (x_status >= 0) x_deg = I2C_get_buf[0] * 2 + I2C_get_buf[1];
    }
    return (x_deg);
 }
 
/* *******************************************************
 * sensor initialization
 * ******************************************************* */
 void Sensor_Init()
 {
    SetSensorHTGyro(GYRO_PORT);
    SetSensorLowspeed(COMPASS_PORT);
 }

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

     Sensor_Init();
     gyroZero    = Get_GyroZero();
     compassZero = Get_Compass();

     ClearScreen();
     TextOut(0, LCD_LINE1, "Measure angular " );
     TextOut(0, LCD_LINE2, "displacement by:" );
     TextOut(0, LCD_LINE4, "   Gyro: " );
     TextOut(0, LCD_LINE5, "Compass: " );
     TextOut(0, LCD_LINE6, "Diff(C-G)" );
     TextOut(0, LCD_LINE8, "Reset Quit  Sync" );
     
     currTick   = CurrentTick();
     sampleTick = currTick;
     dispTick   = currTick;
     do
     {
        currTick = CurrentTick();
        //Reset - Set current position as starting point
        if (ButtonPressed(BTNLEFT, true))
        {
           gyroMove    = 0;
           compassZero = Get_Compass();
           sampleTick  = currTick;
           dispTick    = currTick;
        }
        
        //Sync - Set Gyro current angular displacement as Compass abs position
        if (ButtonPressed(BTNRIGHT, true))
        {
           gyroMove    = Get_Compass() - compassZero;
           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;
           compassMove= Get_Compass() - compassZero;
           sampleTick = currTick;
           lastVal    = currVal;
        }
        
        //Display angular displacement value of Gyro and Compass
        if ((currTick - dispTick) >= WATCH_TRAVEL_PERIOD)
        {
           TextOut(54,LCD_LINE4, "       " );
           TextOut(54,LCD_LINE5, "       " );
           TextOut(54,LCD_LINE6, "       " );
           NumOut(54, LCD_LINE4, gyroMove / (SCALE * WEIGHT * 1000));
           NumOut(54, LCD_LINE5, compassMove);
           NumOut(54, LCD_LINE6, compassMove - (gyroMove / (SCALE * WEIGHT * 1000)));
           dispTick = currTick;
        }

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