void carTest()
{
	comhelper.Open();
	comhelper.Set(9600);
	comhelper.Write(str, 2);
	//cout << 1 << endl;
	str[0] = 'L';
	comhelper.Write(str, 2);
	//mycar.run();
}
Ejemplo n.º 2
0
static int execute_set_tx_frequency_arm(char *szPort, UINT8 carrier_on, UINT16 tx_frequency, int tx_power)
{
    ComHelper SerialPort;

    if (!SerialPort.OpenPort(szPort))
    {
        printf("Open %s port Failed\n", szPort);
        return 0;
    }
    int chan_num = tx_frequency - 2400;
    UINT8 hci_set_tx_frequency_arm[] = {0x01, 0x014, 0xfc, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    UINT8 hci_set_tx_frequency_arm_cmd_complete_event[] = {0x04, 0x0e, 0x04, 0x01, 0x014, 0xfc, 0x00};
    hci_set_tx_frequency_arm[4] = (carrier_on == 0) ? 1 : 0;
    hci_set_tx_frequency_arm[5] = (carrier_on == 1) ? chan_num : 2;
    hci_set_tx_frequency_arm[6] = 0;                // unmodulated
    hci_set_tx_frequency_arm[7] = 0;                // modulation type  
    hci_set_tx_frequency_arm[8] = (carrier_on == 1) ? 8 : 0;
    hci_set_tx_frequency_arm[9] = tx_power;

    printf ("Sending HCI Command:\n");
    HexDump(hci_set_tx_frequency_arm, sizeof(hci_set_tx_frequency_arm));

    // write HCI reset
    SerialPort.Write(hci_set_tx_frequency_arm, sizeof(hci_set_tx_frequency_arm));

    // read HCI response header
    DWORD dwRead = SerialPort.Read((LPBYTE)&in_buffer[0], 3);

    // read HCI response payload
    if (dwRead == 3 && in_buffer[2] > 0)
        dwRead += SerialPort.Read((LPBYTE)&in_buffer[3], in_buffer[2]);

    printf ("Received HCI Event:\n");
    HexDump(in_buffer, dwRead);
    if (dwRead == sizeof(hci_set_tx_frequency_arm_cmd_complete_event))
    {
        if (memcmp(in_buffer, hci_set_tx_frequency_arm_cmd_complete_event, dwRead) == 0)
        {
            printf ("Success\n");
            return 1;
        }
    }
    return FALSE;
}
Ejemplo n.º 3
0
static int execute_le_transmitter_test(char *szPort, UINT8 chan_number, UINT8 length, UINT8 pattern)
{
    ComHelper SerialPort;

    if (!SerialPort.OpenPort(szPort))
	{
        printf("Open %s port Failed\n", szPort);
		return 0;
	}

    UINT8 hci_le_transmitter_test[] = {0x01, 0x01E, 0x20, 0x03, 0x00, 0x00, 0x00};
    UINT8 hci_le_transmitter_test_cmd_complete_event[] = {0x04, 0x0e, 0x04, 0x01, 0x01E, 0x20, 0x00};
    hci_le_transmitter_test[4] = chan_number;
    hci_le_transmitter_test[5] = length;
    hci_le_transmitter_test[6] = pattern;
    printf ("Sending HCI Command:\n");
    HexDump(hci_le_transmitter_test, sizeof(hci_le_transmitter_test));

    // write HCI reset
    SerialPort.Write(hci_le_transmitter_test, sizeof(hci_le_transmitter_test));

    // read HCI response header
    DWORD dwRead = SerialPort.Read((LPBYTE)&in_buffer[0], 3);

    // read HCI response payload
    if (dwRead == 3 && in_buffer[2] > 0)
        dwRead += SerialPort.Read((LPBYTE)&in_buffer[3], in_buffer[2]);

    printf ("Received HCI Event:\n");
    HexDump(in_buffer, dwRead);
    if (dwRead == sizeof(hci_le_transmitter_test_cmd_complete_event))
    {
        if (memcmp(in_buffer, hci_le_transmitter_test_cmd_complete_event, dwRead) == 0)
        {
            printf("Success\n");
			printf("LE Transmitter Test running, to stop execute le_test_end\n");
			return 1;
        }
    }
    return FALSE;
}
Ejemplo n.º 4
0
static int execute_le_test_end(char *szPort)
{
    ComHelper SerialPort;

    if (!SerialPort.OpenPort(szPort))
	{
        printf("Open %s port Failed\n", szPort);
		return 0;
	}

    UINT8 hci_le_test_end[] = {0x01, 0x1f, 0x20, 0x00};
    UINT8 hci_le_test_end_cmd_complete_event[] = {0x04, 0x0e, 0x06, 0x01, 0x1f, 0x20, 0x00};

    printf ("Sending HCI Command:\n");
    HexDump(hci_le_test_end, sizeof(hci_le_test_end));

    // write HCI reset
    SerialPort.Write(hci_le_test_end, sizeof(hci_le_test_end));

    // read HCI response header
    DWORD dwRead = SerialPort.Read((LPBYTE)&in_buffer[0], 3);

    // read HCI response payload
    if (dwRead == 3 && in_buffer[2] > 0)
        dwRead += SerialPort.Read((LPBYTE)&in_buffer[3], in_buffer[2]);

    printf ("Received HCI Event:\n");
    HexDump(in_buffer, dwRead);
    if ((dwRead > sizeof(hci_le_test_end_cmd_complete_event))
     && (memcmp(in_buffer, hci_le_test_end_cmd_complete_event, sizeof(hci_le_test_end_cmd_complete_event)) == 0))
    {
        printf("Success num_packets_received %d\n", in_buffer[7] + (in_buffer[8] << 8));
        return TRUE;
    }
    else
    {
        return FALSE;
    }
}
Ejemplo n.º 5
0
static int execute_reset(char *szPort)
{
    ComHelper SerialPort;

    if (!SerialPort.OpenPort(szPort))
	{
        printf("Open %s port Failed\n", szPort);
		return 0;
	}

    UINT8 hci_reset[] = {0x01, 0x03, 0x0c, 0x00};
    UINT8 hci_reset_cmd_complete_event[] = {0x04, 0x0e, 0x04, 0x01, 0x03, 0x0c, 0x00};

    printf ("Sending HCI Command:\n");
    HexDump(hci_reset, sizeof(hci_reset));

    // write HCI reset
    SerialPort.Write(hci_reset, sizeof(hci_reset));

    // read HCI response header
    DWORD dwRead = SerialPort.Read((LPBYTE)&in_buffer[0], 3);

    // read HCI response payload
    if (dwRead == 3 && in_buffer[2] > 0)
        dwRead += SerialPort.Read((LPBYTE)&in_buffer[3], in_buffer[2]);

    printf ("Received HCI Event:\n");
    HexDump(in_buffer, dwRead);
    if (dwRead == sizeof(hci_reset_cmd_complete_event))
    {
        if (memcmp(in_buffer, hci_reset_cmd_complete_event, dwRead) == 0)
        {
            printf ("Success\n");
            return 1;
        }
    }
    return 0;
}
int main()
{
	//carTest();
	//str[0] = 'A';
	//if (comhelper.Write(str, 2) == false)
	//{
		//cout << "error" << endl;
	//}
	
	
	//str[0] = 'R';
	comhelper.Open();
	comhelper.Set(9600);
	comhelper.Write(str, 2);
	
	vis[0] = 1;
	//mycar.run();
	VideoCapture cap; //定义一个摄像头捕捉的类对象
	Rect trackwindow1;     //第一个标志
	Rect trackwindow2;     //第二个标志
	int hsize = 16;
	float hranges[] = { 0, 180 };//hranges在后面的计算直方图函数中要用到
	const float* phranges1 = hranges;
	const float* phranges2 = hranges;
	Mat frame, hsv, hue1, hue2, mask1, mask2, hist1, hist2, histimg1 = Mat::zeros(200, 320, CV_8UC3), histimg2 = Mat::zeros(200, 320, CV_8UC3), backproj1, backproj2;

	
	cap.open(0);//直接调用成员函数打开摄像头

	if (!cap.isOpened())
	{
		cout << "Error" << endl;
		return -1;
	}

	//namedWindow("hist1ogram", 0);
	namedWindow("CamShift Demo", 0);
	
	namedWindow("Rotated", 0);
	//透视变换

	while (1)
	{
		//cout << "test" << endl;
		cap >> frame;
		if (frame.empty())
		{
			break;
		}
		if (type == 0)
		{
			change(frame);
		}
		if (type == 1)
		{
			break;
		}

		char c = (char)waitKey(30);
		if (c == 'p')              //按下p表示开始惊醒透视变换选点,选点方向左上右上左下右下
		{
			type = 0;
		}
		imshow("CamShift Demo", frame);
	}
	imshow("Rotated", rotated);


	//二值化
	src = &IplImage(rotated);
	gray = cvCreateImage(cvGetSize(src), IPL_DEPTH_8U, 1);
	cvCvtColor(src, gray, CV_BGR2GRAY);
	binary = cvCreateImage(cvGetSize(gray), IPL_DEPTH_8U, 1);
	cvThreshold(gray, binary, 128, 1, CV_THRESH_BINARY);
	cvNamedWindow("Binary Image", 0);
	cvShowImage("Binary Image", binary);
	int n = 0;
	cvCreateTrackbar("二值化阈值", "Binary Image", &n, 254, call_back);
	call_back(1);
	cvWaitKey(0);

	//细化
	dst = cvCreateImage(cvGetSize(binary), binary->depth, binary->nChannels);
	thinImage(binary, dst);
	cvNamedWindow("细化", 0);
	cvShowImage("细化", dst);
	cvWaitKey(0);

	//霍夫变换
	cvNamedWindow("霍夫变换", 0);
	storage = cvCreateMemStorage();
	lines = cvHoughLines2(dst, storage, CV_HOUGH_PROBABILISTIC, 1, CV_PI / 180, 30, 30, 40);
	img1 = cvCreateImage(cvGetSize(dst), 8, 1);
	cvSetZero(img1);
	for (int i = 0; i < lines->total; i++)
	{
		CvPoint* line = (CvPoint*)cvGetSeqElem(lines, i);
		cvLine(img1, line[0], line[1], CvScalar(255));
		sp.add(line[0], line[1],i);
		//sp.initial();
	}
	cvShowImage("霍夫变换", img1);
	int Thresh = 20;
	int MinLength = 40;
	int Terval = 40;
	cvCreateTrackbar("阈值", "霍夫变换", &Thresh, 100, hough_call_back0);
	cvCreateTrackbar("最短线段长度", "霍夫变换", &MinLength, 100, hough_call_back1);
	cvCreateTrackbar("线段间隔最大值", "霍夫变换", &Terval, 100, hough_call_back2);
	cvWaitKey(0);
	cvDestroyWindow("Binary Image");
	cvDestroyWindow("细化");
	cvReleaseImage(&gray);
	cvReleaseImageHeader(&binary);
	//cvReleaseImage(&dst);
		

	sp.initial();
	
	sp.test();
	cout << "Please select the start point: ";
	cin >> startPoint;
	if (startPoint == 1)
	{
		sp.reverse();
	}


	//追踪
	setMouseCallback("Rotated", onMouse, 0);//消息响应机制
	createTrackbar("Vmin", "Rotated", &vmin, 256, 0);//createTrackbar函数的功能是在对应的窗口创建滑动条,滑动条Vmin,vmin表示滑动条的值,最大为256
	createTrackbar("Vmax", "Rotated", &vmax, 256, 0);//最后一个参数为0代表没有调用滑动拖动的响应函数
	createTrackbar("Smin", "Rotated", &smin, 256, 0);//vmin,vmax,smin初始值分别为10,256,30

	
	//bool paused = false;

	while (1)
	{
		//carTest();
		if (type == 10 && mark2 == true)
		{
			Turn t = getTurn();
			if (t == Left)
			{
				str[0] = 'L';
				comhelper.Write(str, 2);
				//mycar.turnl();
				cout << "Left" << endl;
			}
			else if (t == Right)
			{
				str[0] = 'R';
				comhelper.Write(str, 2);
				//mycar.turnr();
				cout << "Right" << endl;
			}
			else if (t == Ahead)
			{
				str[0] = 'A';
				comhelper.Write(str, 2);
				//mycar.run();
				cout << "Ahead" << endl;
			}
			else if (t == Stop)
			{
				for (int i = 0; i < 100; i++)
				{
					str[0] = 'S';
					comhelper.Write(str, 2);
					//mycar.stop();
					cout << "Stop" << endl;
				}

				
			}
		}


		
			cap >> frame;//从摄像头抓取一帧图像并输出到frame中
			if (frame.empty())
				break;
		

		warpPerspective(frame, image, warpMatrix, rotated.size(), INTER_LINEAR, BORDER_CONSTANT);
		

		//frame.copyTo(image);
		frame.copyTo(image2);

		
			cvtColor(image, hsv, CV_BGR2HSV);//将rgb摄像头帧转化成hsv空间的
			if (trackObject)//trackObject初始化为0,或者按完键盘的'c'键后也为0,当鼠标单击松开后为-1
			{
				int _vmin = vmin, _vmax = vmax;

				inRange(hsv, Scalar(0, smin, MIN(_vmin, _vmax)),
					Scalar(180, 256, MAX(_vmin, _vmax)), mask1);
				int ch[] = { 0, 0 };
				hue1.create(hsv.size(), hsv.depth());
				hue2.create(hsv.size(), hsv.depth());
				mixChannels(&hsv, 1, &hue1, 1, ch, 1);
				mixChannels(&hsv, 1, &hue2, 1, ch, 1);

				if (trackObject < 0)//鼠标选择区域松开后,该函数内部又将其赋值1
				{
					//ROI指感兴趣的区域
					Mat roi(hue1, selection1), maskroi(mask1, selection1);//mask1保存的hsv的最小值

					calcHist(&roi, 1, 0, maskroi, hist1, 1, &hsize, &phranges1);//将roi的0通道计算直方图并通过mask1放入hist1中,hsize为每一维直方图的大小
					normalize(hist1, hist1, 0, 255, CV_MINMAX);//将hist1矩阵进行数组范围归一化,都归一化到0~255

					trackwindow1 = selection1;
					trackObject = 1;//只要鼠标选完区域松开后,且没有按键盘清0键'c',则trackObject一直保持为1,因此该if函数只能执行一次,除非重新选择跟踪区域

					histimg1 = Scalar::all(0);//清0
					int binW = histimg1.cols / hsize;  
					Mat buf(1, hsize, CV_8UC3);
					for (int i = 0; i < hsize; i++)
						buf.at<Vec3b>(i) = Vec3b(saturate_cast<uchar>(i*180. / hsize), 255, 255);
					cvtColor(buf, buf, CV_HSV2BGR);//将hsv转换成bgr

					for (int i = 0; i < hsize; i++)
					{
						int val = saturate_cast<int>(hist1.at<float>(i)*histimg1.rows / 255);
						rectangle(histimg1, Point(i*binW, histimg1.rows), Point((i + 1)*binW, histimg1.rows - val), Scalar(buf.at<Vec3b>(i)), -1, 8);
					}

				}
				calcBackProject(&hue1, 1, 0, hist1, backproj1, &phranges1);//计算直方图的反向投影,计算hue1图像0通道直方图hist1的反向投影,并赋值到backproj1中
				backproj1 &= mask1;

				trackBox1 = CamShift(backproj1, trackwindow1, TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1));

				if (trackwindow1.area() <= 1)
				{
					int cols = backproj1.cols, rows = backproj1.rows, r = (MIN(cols, rows) + 5) / 6;
					trackwindow1 = Rect(trackwindow1.x - r, trackwindow1.y - r, trackwindow1.x + r, trackwindow1.y + r) &Rect(0, 0, cols, rows);//Rect函数为矩阵的偏移和大小,即第一二个参数为矩阵的左上角点坐标,第三四个参数为矩阵的宽和高
				}

				if (backproj1Mode)
				{
					cvtColor(backproj1, image, CV_GRAY2BGR);//因此投影模式下显示的也是rgb图?
				}
				//cvtColor(backproj1, image, CV_GRAY2BGR);//因此投影模式下显示的也是rgb图?
				if (trackBox1.size.width >= 0 && trackBox1.size.height >= 0)
				{
					ellipse(image, trackBox1, Scalar(0, 0, 255), 3, CV_AA); //画出椭圆
				}
				
			

			if (trackObject >= 2)//trackObject初始化为0,或者按完键盘的'c'键后也为0,当鼠标单击松开后为-1
			{
				int _vmin = vmin, _vmax = vmax;

				//inRange函数的功能是检查输入数组每个元素大小是否在2个给定数值之间,可以有多通道,mask1保存0通道的最小值,也就是h分量
				//这里利用了hsv的3个通道,比较h,0~180,s,smin~256,v,min(vmin,vmax),max(vmin,vmax)。如果3个通道都在对应的范围内,则
				//mask1对应的那个点的值全为1(0xff),否则为0(0x00).
				inRange(hsv, Scalar(0, smin, MIN(_vmin, _vmax)),
					Scalar(180, 256, MAX(_vmin, _vmax)), mask2);
				int ch[] = { 0, 0 };
				hue1.create(hsv.size(), hsv.depth());//hue1初始化为与hsv大小深度一样的矩阵,色调的度量是用角度表示的,红绿蓝之间相差120度,反色相差180度
				hue2.create(hsv.size(), hsv.depth());
				mixChannels(&hsv, 1, &hue1, 1, ch, 1);//将hsv第一个通道(也就是色调)的数复制到hue1中,0索引数组
				mixChannels(&hsv, 1, &hue2, 1, ch, 1);

				if (trackObject == 3)
				{
					//此处的构造函数roi用的是Mat hue1的矩阵头,且roi的数据指针指向hue1,即共用相同的数据,select为其感兴趣的区域

					Mat roi2(hue2, selection2), maskroi(mask2, selection2);//mask1保存的hsv的最小值
					//calchist1()函数第一个参数为输入矩阵序列,第2个参数表示输入的矩阵数目,第3个参数表示将被计算直方图维数通道的列表,第4个参数表示可选的掩码函数
					//第5个参数表示输出直方图,第6个参数表示直方图的维数,第7个参数为每一维直方图数组的大小,第8个参数为每一维直方图bin的边界
					calcHist(&roi2, 1, 0, maskroi, hist2, 1, &hsize, &phranges2);//将roi的0通道计算直方图并通过mask1放入hist1中,hsize为每一维直方图的大小
					normalize(hist2, hist2, 0, 255, CV_MINMAX);//将hist1矩阵进行数组范围归一化,都归一化到0~255

					trackwindow2 = selection2;
					trackObject = 4;//只要鼠标选完区域松开后,且没有按键盘清0键'c',则trackObject一直保持为1,因此该if函数只能执行一次,除非重新选择跟踪区域

					histimg2 = Scalar::all(0);//与按下'c'键是一样的,这里的all(0)表示的是标量全部清0
					int binW = histimg2.cols / hsize;  //hist1ing是一个200*300的矩阵,hsize应该是每一个bin的宽度,也就是hist1ing矩阵能分出几个bin出来
					Mat buf(1, hsize, CV_8UC3);//定义一个缓冲单bin矩阵
					for (int i = 0; i < hsize; i++)//saturate_case函数为从一个初始类型准确变换到另一个初始类型
						buf.at<Vec3b>(i) = Vec3b(saturate_cast<uchar>(i*180. / hsize), 255, 255);//Vec3b为3个char值的向量
					cvtColor(buf, buf, CV_HSV2BGR);//将hsv又转换成bgr

					for (int i = 0; i < hsize; i++)
					{
						//cout << "he" << endl;
						int val = saturate_cast<int>(hist2.at<float>(i)*histimg2.rows / 255);//at函数为返回一个指定数组元素的参考值
						rectangle(histimg2, Point(i*binW, histimg2.rows),    //在一幅输入图像上画一个简单抽的矩形,指定左上角和右下角,并定义颜色,大小,线型等
							Point((i + 1)*binW, histimg2.rows - val),
							Scalar(buf.at<Vec3b>(i)), -1, 8);
					}

				}
				calcBackProject(&hue2, 1, 0, hist2, backproj2, &phranges2);//计算直方图的反向投影,计算hue1图像0通道直方图hist1的反向投影,并赋值到backproj2中	
				backproj2 &= mask2;
				trackBox2 = CamShift(backproj2, trackwindow2, TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1));
				if (  trackwindow2.area() <= 1)
				{
					int cols = backproj2.cols, rows = backproj2.rows, r = (MIN(cols, rows) + 5) / 6;
					trackwindow2 = Rect(trackwindow2.x - r, trackwindow2.y - r, trackwindow2.x + r, trackwindow2.y + r) &Rect(0, 0, cols, rows);//Rect函数为矩阵的偏移和大小,即第一二个参数为矩阵的左上角点坐标,第三四个参数为矩阵的宽和高
				}

				if (backproj1Mode)
				{
					
					cvtColor(backproj2, image2, CV_GRAY2BGR);
					
				}
				if (trackBox2.size.width >= 0 && trackBox2.size.height >= 0)
				{
					ellipse(image, trackBox2, Scalar(0, 0, 255), 3, CV_AA); //画出椭圆
				}
				
			}
		}
		
		//拖出矩形框
		if (mark1 == false &&  selectObject && selection1.width > 0 && selection1.height > 0)
		{
			Mat roi(image, selection1);
			bitwise_not(roi, roi);//bitwise_not为将每一个bit位取反

		}
		else if (mark1 == true && selectObject && selection2.width > 0 && selection2.height > 0)
		{
			//cout << "he" << endl;
			Mat roi2(image, selection2);
			bitwise_not(roi2, roi2);//bitwise_not为将每一个bit位取反
		}

		imshow("CamShift Demo", frame);
		imshow("Rotated", image);
		imshow("histogram", histimg1);
		imshow("histogram2", histimg2);

		char c = (char)waitKey(40);
		if (c == 27)              //退出键
			break;
		switch (c)
		{
		case 's':
			type = 10;
			break;
		case 'b':             //反向投影模型交替
			backproj1Mode = !backproj1Mode;
			break;
		case 'c':            //清零跟踪目标对象
			trackObject = 0;
			histimg1 = Scalar::all(0);
			mark1 = false;
			mark2 = false;
			break;
		case '1':          //标志标志1选取完成
			trackObject=1; 
			mark1 = true;
			break;
		case '2':
			trackObject++;
			mark2 = true;
			break;
		case 'h':          //显示直方图交替
			showhist1 = !showhist1;
			if (!showhist1)
			{
				destroyWindow("histogram");
				destroyWindow("histogram2");
			}
			else
			{
				namedWindow("histogram", 1);
				namedWindow("histogram2", 1);
			}
			break;
		case 'p':          //透视变形
			type = 0;     
			break;
		default:
			;
		}
	}

	
	
	
	return 0;
}