File content as of revision 1:b91387b1d393:

/*
 * mbed library for Pixy2 Camera
 * Copyright (c) 2019 Jordan DUCHÊNE
 */

#include "mbed.h"
#include "Pixy2_Library.h"




Pixy2_Library::Pixy2_Library (PinName p_sda, PinName p_scl, int address)   // Class Constructor
{
    composant = new I2C(p_sda,p_scl);  //sda, sc1
    composant ->frequency(100000);
    I2Caddress = address;
}


void Pixy2_Library::Pixy2_WhiteLED(bool STATE)   // White LEDs BuiltIN ON/OFF methode
{

    unsigned char readdata[21];
    unsigned char lampeON[6]= {0xae,0xc1,22,2,1,0};
    unsigned char lampeOFF[6]= {0xae,0xc1,22,2,0,0};
    if(STATE == true) {
        do {
            composant->write(I2Caddress,(char*)lampeON,6);
            composant->read(I2Caddress,(char*)readdata,10);
            wait(0.01);
        } while(readdata[0] != 175 && readdata[1] != 193);
    } else {
        do {
            composant->write(I2Caddress,(char*)lampeOFF,6);
            composant->read(I2Caddress,(char*)readdata,10);
            wait(0.01);
        } while(readdata[0] != 175 && readdata[1] != 193);
    }

}
void Pixy2_Library::Pixy2_SetServo(int PAN, int TILT)   // Head Servo Crontrol methode
{
    int lowP, hightP, lowT, hightT;
    hightP = PAN >> 8 ;
    lowP = PAN & 0x0f;
    hightT = TILT >> 8 ;
    lowT = TILT & 0x0f;
    unsigned char readdata[21];
    unsigned char SetServ[8]= {0xae,0xc1,18,4,lowP,hightP,lowT,hightP};
    do {
        composant->write(I2Caddress,(char*)SetServ,6);
        composant->read(I2Caddress,(char*)readdata,10);
        wait(0.01);
    } while(readdata[0] != 175 && readdata[1] != 193);
}


void Pixy2_Library::Pixy2_RGBLED(bool STATE)
{

    unsigned char readdata[21];
    unsigned char lampeON[6]= {0xae,0xc1,22,2,0,1};
    unsigned char lampeOFF[6]= {0xae,0xc1,22,2,0,0};
    if(STATE == true) {
        do {
            composant->write(I2Caddress,(char*)lampeON,6);
            composant->read(I2Caddress,(char*)readdata,10);
            wait(0.01);
        } while(readdata[0] != 175 && readdata[1] != 193);
    } else {
        do {
            composant->write(I2Caddress,(char*)lampeOFF,6);
            composant->read(I2Caddress,(char*)readdata,10);
            wait(0.01);
        } while(readdata[0] != 175 && readdata[1] != 193);
    }

}

int Pixy2_Library::Pixy2_GetBlock(char DATA, int BLOCK_NUMBER)
{
    int DataSize = BLOCK_NUMBER * 14 + 6;
    unsigned char writedata[6]= {0xae,0xc1,32,2,255,20};
    unsigned char readdata[DataSize + 1];
    do {
        composant->write(I2Caddress,(char*)writedata,6);
        composant->read(I2Caddress,(char*)readdata,DataSize);
        wait(0.1);
    } while(readdata[0] != 175 && readdata[1] != 193);
    switch(DATA) {
        case 's':
            if(readdata[DataSize - 13]<2) return readdata[7]*256+readdata[DataSize - 14];
            if(readdata[DataSize - 13]>=2) return -2;
            break;
        case 'x':
            if(readdata[DataSize - 11]<2) return readdata[DataSize - 11]*256+readdata[DataSize - 12];
            if(readdata[DataSize - 11]>=2) return -2;
            break;
        case 'y':
            if(readdata[DataSize - 9]<2) return readdata[DataSize - 9]*256+readdata[DataSize - 10];
            if(readdata[DataSize - 9]>=2) return -2;
            break;
        case 'w':
            if(readdata[DataSize - 7]<2) return readdata[DataSize - 7]*256+readdata[DataSize - 8];
            if(readdata[DataSize - 7]>=2) return -2;
            break;
        case 'h':
            if(readdata[DataSize - 5]<2) return readdata[DataSize - 5]*256+readdata[DataSize - 6];
            if(readdata[DataSize - 5]>=2) return -2;
            break;
        case 'a':
            if(readdata[DataSize - 3]<2) return readdata[DataSize - 3]*256+readdata[DataSize - 4];
            if(readdata[DataSize - 3]>=2) return -2;
            break;
        case 'i':
            if(readdata[DataSize - 1]<2) return readdata[DataSize - 1]*256+readdata[DataSize - 2];                                //////////////////////// 1
            if(readdata[DataSize - 1]>=2) return -2;
            break;
    }
}