File content as of revision 13:4f2181174071:

#include "mbed.h"

//******************************************************************
// All variables defined below
//******************************************************************

//communication
DigitalOut myled(LED1);
Serial pc(USBTX, USBRX);
Serial keyOut(p13, p14);
I2C camera1(p9, p10);

//initial camera data
int IRsensorAddress = 0xB0;
int slaveAddress;
char data_buf[16];
char s;
int i;

//point variables
int point1x = 0;
int point1y = 0;
int point2x = 0;
int point2y = 0;
int point3x = 0;
int point3y = 0;
int point4x = 0;
int point4y = 0;

//sensitivity
//Level 5: p0 = 0x96, p1 = 0xFE, p2 = 0xFE, p3 = 0x05
//highest sensitivity to more accurately detect points
int sen0 = 0x96;
int sen1 = 0xFE;
int sen2 = 0xFE;
int sen3 = 0x00;

//previous point values
int prevX = 1023;
int prevY = 1023;

//matrices of x and y coordinates from the first camera
int onex[4];
int oney[4];

//matrices of x and y coordinates from prev point
int prevx[4];
int prevy[4];

//movement
int deadzone = 5;

//counts for clicks
int clickDeadzone = 10;
int fingerDownCount = 0;
int fingerUpCount = 0;
int minLeftClickDur = 4;
int minRightClickDur = 10;
int delayForClick = 2;
int instabilityLimit = 3;
int instabilityCount = 0;



//******************************************************************
// All methods defined below
//******************************************************************

//takes in values for the movement in the x and y direction 
//also can indicate whether you want to "click"
//NOTE: hard coded wait of 0.1
void mouseCommand(char buttons, char x, char y) {
  keyOut.putc(0xFD);
  keyOut.putc(0x00);
  keyOut.putc(0x03);
  keyOut.putc(buttons);
  keyOut.putc(x);
  keyOut.putc(y);
  keyOut.putc(0x00);
  keyOut.putc(0x00);
  keyOut.putc(0x00);
  
  //delay for pushing data
  wait(0.1); //how large does this need to be?
}


//moves mouse on screen from one finger input
//param
// current point (currx, curry)
// previous point (prevx, prevy)
void oneFingerResponse(int currx, int curry, int prevx, int prevy){
    //look at delta btwn prev val and current
    //TODO: moving average
    if((prevx != 1023 || prevy != 1023) && (currx != 1023 && curry != 1023)){
        int diffX = currx - prevx;
        int diffY = -1*(curry - prevy);
    
        //fix diffX
        if(diffX > deadzone){
            diffX -= deadzone;
        } else if (diffX < -1*deadzone){
            diffX += deadzone;
        } else{
            diffX = 0;
        }
        //fix diffY
        if(diffY > deadzone){
            diffY -= deadzone;
        } else if (diffX < -1*deadzone){
            diffY += deadzone;
        } else{
            diffY = 0;
        }
        
        //move x and y
        mouseCommand(0, (char) diffX, (char) diffY);
    }
}


//writes two bytes to the camera
void write2bytes(char data1, char data2){
    char out[2];
    out[0] = data1;
    out[1] = data2;
    camera1.write(slaveAddress, out, 2);
    wait(0.01);   
}



// Initialize WiiMote Camera
void initCamera(void){
    write2bytes(0x30, 0x01); 
    write2bytes(0x00, 0x02); 
    write2bytes(0x00, 0x00); 
    write2bytes(0x71, 0x01); 
    write2bytes(0x00, sen0); 
    write2bytes(0x07, 0x00); 
    write2bytes(sen1, 0x1A);
    write2bytes(sen2, sen3); 
    write2bytes(0x33, 0x03); 
    write2bytes(0x30, 0x08);
    //wait(0.1);

}


//update counts for click 
void updateClickState(int currx, int curry, int prevx, int prevy){
    bool xStable = false;
    bool yStable = false; 

    //TODO: update to handle cases where just one is 1023
    if(currx != 1023 || curry != 1023){
        //check x stability 
        if( currx > prevx ){
            if(currx-prevx < clickDeadzone){
                //barely moved in x dir
                xStable = true;                 
            } 
        } else if( currx < prevx) {
            if(prevx-currx < clickDeadzone){
                //barely moved in x dir
                xStable = true;
            }
        } else {
            //no movement in x
            xStable = true;
        }
        
        //check y stability 
        if( curry > prevy ){
            if(curry-prevy < clickDeadzone){
                //barely moved in y dir
                yStable = true;                 
            } 
        } else if( curry < prevy) {
            if(prevy-curry < clickDeadzone){
                //barely moved in y dir
                yStable = true;
            }
        } else {
            //no movement in y
            yStable = true;
        }
        
        //update finger down count
        if(xStable && yStable){
            //both are stable
            fingerDownCount++;
            //pc.printf("finger down \t");
            
        } else{
            //unstable, increase instability count
            instabilityCount++;
            if(instabilityCount > instabilityLimit){
                //too many instable points, reset to zero
                fingerDownCount = 0;
                instabilityCount = 0;
            }
        }
        
    } else {
        //both are up, increment up count
        fingerUpCount++;
        if(fingerUpCount >= delayForClick && fingerDownCount >= minLeftClickDur){
            //finger was down for long enough to be a click
            //finger was lifted for long enough to indicate click
            if(fingerDownCount >= minRightClickDur){
                //initiate right click
                mouseCommand(0x02, 0 , 0);
                
                //TODO: remove. for debugging purposes only
                pc.printf("#########RIGHT mouse click \t");
            } else {
                //initiate left click
                mouseCommand(0x01, 0 , 0);
                
                //TODO: remove. for debugging purposes only
                pc.printf("********LEFT mouse click \t");
            }
            
            //reset counters
            fingerUpCount = 0;
            fingerDownCount = 0;
        
        }        
    } 
    
        
}


//get data from camera one 
//populates onex and oney with values depending on the measured points
//NOTE: 1023 means nothing was detected
void readCameraData(void){
        
    //request data from camera 
    char out[1];
    out[0] = 0x36;   
    camera1.write(slaveAddress, out, 1);
    //wait(0.2); //do we need this?
    
    //get data from camera
    camera1.read(slaveAddress, data_buf, 16);
        
    //POINT 1
    //get data
    point1x = data_buf[1];
    point1y = data_buf[2];
    s = data_buf[3];
    //load x,y    
    onex[0] = point1x + ((s & 0x30) << 4);
    oney[0] = point1y + ((s & 0xC0) << 2);
    
    
    //>>>>>>>>>>>>>>>>>Begin unfinished code for moving 

    oneFingerResponse(onex[0], oney[0], prevX, prevY);    
    updateClickState(onex[0], oney[0], prevX, prevY);
    
    
    //update prev values
    prevX = onex[0];
    prevY = oney[0];
    
    
    //<<<<<<<<<<<<<<<<End unfinished code for moving averages
    
    //POINT 2
    //get data
    point2x = data_buf[4];
    point2y = data_buf[5];
    s = data_buf[6];
    //load x,y
    onex[1] = point2x + ((s & 0x30) << 4);
    oney[1] = point2y + ((s & 0xC0) << 2);
      
    //POINT 3
    //get data
    point3x = data_buf[7];
    point3y = data_buf[8];
    s = data_buf[9];
    //load x,y
    onex[2] = point3x + ((s & 0x30) << 4);
    oney[2] = point3y + ((s & 0xC0) << 2);
    
    //POINT 4
    //get data
    point4x = data_buf[10];
    point4y = data_buf[11];
    s = data_buf[12];
    //load x,y
    onex[3] = point4x + ((s & 0x30) << 4);
    oney[3] = point4y + ((s & 0xC0) << 2);
    
}

//print to serial monitor the coordinates of the points stored in
//the passed x and y arrays
void printCamData(int xcor[4], int ycor[4]){
    for(int i = 0; i<4; i++){
        int x = xcor[i];
        int y = ycor[i];
        //determine what to print
        //x coordinate
        pc.printf(" %d,", x);
        
        //y coordinate
        pc.printf(" %d\t", y);     
    }
    
    //new line and delay
    pc.printf("\n");      
    //wait(0.01);
}




//entrance to the code
int main() {
    myled = 0;
    //slaveAddress = IRsensorAddress >> 1;
    slaveAddress = IRsensorAddress;
    initCamera();
    
    //update baud rate
    pc.baud(115200);
    
    //loop to search for new info using the camera    
    while(1) {
        
        //wait 
        //wait(0.04);
        
        //toggle test LED 
        myled = 1 - myled;
        
        //get the camera data
        readCameraData();
        
        //print points
        printCamData(onex, oney);  
        
        //uncomment below to test infinite print
        //keyOut.putc(0x41);

        
        //uncomment below to infinitely move mouse in a square
//        double delay = 0.1;
//        int change = 75;
//        mouseCommand(0,0, (char) -1*change);
//        wait(delay);
//        mouseCommand(0,(char) -1*change,0);
//        wait(delay);  
//        mouseCommand(0,0, (char) change);
//        wait(delay);
//        mouseCommand(0,(char) change,0);
//        wait(delay);  
        
        
    }
}