Quadrifoglio / camera

高木のpixy2

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

camera.cpp@0:12ef615ee47f, 2020-01-27 (annotated)

Committer:
THtakahiro702286
Date:
Mon Jan 27 08:54:38 2020 +0000
Revision:
0:12ef615ee47f
Child:
1:91c45dda6be1
takaki pixy2;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
THtakahiro702286 0:12ef615ee47f 1 #include "camera.h"
THtakahiro702286 0:12ef615ee47f 2
THtakahiro702286 0:12ef615ee47f 3 Cam::Cam(PinName mosi_sda_tx, PinName miso_scl_rx, PinName sclk) :
THtakahiro702286 0:12ef615ee47f 4 spi(mosi_sda_tx, miso_scl_rx, sclk)
THtakahiro702286 0:12ef615ee47f 5
THtakahiro702286 0:12ef615ee47f 6 {
THtakahiro702286 0:12ef615ee47f 7 spi.frequency(4000000);
THtakahiro702286 0:12ef615ee47f 8 sendData[0] = 174;
THtakahiro702286 0:12ef615ee47f 9 sendData[1] = 193;
THtakahiro702286 0:12ef615ee47f 10 sendData[2] = 32;
THtakahiro702286 0:12ef615ee47f 11 sendData[3] = 2;
THtakahiro702286 0:12ef615ee47f 12 sendData[4] = 7;
THtakahiro702286 0:12ef615ee47f 13 sendData[5] = 7;
THtakahiro702286 0:12ef615ee47f 14 for(int i=0; i < 4; i++)
THtakahiro702286 0:12ef615ee47f 15 {
THtakahiro702286 0:12ef615ee47f 16 ball[i] = new lpf(1.0/60,0.1);
THtakahiro702286 0:12ef615ee47f 17 blue[i] = new lpf(1.0/60,0.5);
THtakahiro702286 0:12ef615ee47f 18 yellow[i] = new lpf(1.0/60,0.5);
THtakahiro702286 0:12ef615ee47f 19 }
THtakahiro702286 0:12ef615ee47f 20 thread.start(callback(this, &Cam::SPIloop));
THtakahiro702286 0:12ef615ee47f 21 }
THtakahiro702286 0:12ef615ee47f 22
THtakahiro702286 0:12ef615ee47f 23 void Cam::SPIloop()
THtakahiro702286 0:12ef615ee47f 24 {
THtakahiro702286 0:12ef615ee47f 25 while(1)
THtakahiro702286 0:12ef615ee47f 26 {
THtakahiro702286 0:12ef615ee47f 27 for(int i = 0; i < 6; i++)
THtakahiro702286 0:12ef615ee47f 28 {
THtakahiro702286 0:12ef615ee47f 29 spi.write(sendData[i]);
THtakahiro702286 0:12ef615ee47f 30 }
THtakahiro702286 0:12ef615ee47f 31 for(int i = 0; i < 56; i++)
THtakahiro702286 0:12ef615ee47f 32 {
THtakahiro702286 0:12ef615ee47f 33 pixydata[i] = spi.write(0x00);
THtakahiro702286 0:12ef615ee47f 34 }
THtakahiro702286 0:12ef615ee47f 35 // if(pixydata[9] == 0xc1 && pixydata[10] == 0x21)
THtakahiro702286 0:12ef615ee47f 36 // {
THtakahiro702286 0:12ef615ee47f 37 // data[0] = (pixydata[15] << 8) | pixydata[14]; //名前のシグナル番号
THtakahiro702286 0:12ef615ee47f 38 // data[1] = (pixydata[17] << 8) | pixydata[16]; //対象のx軸の中心
THtakahiro702286 0:12ef615ee47f 39 // data[2] = (pixydata[19] << 8) | pixydata[18]; //対象のy軸の中心
THtakahiro702286 0:12ef615ee47f 40 // data[3] = (pixydata[21] << 8) | pixydata[20]; //横幅
THtakahiro702286 0:12ef615ee47f 41 // data[4] = (pixydata[23] << 8) | pixydata[22]; //縦幅
THtakahiro702286 0:12ef615ee47f 42 // data[5] = (pixydata[25] << 8) | pixydata[24]; //Angle(多分てか絶対0)
THtakahiro702286 0:12ef615ee47f 43 // data[6] = pixydata[26]; //わからん
THtakahiro702286 0:12ef615ee47f 44 // data[7] = pixydata[27]; //映ってる時間
THtakahiro702286 0:12ef615ee47f 45 //
THtakahiro702286 0:12ef615ee47f 46 // data[8] = (pixydata[29] << 8) | pixydata[28];
THtakahiro702286 0:12ef615ee47f 47 // data[9] = (pixydata[31] << 8) | pixydata[30];
THtakahiro702286 0:12ef615ee47f 48 // data[10] = (pixydata[33] << 8) | pixydata[32];
THtakahiro702286 0:12ef615ee47f 49 // data[11] = (pixydata[35] << 8) | pixydata[34];
THtakahiro702286 0:12ef615ee47f 50 // data[12] = (pixydata[37] << 8) | pixydata[36];
THtakahiro702286 0:12ef615ee47f 51 // data[13] = (pixydata[39] << 8) | pixydata[38];
THtakahiro702286 0:12ef615ee47f 52 // data[14] = pixydata[40];
THtakahiro702286 0:12ef615ee47f 53 // data[15] = pixydata[41];
THtakahiro702286 0:12ef615ee47f 54 //
THtakahiro702286 0:12ef615ee47f 55 // data[16] = (pixydata[43] << 8) | pixydata[42];
THtakahiro702286 0:12ef615ee47f 56 // data[17] = (pixydata[45] << 8) | pixydata[44];
THtakahiro702286 0:12ef615ee47f 57 // data[18] = (pixydata[47] << 8) | pixydata[46];
THtakahiro702286 0:12ef615ee47f 58 // data[19] = (pixydata[49] << 8) | pixydata[48];
THtakahiro702286 0:12ef615ee47f 59 // data[20] = (pixydata[51] << 8) | pixydata[50];
THtakahiro702286 0:12ef615ee47f 60 // data[21] = (pixydata[53] << 8) | pixydata[52];
THtakahiro702286 0:12ef615ee47f 61 // data[22] = pixydata[54];
THtakahiro702286 0:12ef615ee47f 62 // data[23] = pixydata[55];
THtakahiro702286 0:12ef615ee47f 63 // }
THtakahiro702286 0:12ef615ee47f 64 if(pixydata[9] == 0xc1 && pixydata[10] == 0x21)
THtakahiro702286 0:12ef615ee47f 65 {
THtakahiro702286 0:12ef615ee47f 66 data[0] = (pixydata[15] << 8) | pixydata[14];
THtakahiro702286 0:12ef615ee47f 67 data[1] = (pixydata[29] << 8) | pixydata[28];
THtakahiro702286 0:12ef615ee47f 68 data[2] = (pixydata[43] << 8) | pixydata[42];
THtakahiro702286 0:12ef615ee47f 69 ballSig = blueSig = yellowSig = false;
THtakahiro702286 0:12ef615ee47f 70 for(int i = 0; i < pixydata[11] / 0xe; i++)
THtakahiro702286 0:12ef615ee47f 71 {
THtakahiro702286 0:12ef615ee47f 72 if(data[i] == 1)
THtakahiro702286 0:12ef615ee47f 73 {
THtakahiro702286 0:12ef615ee47f 74 ballSig = true;
THtakahiro702286 0:12ef615ee47f 75 ballData[0] = (pixydata[14*i + 17] << 8) | pixydata[14*i + 16];//対象のx軸の中心
THtakahiro702286 0:12ef615ee47f 76 ballData[1] = (pixydata[14*i + 19] << 8) | pixydata[14*i + 18];//対象のy軸の中心
THtakahiro702286 0:12ef615ee47f 77 ballData[2] = (pixydata[14*i + 21] << 8) | pixydata[14*i + 20];//横幅
THtakahiro702286 0:12ef615ee47f 78 ballData[3] = (pixydata[14*i + 23] << 8) | pixydata[14*i + 22];//縦幅
THtakahiro702286 0:12ef615ee47f 79 ballData[4] = (pixydata[14*i + 25] << 8) | pixydata[14*i +24];//Angle(多分てか絶対0)
THtakahiro702286 0:12ef615ee47f 80 ballData[5] = pixydata[14*i + 26]; //わからん
THtakahiro702286 0:12ef615ee47f 81 ballData[6] = pixydata[14*i + 27]; //映ってる時間
THtakahiro702286 0:12ef615ee47f 82 }
THtakahiro702286 0:12ef615ee47f 83 if(data[i] == 2)
THtakahiro702286 0:12ef615ee47f 84 {
THtakahiro702286 0:12ef615ee47f 85 blueSig = true;
THtakahiro702286 0:12ef615ee47f 86 blueData[0] = (pixydata[14*i + 17] << 8) | pixydata[14*i + 16];//対象のx軸の中心
THtakahiro702286 0:12ef615ee47f 87 blueData[1] = (pixydata[14*i + 19] << 8) | pixydata[14*i + 18];//対象のy軸の中心
THtakahiro702286 0:12ef615ee47f 88 blueData[2] = (pixydata[14*i + 21] << 8) | pixydata[14*i + 20];//横幅
THtakahiro702286 0:12ef615ee47f 89 blueData[3] = (pixydata[14*i + 23] << 8) | pixydata[14+i * 22];//縦幅
THtakahiro702286 0:12ef615ee47f 90 blueData[4] = (pixydata[14*i + 25] << 8) | pixydata[14*i +24];//Angle(多分てか絶対0)
THtakahiro702286 0:12ef615ee47f 91 blueData[5] = pixydata[14*i + 26]; //わからん
THtakahiro702286 0:12ef615ee47f 92 blueData[6] = pixydata[14*i + 27]; //映ってる時間
THtakahiro702286 0:12ef615ee47f 93 }
THtakahiro702286 0:12ef615ee47f 94 if(data[i] == 3)
THtakahiro702286 0:12ef615ee47f 95 {
THtakahiro702286 0:12ef615ee47f 96 yellowSig = true;
THtakahiro702286 0:12ef615ee47f 97 yellowData[0] = (pixydata[14*i + 17] << 8) | pixydata[14*i + 16];//対象のx軸の中心
THtakahiro702286 0:12ef615ee47f 98 yellowData[1] = (pixydata[14*i + 19] << 8) | pixydata[14*i + 18];//対象のy軸の中心
THtakahiro702286 0:12ef615ee47f 99 yellowData[2] = (pixydata[14*i + 21] << 8) | pixydata[14*i + 20];//横幅
THtakahiro702286 0:12ef615ee47f 100 yellowData[3] = (pixydata[14*i + 23] << 8) | pixydata[14+i * 22];//縦幅
THtakahiro702286 0:12ef615ee47f 101 yellowData[4] = (pixydata[14*i + 25] << 8) | pixydata[14*i +24];//Angle(多分てか絶対0)
THtakahiro702286 0:12ef615ee47f 102 yellowData[5] = pixydata[14*i + 26]; //わからん
THtakahiro702286 0:12ef615ee47f 103 yellowData[6] = pixydata[14*i + 27]; //映ってる時間
THtakahiro702286 0:12ef615ee47f 104 }
THtakahiro702286 0:12ef615ee47f 105 }
THtakahiro702286 0:12ef615ee47f 106 }
THtakahiro702286 0:12ef615ee47f 107 ThisThread::sleep_for(1000.0/40);
THtakahiro702286 0:12ef615ee47f 108 }
THtakahiro702286 0:12ef615ee47f 109 }
THtakahiro702286 0:12ef615ee47f 110
THtakahiro702286 0:12ef615ee47f 111 uint16_t Cam::ballX(){
THtakahiro702286 0:12ef615ee47f 112 return ball[0]->path_value(ballData[0]);
THtakahiro702286 0:12ef615ee47f 113 }
THtakahiro702286 0:12ef615ee47f 114
THtakahiro702286 0:12ef615ee47f 115 uint16_t Cam::ballY(){
THtakahiro702286 0:12ef615ee47f 116 return ball[1]->path_value(ballData[1]);
THtakahiro702286 0:12ef615ee47f 117 }
THtakahiro702286 0:12ef615ee47f 118
THtakahiro702286 0:12ef615ee47f 119 uint16_t Cam::ballWidth(){
THtakahiro702286 0:12ef615ee47f 120 return ball[2]->path_value(ballData[2]);
THtakahiro702286 0:12ef615ee47f 121 }
THtakahiro702286 0:12ef615ee47f 122
THtakahiro702286 0:12ef615ee47f 123 uint16_t Cam::ballHeight(){
THtakahiro702286 0:12ef615ee47f 124 return ball[3]->path_value(ballData[3]);
THtakahiro702286 0:12ef615ee47f 125 }
THtakahiro702286 0:12ef615ee47f 126
THtakahiro702286 0:12ef615ee47f 127 uint16_t Cam::blueX(){
THtakahiro702286 0:12ef615ee47f 128 return blue[0]->path_value(blueData[0]);
THtakahiro702286 0:12ef615ee47f 129 }
THtakahiro702286 0:12ef615ee47f 130
THtakahiro702286 0:12ef615ee47f 131 uint16_t Cam::blueY(){
THtakahiro702286 0:12ef615ee47f 132 return blue[1]->path_value(blueData[1]);
THtakahiro702286 0:12ef615ee47f 133 }
THtakahiro702286 0:12ef615ee47f 134
THtakahiro702286 0:12ef615ee47f 135 uint16_t Cam::blueWidth(){
THtakahiro702286 0:12ef615ee47f 136 return blue[2]->path_value(blueData[2]);
THtakahiro702286 0:12ef615ee47f 137 }
THtakahiro702286 0:12ef615ee47f 138
THtakahiro702286 0:12ef615ee47f 139 uint16_t Cam::blueHeight(){
THtakahiro702286 0:12ef615ee47f 140 return blue[3]->path_value(blueData[3]);
THtakahiro702286 0:12ef615ee47f 141 }
THtakahiro702286 0:12ef615ee47f 142
THtakahiro702286 0:12ef615ee47f 143 uint16_t Cam::yellowX(){
THtakahiro702286 0:12ef615ee47f 144 return yellow[0]->path_value(yellowData[0]);
THtakahiro702286 0:12ef615ee47f 145 }
THtakahiro702286 0:12ef615ee47f 146
THtakahiro702286 0:12ef615ee47f 147 uint16_t Cam::yellowY(){
THtakahiro702286 0:12ef615ee47f 148 return yellow[1]->path_value(yellowData[1]);
THtakahiro702286 0:12ef615ee47f 149 }
THtakahiro702286 0:12ef615ee47f 150
THtakahiro702286 0:12ef615ee47f 151 uint16_t Cam::yellowWidth(){
THtakahiro702286 0:12ef615ee47f 152 return yellow[2]->path_value(yellowData[2]);
THtakahiro702286 0:12ef615ee47f 153 }
THtakahiro702286 0:12ef615ee47f 154
THtakahiro702286 0:12ef615ee47f 155 uint16_t Cam::yellowHeight(){
THtakahiro702286 0:12ef615ee47f 156 return yellow[3]->path_value(yellowData[3]);
THtakahiro702286 0:12ef615ee47f 157 }
THtakahiro702286 0:12ef615ee47f 158
THtakahiro702286 0:12ef615ee47f 159 uint16_t Cam::getData(int i){
THtakahiro702286 0:12ef615ee47f 160 return data[i];
THtakahiro702286 0:12ef615ee47f 161 }
THtakahiro702286 0:12ef615ee47f 162
THtakahiro702286 0:12ef615ee47f 163 uint16_t Cam::rawBallCenterX(){
THtakahiro702286 0:12ef615ee47f 164 return ballData[0];
THtakahiro702286 0:12ef615ee47f 165 }
THtakahiro702286 0:12ef615ee47f 166
THtakahiro702286 0:12ef615ee47f 167 uint16_t Cam::rawBallCenterY(){
THtakahiro702286 0:12ef615ee47f 168 return ballData[1];
THtakahiro702286 0:12ef615ee47f 169 }
THtakahiro702286 0:12ef615ee47f 170
THtakahiro702286 0:12ef615ee47f 171 uint16_t Cam::rawBallWidth(){
THtakahiro702286 0:12ef615ee47f 172 return ballData[2];
THtakahiro702286 0:12ef615ee47f 173 }
THtakahiro702286 0:12ef615ee47f 174
THtakahiro702286 0:12ef615ee47f 175 uint16_t Cam::rawBallHeight(){
THtakahiro702286 0:12ef615ee47f 176 return ballData[3];
THtakahiro702286 0:12ef615ee47f 177 }
THtakahiro702286 0:12ef615ee47f 178
THtakahiro702286 0:12ef615ee47f 179 uint16_t Cam::rawBlueCenterX(){
THtakahiro702286 0:12ef615ee47f 180 return blueData[0];
THtakahiro702286 0:12ef615ee47f 181 }
THtakahiro702286 0:12ef615ee47f 182
THtakahiro702286 0:12ef615ee47f 183 uint16_t Cam::rawBlueCenterY(){
THtakahiro702286 0:12ef615ee47f 184 return blueData[1];
THtakahiro702286 0:12ef615ee47f 185 }
THtakahiro702286 0:12ef615ee47f 186
THtakahiro702286 0:12ef615ee47f 187 uint16_t Cam::rawBlueWidth(){
THtakahiro702286 0:12ef615ee47f 188 return blueData[2];
THtakahiro702286 0:12ef615ee47f 189 }
THtakahiro702286 0:12ef615ee47f 190
THtakahiro702286 0:12ef615ee47f 191 uint16_t Cam::rawBlueHeight(){
THtakahiro702286 0:12ef615ee47f 192 return blueData[3];
THtakahiro702286 0:12ef615ee47f 193 }
THtakahiro702286 0:12ef615ee47f 194
THtakahiro702286 0:12ef615ee47f 195 uint16_t Cam::rawYellowCenterX(){
THtakahiro702286 0:12ef615ee47f 196 return yellowData[0];
THtakahiro702286 0:12ef615ee47f 197 }
THtakahiro702286 0:12ef615ee47f 198
THtakahiro702286 0:12ef615ee47f 199 uint16_t Cam::rawYellowCenterY(){
THtakahiro702286 0:12ef615ee47f 200 return yellowData[1];
THtakahiro702286 0:12ef615ee47f 201 }
THtakahiro702286 0:12ef615ee47f 202
THtakahiro702286 0:12ef615ee47f 203 uint16_t Cam::rawYellowWidth(){
THtakahiro702286 0:12ef615ee47f 204 return yellowData[2];
THtakahiro702286 0:12ef615ee47f 205 }
THtakahiro702286 0:12ef615ee47f 206
THtakahiro702286 0:12ef615ee47f 207 uint16_t Cam::rawYellowHeight(){
THtakahiro702286 0:12ef615ee47f 208 return yellowData[3];
THtakahiro702286 0:12ef615ee47f 209 }
THtakahiro702286 0:12ef615ee47f 210
THtakahiro702286 0:12ef615ee47f 211 bool Cam::ballSignal(){
THtakahiro702286 0:12ef615ee47f 212 return ballSig;
THtakahiro702286 0:12ef615ee47f 213 }
THtakahiro702286 0:12ef615ee47f 214
THtakahiro702286 0:12ef615ee47f 215 bool Cam::blueSignal(){
THtakahiro702286 0:12ef615ee47f 216 return blueSig;
THtakahiro702286 0:12ef615ee47f 217 }
THtakahiro702286 0:12ef615ee47f 218
THtakahiro702286 0:12ef615ee47f 219 bool Cam::yellowSignal(){
THtakahiro702286 0:12ef615ee47f 220 return yellowSig;
THtakahiro702286 0:12ef615ee47f 221 }