File content as of revision 0:817f868ca9c1:

#include "GestureSensor.h"

GestureSensor::GestureSensor(PinName sda, PinName scl)
{
    wait_ms(15);
    i2cp = new I2C(sda, scl);
    
    restart();
}

void GestureSensor::restart()
{
    command(RESET);
    wait_ms(15);
    
    // Setting up LED Power to full
    wreg(HW_KEY,HW_KEY_VAL0);
    wreg(PS_LED21,0xAA);
    wreg(PS_LED3,0x0A);
    wreg(PARAM_WR, ALS_IR_TASK + ALS_VIS_TASK + PS1_TASK + PS2_TASK + PS3_TASK);
    
    command(PARAM_SET + (CHLIST & 0x1F));
    
    wreg(INT_CFG,0);
    wreg(IRQ_ENABLE,0);
    wreg(IRQ_MODE1,0);
    wreg(IRQ_MODE2,0);
}

void GestureSensor::command(char code)
{
    int num;
    
    num = rreg(RESPONSE,1);
    while(num!=0)
    {
        wreg(COMMAND,NOP);
        num = rreg(RESPONSE,1);
    }
    do{
        wreg(COMMAND,code);
        if(code==RESET) break;
        num = rreg(RESPONSE,1);
    }while(num==0);
}

//read a register
char GestureSensor::rreg(char address, int data)
{
    char array[1];
    char array2[1];
    
    array[0] = address;
    i2cp->write((IR_ADDRESS << 1) & 0xFE, array, data);
    wait_ms(1);
    
    i2cp->read((IR_ADDRESS << 1) | 0x01, array2, data);
    wait_ms(1);
    
    return array2[0];
}

//write to register
void GestureSensor::wreg(char address, char data) 
{  
    char array[2];

    array[0] = address;
    array[1] = data;
    i2cp->write((IR_ADDRESS << 1) & 0xFE, array, 2);
    wait_ms(1); 
}

void GestureSensor::baseline(int time, int num)
{
    wait(time);
    sampleleft = sample_left(num);
    sampletop = sample_top(num);
    sampleright = sample_right(num);
}

//Check Left LED Samples
int GestureSensor::sample_left(int num)
{
    int stack = 0;
    
    command(PSALS_FORCE);
    
    for(int n=num; n>0; n=n-1)
    {
        firstHalf = rreg(PS1_DATA0,1);
        secondHalf = rreg(PS1_DATA1,1);
        stack = stack + (secondHalf * 256) + firstHalf;
    }
    left = stack / num;
    
    if(left > sampleleft)
        left = left - sampleleft;
    else
        left = 0;
    
    return left;
}

//Check Top LED Samples
int GestureSensor::sample_top(int num)
{
    int stack = 0;
    
    command(PSALS_FORCE);
    
    for(int n=num; n>0; n=n-1)
    {
        firstHalf = rreg(PS2_DATA0,1);
        secondHalf = rreg(PS2_DATA1,1);
        stack = stack + (secondHalf * 256) + firstHalf;
    }
    top = stack / num;
    
    if(top > sampletop)
        top = top - sampletop;
    else
        top = 0;
    
    return top;
}

//Check Right LED samples
int GestureSensor::sample_right(int num)
{
    int stack = 0;
    
    command(PSALS_FORCE);
    
    for(int n=num; n>0; n=n-1)
    {
        firstHalf = rreg(PS3_DATA0,1);
        secondHalf = rreg(PS3_DATA1,1);
        stack = stack + (secondHalf * 256) + firstHalf;
    }
    right = stack / num;
    
    if(right > sampleright)
        right = right - sampleright;
    else
        right = 0;
    
    return right;
}