File content as of revision 1:91c45dda6be1:

#include "camera.h"

Cam::Cam(PinName mosi_sda_tx, PinName miso_scl_rx, PinName sclk) :
spi(mosi_sda_tx, miso_scl_rx, sclk)

{
    spi.frequency(4000000);
    sendData[0] = 174;
    sendData[1] = 193;
    sendData[2] = 32;
    sendData[3] = 2;
    sendData[4] = 7;
    sendData[5] = 7;
    for(int i=0; i < 4; i++)
    {
        ball[i] = new lpf(1.0/60,0.1);
        blue[i] = new lpf(1.0/60,0.5);
        yellow[i] = new lpf(1.0/60,0.5);
    }
    thread.start(callback(this, &Cam::SPIloop));
}

void Cam::SPIloop()
{
    while(1)
    {
        for(int i = 0; i < 6; i++) 
        {
            spi.write(sendData[i]);
        }
        for(int i = 0; i < 56; i++) 
        {
            pixydata[i] = spi.write(0x00);
        }
//        if(pixydata[9] == 0xc1 && pixydata[10] == 0x21)
//        {
//            data[0] = (pixydata[15] << 8) | pixydata[14];  //名前のシグナル番号
//            data[1] = (pixydata[17] << 8) | pixydata[16];  //対象のx軸の中心
//            data[2] = (pixydata[19] << 8) | pixydata[18];  //対象のy軸の中心
//            data[3] = (pixydata[21] << 8) | pixydata[20];  //横幅
//            data[4] = (pixydata[23] << 8) | pixydata[22];  //縦幅
//            data[5] = (pixydata[25] << 8) | pixydata[24];  //Angle(多分てか絶対0)
//            data[6] = pixydata[26];                        //わからん
//            data[7] = pixydata[27];                        //映ってる時間
//            
//            data[8] = (pixydata[29] << 8) | pixydata[28];
//            data[9] = (pixydata[31] << 8) | pixydata[30];
//            data[10] = (pixydata[33] << 8) | pixydata[32];
//            data[11] = (pixydata[35] << 8) | pixydata[34];
//            data[12] = (pixydata[37] << 8) | pixydata[36];
//            data[13] = (pixydata[39] << 8) | pixydata[38];
//            data[14] = pixydata[40];
//            data[15] = pixydata[41];
//            
//            data[16] = (pixydata[43] << 8) | pixydata[42];
//            data[17] = (pixydata[45] << 8) | pixydata[44];
//            data[18] = (pixydata[47] << 8) | pixydata[46];
//            data[19] = (pixydata[49] << 8) | pixydata[48];
//            data[20] = (pixydata[51] << 8) | pixydata[50];
//            data[21] = (pixydata[53] << 8) | pixydata[52];
//            data[22] = pixydata[54];
//            data[23] = pixydata[55];
//        }
        if(pixydata[9] == 0xc1 && pixydata[10] == 0x21)
        {
            data[0] = (pixydata[15] << 8) | pixydata[14];
            data[1] = (pixydata[29] << 8) | pixydata[28];
            data[2] = (pixydata[43] << 8) | pixydata[42];
            ballSig = blueSig = yellowSig = false;
            for(int i = 0; i < pixydata[11] / 0xe; i++)
            {
                if(data[i] == 1)
                {
                    ballSig = true;
                    ballData[0] = (pixydata[14*i + 17] << 8) | pixydata[14*i + 16];//対象のx軸の中心
                    ballData[1] = (pixydata[14*i + 19] << 8) | pixydata[14*i + 18];//対象のy軸の中心
                    ballData[2] = (pixydata[14*i + 21] << 8) | pixydata[14*i + 20];//横幅
                    ballData[3] = (pixydata[14*i + 23] << 8) | pixydata[14*i + 22];//縦幅
                    ballData[4] = (pixydata[14*i + 25] << 8) | pixydata[14*i +24];//Angle(多分てか絶対0)
                    ballData[5] = pixydata[14*i + 26];                      //わからん
                    ballData[6] = pixydata[14*i + 27];                      //映ってる時間
                }
                if(data[i] == 2)
                {
                    blueSig = true;
                    blueData[0] = (pixydata[14*i + 17] << 8) | pixydata[14*i + 16];//対象のx軸の中心
                    blueData[1] = (pixydata[14*i + 19] << 8) | pixydata[14*i + 18];//対象のy軸の中心
                    blueData[2] = (pixydata[14*i + 21] << 8) | pixydata[14*i + 20];//横幅
                    blueData[3] = (pixydata[14*i + 23] << 8) | pixydata[14+i * 22];//縦幅
                    blueData[4] = (pixydata[14*i + 25] << 8) | pixydata[14*i +24];//Angle(多分てか絶対0)
                    blueData[5] = pixydata[14*i + 26];                      //わからん
                    blueData[6] = pixydata[14*i + 27];                      //映ってる時間
                }
                if(data[i] == 3)
                {
                    yellowSig = true;
                    yellowData[0] = (pixydata[14*i + 17] << 8) | pixydata[14*i + 16];//対象のx軸の中心
                    yellowData[1] = (pixydata[14*i + 19] << 8) | pixydata[14*i + 18];//対象のy軸の中心
                    yellowData[2] = (pixydata[14*i + 21] << 8) | pixydata[14*i + 20];//横幅
                    yellowData[3] = (pixydata[14*i + 23] << 8) | pixydata[14+i * 22];//縦幅
                    yellowData[4] = (pixydata[14*i + 25] << 8) | pixydata[14*i +24];//Angle(多分てか絶対0)
                    yellowData[5] = pixydata[14*i + 26];                      //わからん
                    yellowData[6] = pixydata[14*i + 27];                      //映ってる時間
                }
            }
        }
        ThisThread::sleep_for(1000.0/40);
    }
}

uint16_t Cam::ballX(){
    return ball[0]->path_value(ballData[0]);
}

uint16_t Cam::ballY(){
    return ball[1]->path_value(ballData[1]);
}

uint16_t Cam::ballWidth(){
    return ball[2]->path_value(ballData[2]);
}

uint16_t Cam::ballHeight(){
    return ball[3]->path_value(ballData[3]);
}

uint16_t Cam::blueX(){
    return blue[0]->path_value(blueData[0]);
}

uint16_t Cam::blueY(){
    return blue[1]->path_value(blueData[1]);
}

uint16_t Cam::blueWidth(){
    return blue[2]->path_value(blueData[2]);
}

uint16_t Cam::blueHeight(){
    return blue[3]->path_value(blueData[3]);
}

uint16_t Cam::yellowX(){
    return yellow[0]->path_value(yellowData[0]);
}

uint16_t Cam::yellowY(){
    return yellow[1]->path_value(yellowData[1]);
}

uint16_t Cam::yellowWidth(){
    return yellow[2]->path_value(yellowData[2]);
}

uint16_t Cam::yellowHeight(){
    return yellow[3]->path_value(yellowData[3]);
}

uint16_t Cam::getData(int i){
    return data[i];
}

//uint16_t Cam::rawBallCenterX(){
//    return ballData[0];
//}
//
//uint16_t Cam::rawBallCenterY(){
//    return ballData[1];
//}
//
//uint16_t Cam::rawBallWidth(){
//    return ballData[2];
//}
//
//uint16_t Cam::rawBallHeight(){
//    return ballData[3];
//}
//
//uint16_t Cam::rawBlueCenterX(){
//    return blueData[0];
//}
//
//uint16_t Cam::rawBlueCenterY(){
//    return blueData[1];
//}
//
//uint16_t Cam::rawBlueWidth(){
//    return blueData[2];
//}
//
//uint16_t Cam::rawBlueHeight(){
//    return blueData[3];
//}
//
//uint16_t Cam::rawYellowCenterX(){
//    return yellowData[0];
//}
//
//uint16_t Cam::rawYellowCenterY(){
//    return yellowData[1];
//}
//
//uint16_t Cam::rawYellowWidth(){
//    return yellowData[2];
//}
//
//uint16_t Cam::rawYellowHeight(){
//    return yellowData[3];
//}

bool Cam::ballSignal(){
    return ballSig;
}

bool Cam::blueSignal(){
    return blueSig;
}

bool Cam::yellowSignal(){
    return yellowSig;
}