#include <iostream>
#include "SerialPort.hpp"
#include "Vision.hpp"
#include <cstring>
#include <string>
#include "pid.hpp"
#include <stdio.h>
#include <cmath>
extern SP::SerialPort serial; // 修改为你的串口设备
extern char msg[6];
//extern pid::PIDCLASS pid1;
pid::PIDCLASS pid1(
1.40F,
0,
0.060F,
100.0F,
100.0F,
-100.0F,
0.040F
);
double th = 20;/* 二值化阈值 */
float Xposition = 0;
float pidoutput = 0;
int pidoutINT = 0;
u_int8_t MX = 125;
char STR[20]={0};
int main() {
cv::VideoCapture Camera01;/* 定义相机 */
cv::Mat Picture0,Picture_B,Picture_Gray,inary,Picture_Binary_Deal,Picture_Binary_Deal_ROI,
Picture_Binary_Deal_ROI_L,Picture_Binary_Deal_ROI_R,Picture_Binary;/* 定义图片 */
Camera01.open(0);/* 打开系统视频设备0 */
if( !Camera01.isOpened() ){std::cout<<"初始化时相机打开失败"<<std::endl;return -1;}/* 相机打开失败时 结束程序 */
Camera01.set(cv::CAP_PROP_FRAME_WIDTH,320);/* 设置相机分辨率 */
Camera01.set(cv::CAP_PROP_FRAME_HEIGHT,240);
//uart_init();
uart_init();
while( Camera01.read(Picture0) ){ /* 读取图片到Picture0 */
int keyvalue = 0;
usleep(40000);/* 40000us=40ms 休眠 */
cv::cvtColor(Picture0, Picture_Gray, cv::COLOR_BGR2GRAY);/* 灰度化 */
cv::threshold(Picture_Gray, Picture_Binary, th, 255, cv::THRESH_BINARY_INV); // 反二值化,黑线变白
cv::Mat Picture_element = cv::getStructuringElement(cv::MORPH_RECT,cv::Size(5,5));//形态学操作
cv::morphologyEx(Picture_Binary, Picture_Binary_Deal, cv::MORPH_OPEN, Picture_element);//开运算
cv::Rect rect_roizone = cv::Rect(0,Picture_Binary_Deal.size().height/2,
Picture_Binary_Deal.size().width,Picture_Binary_Deal.size().height/2);
Picture_Binary_Deal_ROI = Picture_Binary_Deal( rect_roizone );
/* 设定两个ROI */
cv::Rect rect_roizone_Left = cv::Rect(0,0,
Picture_Binary_Deal_ROI.size().width/2,Picture_Binary_Deal_ROI.size().height);
cv::Rect rect_roizone_Right = cv::Rect( Picture_Binary_Deal_ROI.size().width/2 , 0 ,
Picture_Binary_Deal_ROI.size().width/2,Picture_Binary_Deal_ROI.size().height);
Picture_Binary_Deal_ROI_L = Picture_Binary_Deal_ROI( rect_roizone_Left );/* 设定下半部分屏幕1/3 2/3区域为ROI */
Picture_Binary_Deal_ROI_R = Picture_Binary_Deal_ROI( rect_roizone_Right );
cv::Moments Moment1 = cv::moments(Picture_Binary_Deal_ROI,true);/* 算质心 */
cv::Moments Moment1L = cv::moments(Picture_Binary_Deal_ROI_L,true);/* 算左侧质心 */
cv::Moments Moment1R = cv::moments(Picture_Binary_Deal_ROI_R,true);/* 算右侧质心 */
float XmL = (float)(Moment1L.m10/Moment1L.m00), XmR = (float)(Moment1R.m10/Moment1R.m00);
float temp= (float)(Moment1.m10/Moment1.m00);
if( std::isnan(temp) ){ Xposition=0; }
else{Xposition = temp;}
//Xposition = /* 获得车道的X坐标 */
cv::Point p01( Xposition, Moment1.m01/Moment1.m00 + Picture_Binary.size().height/2);/* 求质心点 */
//cv::Point p01(Moment1.m10/Moment1.m00,Picture_Binary_Deal.size().height/2+Picture_Binary.size().height/2);/* 质心 */
pidoutput = pid1.pidf( (float)(Picture_Binary.size().width/2),(float)(Xposition) );
pidoutINT = (int)(pidoutput);
std::cout << "pidout:"<<pidoutINT<<std::endl;
MX = pidoutINT + 120;/* MX发送给下位机 120->0 50-> -70 由于\0是字符串结束符 故不能发送0 */
if(MX<1){ MX=1;}if(MX>255){MX=255;}
sprintf(STR,"pidour:%d ",pidoutINT);
cv::Point p03( XmL , Picture_Binary.size().height/2+Moment1L.m01/Moment1L.m00);
cv::Point p04( Picture_Binary.size().width/2+XmR , Picture_Binary.size().height/2+Moment1R.m01/Moment1R.m00);
cv::Point p00( Picture_Binary.size().width/2 , Picture_Binary.size().height );
cv::Point p02( Picture_Binary.size().width/2 + pidoutINT,
Moment1.m01/Moment1.m00 + Picture_Binary.size().height/2);/* pid输出以点的形式呈现 */
cv::rectangle(Picture0,rect_roizone,cv::Scalar(165,0,0),2);
cv::circle(Picture0, p02, 4, cv::Scalar(255, 0, 0), 2);/* 画点 */
cv::circle(Picture0, p01, 8, cv::Scalar(0, 255, 0), 2);/* 画点 */
cv::circle(Picture0, p03, 5, cv::Scalar(200, 233, 0), 1);/* 画左侧质心点 */
cv::circle(Picture0, p04, 5, cv::Scalar(200, 233, 0), 1);/* 画右侧质心点 */
std::cout << "XmL="<<XmL <<" XmR="<< (Picture_Binary.size().width/2+XmR) << std::endl;
if( (!std::isnan(XmL)) && (!std::isnan(XmR) )&& (abs(XmL - XmR) > 100.0))
{
//std::cout << "两条路" << std::endl;
cv::line( Picture0, p00, p03, cv::Scalar(255, 0, 0), 1 );
}
cv::line(Picture0, cv::Point( Picture0.size().width/2 , 0 ),
cv::Point(Picture0.size().width/2, Picture0.size().height),
cv::Scalar(0, 0, 200));/* 画中线 */
cv::putText(Picture0,STR,cv::Point(4,25),cv::FONT_HERSHEY_SIMPLEX,0.5,cv::Scalar_(200,200,100),2,8 ) ;
//cv::imshow("WightBlackPic",Picture_Binary_Deal);/* 显示图片 */
cv::imshow("CapPicture",Picture0);/* 显示图片 */
cv::imshow("ROI",Picture_Binary_Deal_ROI);/* 显示图片 */
cv::imshow("ROI_Left",Picture_Binary_Deal_ROI_L);
cv::imshow("ROI_Right",Picture_Binary_Deal_ROI_R);
cv::moveWindow("ROI",450,0);cv::moveWindow("ROI_Left",450,160);cv::moveWindow("ROI_Right",450+320,160);
cv::moveWindow("CapPicture",0,0);
if( cv::waitKey(1) == 'q' ){std::cout<<"检测到按下q退出"<<std::endl;return 1;}/* 图片显示停留1ms 除非检测到按键'q' 退出程序 */
msg[0]='S';
msg[1]=MX;
msg[2]=0xFF;
msg[3]=0xFF;
msg[4]='E';
//msg[5]=0;
uart();
}
return 0;
}