Diff: GestureSensor.cpp

Revision:

0:817f868ca9c1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GestureSensor.cpp	Fri Mar 13 20:53:11 2015 +0000
@@ -0,0 +1,150 @@
+
+#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;
+}