Diff: camera.cpp

Revision:

0:12ef615ee47f

Child:

1:91c45dda6be1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/camera.cpp	Mon Jan 27 08:54:38 2020 +0000
@@ -0,0 +1,221 @@
+#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;
+}