高木のpixy2

Dependents:   00_yotsuba 10_motor-test 00_yotsuba 200_yotsuba_21

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers camera.cpp Source File

camera.cpp

00001 #include "camera.h"
00002 
00003 Cam::Cam(PinName mosi_sda_tx, PinName miso_scl_rx, PinName sclk) :
00004 spi(mosi_sda_tx, miso_scl_rx, sclk)
00005 
00006 {
00007     spi.frequency(4000000);
00008     sendData[0] = 174;
00009     sendData[1] = 193;
00010     sendData[2] = 32;
00011     sendData[3] = 2;
00012     sendData[4] = 7;
00013     sendData[5] = 7;
00014     for(int i=0; i < 4; i++)
00015     {
00016         ball[i] = new lpf(1.0/60,0.1);
00017         blue[i] = new lpf(1.0/60,0.5);
00018         yellow[i] = new lpf(1.0/60,0.5);
00019     }
00020     thread.start(callback(this, &Cam::SPIloop));
00021 }
00022 
00023 void Cam::SPIloop()
00024 {
00025     while(1)
00026     {
00027         for(int i = 0; i < 6; i++) 
00028         {
00029             spi.write(sendData[i]);
00030         }
00031         for(int i = 0; i < 56; i++) 
00032         {
00033             pixydata[i] = spi.write(0x00);
00034         }
00035 //        if(pixydata[9] == 0xc1 && pixydata[10] == 0x21)
00036 //        {
00037 //            data[0] = (pixydata[15] << 8) | pixydata[14];  //名前のシグナル番号
00038 //            data[1] = (pixydata[17] << 8) | pixydata[16];  //対象のx軸の中心
00039 //            data[2] = (pixydata[19] << 8) | pixydata[18];  //対象のy軸の中心
00040 //            data[3] = (pixydata[21] << 8) | pixydata[20];  //横幅
00041 //            data[4] = (pixydata[23] << 8) | pixydata[22];  //縦幅
00042 //            data[5] = (pixydata[25] << 8) | pixydata[24];  //Angle(多分てか絶対0)
00043 //            data[6] = pixydata[26];                        //わからん
00044 //            data[7] = pixydata[27];                        //映ってる時間
00045 //            
00046 //            data[8] = (pixydata[29] << 8) | pixydata[28];
00047 //            data[9] = (pixydata[31] << 8) | pixydata[30];
00048 //            data[10] = (pixydata[33] << 8) | pixydata[32];
00049 //            data[11] = (pixydata[35] << 8) | pixydata[34];
00050 //            data[12] = (pixydata[37] << 8) | pixydata[36];
00051 //            data[13] = (pixydata[39] << 8) | pixydata[38];
00052 //            data[14] = pixydata[40];
00053 //            data[15] = pixydata[41];
00054 //            
00055 //            data[16] = (pixydata[43] << 8) | pixydata[42];
00056 //            data[17] = (pixydata[45] << 8) | pixydata[44];
00057 //            data[18] = (pixydata[47] << 8) | pixydata[46];
00058 //            data[19] = (pixydata[49] << 8) | pixydata[48];
00059 //            data[20] = (pixydata[51] << 8) | pixydata[50];
00060 //            data[21] = (pixydata[53] << 8) | pixydata[52];
00061 //            data[22] = pixydata[54];
00062 //            data[23] = pixydata[55];
00063 //        }
00064         if(pixydata[9] == 0xc1 && pixydata[10] == 0x21)
00065         {
00066             data[0] = (pixydata[15] << 8) | pixydata[14];
00067             data[1] = (pixydata[29] << 8) | pixydata[28];
00068             data[2] = (pixydata[43] << 8) | pixydata[42];
00069             ballSig = blueSig = yellowSig = false;
00070             for(int i = 0; i < pixydata[11] / 0xe; i++)
00071             {
00072                 if(data[i] == 1)
00073                 {
00074                     ballSig = true;
00075                     ballData[0] = (pixydata[14*i + 17] << 8) | pixydata[14*i + 16];//対象のx軸の中心
00076                     ballData[1] = (pixydata[14*i + 19] << 8) | pixydata[14*i + 18];//対象のy軸の中心
00077                     ballData[2] = (pixydata[14*i + 21] << 8) | pixydata[14*i + 20];//横幅
00078                     ballData[3] = (pixydata[14*i + 23] << 8) | pixydata[14*i + 22];//縦幅
00079                     ballData[4] = (pixydata[14*i + 25] << 8) | pixydata[14*i +24];//Angle(多分てか絶対0)
00080                     ballData[5] = pixydata[14*i + 26];                      //わからん
00081                     ballData[6] = pixydata[14*i + 27];                      //映ってる時間
00082                 }
00083                 if(data[i] == 2)
00084                 {
00085                     blueSig = true;
00086                     blueData[0] = (pixydata[14*i + 17] << 8) | pixydata[14*i + 16];//対象のx軸の中心
00087                     blueData[1] = (pixydata[14*i + 19] << 8) | pixydata[14*i + 18];//対象のy軸の中心
00088                     blueData[2] = (pixydata[14*i + 21] << 8) | pixydata[14*i + 20];//横幅
00089                     blueData[3] = (pixydata[14*i + 23] << 8) | pixydata[14+i * 22];//縦幅
00090                     blueData[4] = (pixydata[14*i + 25] << 8) | pixydata[14*i +24];//Angle(多分てか絶対0)
00091                     blueData[5] = pixydata[14*i + 26];                      //わからん
00092                     blueData[6] = pixydata[14*i + 27];                      //映ってる時間
00093                 }
00094                 if(data[i] == 3)
00095                 {
00096                     yellowSig = true;
00097                     yellowData[0] = (pixydata[14*i + 17] << 8) | pixydata[14*i + 16];//対象のx軸の中心
00098                     yellowData[1] = (pixydata[14*i + 19] << 8) | pixydata[14*i + 18];//対象のy軸の中心
00099                     yellowData[2] = (pixydata[14*i + 21] << 8) | pixydata[14*i + 20];//横幅
00100                     yellowData[3] = (pixydata[14*i + 23] << 8) | pixydata[14+i * 22];//縦幅
00101                     yellowData[4] = (pixydata[14*i + 25] << 8) | pixydata[14*i +24];//Angle(多分てか絶対0)
00102                     yellowData[5] = pixydata[14*i + 26];                      //わからん
00103                     yellowData[6] = pixydata[14*i + 27];                      //映ってる時間
00104                 }
00105             }
00106         }
00107         ThisThread::sleep_for(1000.0/40);
00108     }
00109 }
00110 
00111 uint16_t Cam::ballX(){
00112     return ball[0]->path_value(ballData[0]);
00113 }
00114 
00115 uint16_t Cam::ballY(){
00116     return ball[1]->path_value(ballData[1]);
00117 }
00118 
00119 uint16_t Cam::ballWidth(){
00120     return ball[2]->path_value(ballData[2]);
00121 }
00122 
00123 uint16_t Cam::ballHeight(){
00124     return ball[3]->path_value(ballData[3]);
00125 }
00126 
00127 uint16_t Cam::blueX(){
00128     return blue[0]->path_value(blueData[0]);
00129 }
00130 
00131 uint16_t Cam::blueY(){
00132     return blue[1]->path_value(blueData[1]);
00133 }
00134 
00135 uint16_t Cam::blueWidth(){
00136     return blue[2]->path_value(blueData[2]);
00137 }
00138 
00139 uint16_t Cam::blueHeight(){
00140     return blue[3]->path_value(blueData[3]);
00141 }
00142 
00143 uint16_t Cam::yellowX(){
00144     return yellow[0]->path_value(yellowData[0]);
00145 }
00146 
00147 uint16_t Cam::yellowY(){
00148     return yellow[1]->path_value(yellowData[1]);
00149 }
00150 
00151 uint16_t Cam::yellowWidth(){
00152     return yellow[2]->path_value(yellowData[2]);
00153 }
00154 
00155 uint16_t Cam::yellowHeight(){
00156     return yellow[3]->path_value(yellowData[3]);
00157 }
00158 
00159 uint16_t Cam::getData(int i){
00160     return data[i];
00161 }
00162 
00163 //uint16_t Cam::rawBallCenterX(){
00164 //    return ballData[0];
00165 //}
00166 //
00167 //uint16_t Cam::rawBallCenterY(){
00168 //    return ballData[1];
00169 //}
00170 //
00171 //uint16_t Cam::rawBallWidth(){
00172 //    return ballData[2];
00173 //}
00174 //
00175 //uint16_t Cam::rawBallHeight(){
00176 //    return ballData[3];
00177 //}
00178 //
00179 //uint16_t Cam::rawBlueCenterX(){
00180 //    return blueData[0];
00181 //}
00182 //
00183 //uint16_t Cam::rawBlueCenterY(){
00184 //    return blueData[1];
00185 //}
00186 //
00187 //uint16_t Cam::rawBlueWidth(){
00188 //    return blueData[2];
00189 //}
00190 //
00191 //uint16_t Cam::rawBlueHeight(){
00192 //    return blueData[3];
00193 //}
00194 //
00195 //uint16_t Cam::rawYellowCenterX(){
00196 //    return yellowData[0];
00197 //}
00198 //
00199 //uint16_t Cam::rawYellowCenterY(){
00200 //    return yellowData[1];
00201 //}
00202 //
00203 //uint16_t Cam::rawYellowWidth(){
00204 //    return yellowData[2];
00205 //}
00206 //
00207 //uint16_t Cam::rawYellowHeight(){
00208 //    return yellowData[3];
00209 //}
00210 
00211 bool Cam::ballSignal(){
00212     return ballSig;
00213 }
00214 
00215 bool Cam::blueSignal(){
00216     return blueSig;
00217 }
00218 
00219 bool Cam::yellowSignal(){
00220     return yellowSig;
00221 }