高木のpixy2
Dependents: 00_yotsuba 10_motor-test 00_yotsuba 200_yotsuba_21
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 }
Generated on Fri Jul 15 2022 02:47:18 by
1.7.2