int main(int argc, char **argv) {
Tserial* p;
p = new Tserial();
p->connect(UART_COMM_PORT, UART_BAUD_RATE, spNONE);
ros::init(argc, argv, "yaw_publisher");
ros::NodeHandle n;
ros::Publisher yaw_pub = n.advertise<std_msgs::Float32>("yaw", 10);
std_msgs::Float32 msg;
while(ros::ok()) {
char data_in[10];
while(p->getChar() != '!');
p->getArray(data_in, 8);
msg.data = convertDataToVal2(data_in);
yaw_pub.publish(msg);
}
p->disconnect();
return 0;
}
void navCommand(int l, int r)
{
Tserial* p = new Tserial();
p->connect("COM2",9600,spNONE);
p->sendChar('w');
p->sendChar('0'+l);
p->sendChar('0'+r);
p->disconnect();
}
int main()
{ char a;
Tserial *com;
com = new Tserial();
com->connect("COM11", 9600, spNONE); //check com port
cvWaitKey(5000);
while(1)
{
a=com->getChar();
cout<<a<<endl;
//cvWaitKey(1000);
// cout<<"i";
}
}
int main()
{
Tserial* p;
p = new Tserial();
p->connect(UART_COMM_PORT, UART_BAUD_RATE, spNONE,VERBOSE);
char array[2] = {' ', ' '};
p->sendArray(array, 2);
usleep(100);
p->disconnect();
srand(time(0));
for(int j = 0; j < 15; j++) {
for(int i = 0; i < 5; i++) {
printf("sendCommand2\n");
sendCommand0(p);
usleep(100 * 1000);
}
for(int i = 0; i < 5; i++) {
printf("sendCommand1\n");
sendCommand0(p);
usleep(100 * 1000);
}
}
p->connect(UART_COMM_PORT, UART_BAUD_RATE, spNONE,VERBOSE);
p->sendChar(' ');
usleep(100);
p->disconnect();
}
double ReadIR(Tserial& serial)
{
int nbytes = serial.getNbrOfBytes();
std::stringstream ss;
for(int i = 0; i < nbytes; i++)
{
ss << serial.getChar();
}
double distance;
while(ss.good())
{
std::string str;
std::getline(ss,str);
if(!str.empty())
{
std::stringstream test(str);
test >> distance;
}
}
int send(int mode, float angle)
{
Tserial* p = new Tserial();
p->connect("COM2",9600,spNONE);
switch(mode)
{
case TEST:
p->sendChar('w');
p->sendChar('5');
p->sendChar('5');
break;
case FORWARD:
p->sendChar('w');
if(angle >= 10)
{
p->sendChar('3');
p->sendChar('7');
}
else if(angle <= -10)
{
p->sendChar('7');
p->sendChar('3');
}
else
{
p->sendChar('5');
p->sendChar('5');
}
break;
case BRAKE:
p->sendChar('s');
break;
default:
break;
}
p->disconnect();
return 0;
}
void navCommand(float tangle, float cangle)
{
printf("tangle-cangle:%f\n", tangle-cangle);
Tserial* p = new Tserial();
p->connect("COM2",9600,spNONE);
if((tangle - cangle < 5) && (tangle - cangle > -5))
{
//printf("Foward\n");
p->sendChar('w');
p->sendChar('5');
p->sendChar('5');
}
else if(tangle - cangle >= 5)
{
//printf("Right\n");
p->sendChar('d');
}
else if(tangle - cangle <= -5)
{
//printf("Left\n");
p->sendChar('a');
}
/*if(angle > 1)
{
p->sendChar('w');
p->sendChar('3');
p->sendChar('7');
}
else if (angle < -1)
{
p->sendChar('w');
p->sendChar('7');
p->sendChar('3');
}
else
{
p->sendChar('w');
p->sendChar('5');
p->sendChar('5');
}*/
p->disconnect();
}
int main(){
Tserial *com;
char ch;
com = new Tserial();
com->connect("COM3", 4800, spNONE);
CvCapture *capture = 0;
IplImage *frame = 0;
int key = 0;
com->sendChar('a');
int rpx,rpy,ryx,ryy,bx,by,rpx1,rpy1,ryx1,ryy1,bx1,by1,ccx1,ccy1;
int ccx=91,ccy=45;
double theta1,theta2;
/* initialize camera */
capture = cvCaptureFromCAM(0);
cvSetCaptureProperty( capture, CV_CAP_PROP_FRAME_WIDTH, 1024 );
cvSetCaptureProperty( capture, CV_CAP_PROP_FRAME_HEIGHT, 720 );
/* always check */
if ( !capture ) {
fprintf( stderr, "Cannot open initialize webcam!\n" );
return 1;
}
/* create a window for the video */
cvNamedWindow( "image", CV_WINDOW_AUTOSIZE );
while( key != 'q' ) {
/* get a frame */
img = cvQueryFrame( capture );
/* always check */
if( !img ) break;
img0=cvCreateImage(cvGetSize(img),IPL_DEPTH_8U,3);
cvCvtColor(img,img0,CV_BGR2HSV);
cvSetMouseCallback( "image", mouseHandler, img0 );
// cvThreshold(img0, img0, 85, 255, CV_THRESH_BINARY);
cvDilate(img0,img0,NULL,1);
// cvErode(img0,img0,NULL,1);
/* display curent frame */
cvShowImage( "image", img );
rpx=corp(img0,1);
ryx=cory(img0,1);
bx=corr(img0,1);
rpy=corp(img0,0);
ryy=cory(img0,0);
by=corr(img0,0);
printf("px=%d\tyx=%d\trx=%d\t py=%d\t yy=%d\t ry=%d \n",rpx,ryx,bx,rpy,ryy,by);
rpx1=rpx-ryx;
rpy1=rpy-ryy;
bx1=bx-ryx;
by1=by-ryy;
ccx1=ccx-ryx;
ccy1=ccy-ryy;
//printf("%lf\t %lf\t %lf\t%lf\n", theta1, theta2,theta2-theta1,M_PI+theta2-theta1);
if(bx!= 0 ){
com->sendChar('o');
theta1=atan((double)rpy1/(double)rpx1);
theta2=atan((double)by1/(double)bx1);
if(theta1-theta2<0.4 && theta1-theta2>-0.4 && rpx1*bx1>0){
com->sendChar('f');
}
else if(theta1-theta2<0.1 && theta1-theta2>-0.1 && rpx1*bx1<0)
{
com->sendChar('l');
}
else if(rpx1>0 && bx1>0 && theta2-theta1<0)
com->sendChar('l');
else if(rpx1>0 && bx1>0 && theta2-theta1>0)
com->sendChar('r');
else if(rpx1 < 0 && bx1 < 0 && theta2-theta1 < 0)
com->sendChar('l');
else if(rpx1 < 0 && bx1 < 0 && theta2-theta1 > 0)
com->sendChar('r');
else if(rpx1>0 && bx1<0 &&M_PI+ theta2-theta1>0 && M_PI+theta2-theta1<M_PI){
com->sendChar('R');
//cvWaitKey(1);
}
else if(rpx1>0 && bx1<0 && M_PI+ theta2-theta1>M_PI){
com->sendChar('L');
//cvWaitKey(1);
}
else if(rpx1<0 && bx1>0 &&M_PI+ theta1-theta2>0.1 && M_PI+ theta1-theta2 <M_PI){
com->sendChar('L');
//cvWaitKey(1);
}
else if(rpx1<0 && bx1>0 && M_PI+ theta1-theta2<M_PI){
com->sendChar('R');
//cvWaitKey(1);
}
}
else if (bx==0&&sqrt(double((ccx-rpx)*(ccx-rpx))+double((ccy-rpy)*(ccy-rpy)))>=35.0){
//printf("%lf", sqrt(double((ccx-rpx)*(ccx-rpx))+double((ccy-rpy)*(ccy-rpy))));
theta1=atan((double)rpy1/(double)rpx1);
theta2=atan((double)ccy1/(double)ccx1);
com->sendChar('c');
if(theta1-theta2<0.4 && theta1-theta2>-0.4 && rpx1*ccx1>0){
com->sendChar('f');
}
else if(theta1-theta2<0.1 && theta1-theta2>-0.1 && rpx1*ccx1<0)
{
com->sendChar('l');
}
else if(rpx1>0 && ccx1>0 && theta2-theta1<0)
com->sendChar('l');
else if(rpx1>0 && ccx1>0 && theta2-theta1>0)
com->sendChar('r');
else if(rpx1 < 0 && ccx1 < 0 && theta2-theta1 < 0)
com->sendChar('l');
else if(rpx1 < 0 && ccx1 < 0 && theta2-theta1 > 0)
com->sendChar('r');
else if(rpx1>0 && ccx1<0 &&M_PI+ theta2-theta1>0 && M_PI+theta2-theta1<M_PI){
com->sendChar('R');
//cvWaitKey(1);
}
else if(rpx1>0 && ccx1<0 && M_PI+ theta2-theta1>M_PI){
com->sendChar('L');
//cvWaitKey(1);
}
else if(rpx1<0 && ccx1>0 &&M_PI+ theta1-theta2>0.1 && M_PI+ theta1-theta2 <M_PI){
com->sendChar('L');
//cvWaitKey(1);
}
else if(rpx1<0 && ccx1>0 && M_PI+ theta1-theta2<M_PI){
com->sendChar('R');
//cvWaitKey(1);
}
}
else if(sqrt(double((ccx-rpx)*(ccx-rpx))+double((ccy-rpy)*(ccy-rpy)))<40.0){
com->sendChar('s');
com->sendChar('o');
}
/* exit if user press 'q' */
key = cvWaitKey(1);
cvReleaseImage(&img0);
//com->sendChar('d');
}
/* free memory */
cvDestroyWindow( "image" );
cvReleaseCapture( &capture );
com->disconnect();
return 0;
}
int main()
{
int check_capture=1;
int z;
char done, done2;
Tserial *com;
com = new Tserial();
com->connect("COM9",4800, spNONE); // set the com port, also the baud rate
CvCapture *capture=cvCreateCameraCapture(0);//initiate camera
// com->sendChar('d');
for(z=0;z<200000000&&capture!=NULL;z++);//starting camera takes some time
if(capture==NULL)
{
cout<<"camera not initialised\n";
return -1;
}
while(check_capture!=0)
{
// cout<<"hi\n";
// IplImage* img1;
IplImage* img;
IplImage* img_hsv;
CvCapture *capture=cvCreateCameraCapture(0);//initiate camera
img=cvQueryFrame(capture);//take current image in camera and give it to frame pointer
// cout<<"hi2\n";
cvNamedWindow("Picture");
cvShowImage("Picture",img);
//cvSetImageROI(img1, cvRect(150,77,800,800)); //set the values accordingly
//img = cvCreateImage(cvGetSize(img1),img1->depth,img1->nChannels);
/* copy subimage */
//cvCopy(img1,img, NULL);
//cvResetImageROI(img1);
img_hsv=cvCreateImage(cvGetSize(img),IPL_DEPTH_8U,3); //Conversion to hsv
cvCvtColor(img,img_hsv,CV_BGR2HSV);
done=detectregion(img_hsv);
if(done=='w')
{
com->sendChar('d');
//cout<<"hi"<<endl;
for(int k=0; k<=800; k++)
com->sendChar('B');
//cout<<"asdfgh"<<endl;
com->sendChar('G');
// cvWaitKey(10);
}
com->sendChar(done);
/* if(done=='d')
{
for(int k=0; k<=800; k++)
com->sendChar('b');
// com->sendChar('g');
//cout<<"hi"<<endl;
// cvWaitKey(10);
}*/
//cvReleaseImage(&img1);
cvReleaseImage(&img);
cvReleaseImage(&img_hsv);
// cout<<"hello\n";
// cvWaitKey(0);
// cvDestroyWindow("Picture");
// cout<<"hello2\n";
// cvWaitKey(1);
}
}
int main(){
Tserial *com;
char ch;
com = new Tserial();
com->connect("COM3", 4800, spNONE);
CvCapture *capture = 0;
IplImage *frame = 0;
int key = 0;
com->sendChar('a');
int rpx,rpy,ryx,ryy,bx,by,rpx1,rpy1,ryx1,ryy1,bx1,by1;
double theta1=0.1,theta2=0.1;
/* initialize camera */
capture = cvCaptureFromCAM(0);
cvSetCaptureProperty( capture, CV_CAP_PROP_FRAME_WIDTH, 1024 );
cvSetCaptureProperty( capture, CV_CAP_PROP_FRAME_HEIGHT, 720 );
/* always check */
if ( !capture ) {
fprintf( stderr, "Cannot open initialize webcam!\n" );
return 1;
}
/* create a window for the video */
cvNamedWindow( "image", CV_WINDOW_AUTOSIZE );
while( key != 'q' ) {
/* get a frame */
img = cvQueryFrame( capture );
/* always check */
if( !img ) break;
img0=cvCreateImage(cvGetSize(img),IPL_DEPTH_8U,3);
cvCvtColor(img,img0,CV_BGR2HSV);
// cvSetMouseCallback( "image", mouseHandler, img0 );
// cvThreshold(img0, img0, 85, 255, CV_THRESH_BINARY);
/* display curent frame */
cvShowImage( "image", img0 );
rpx=corp(img0,1);
ryx=cory(img0,1);
bx=corr(img0,1);
rpy=corp(img0,0);
ryy=cory(img0,0);
by=corr(img0,0);
rpx1=rpx-ryx;
rpy1=rpx-ryy;
bx1=bx-ryx;
by1=by-ryy;
theta1=atan((double)rpy1/(double)rpx1);
theta2=atan((double)by1/(double)bx1);
if(theta1>0 && theta1-theta2>0 && rpx1>0)
com->sendChar('r');
else if(theta1<=0 && M_PI+theta1-theta2>0)
com->sendChar('l');
else if(theta1<0 && theta2>=0 && rpx<ryx)
com->sendChar('r');
else if(theta1>0 && theta1-theta2<0)
com->sendChar('l');
else if(theta1>0 && theta1-theta2>0 && rpx1<0)
com->sendChar('l');
else if(theta1-theta2==0.0 && rpx1*bx1>0)
com->sendChar('f');
else if(theta1-theta2==0.0 && rpx1*bx1<0){
com->sendChar('r');
cvWaitKey(5);
}
/* exit if user press 'q' */
key = cvWaitKey( 1 );
cvReleaseImage(&img0);
}
/* free memory */
cvDestroyWindow( "image" );
cvReleaseCapture( &capture );
com->disconnect();
return 0;
}
int main()
{
Tserial *com;
char ch;
com = new Tserial();
com->connect("COM3", 4800, spNONE);
CvCapture *capture = 0;
IplImage *frame = 0;
int key = 0;
// com->sendChar('a');
int pinkx,pinky,yellowx,yellowy,bluey,greeny,violetx,violety,botx,boty,i=0;
int hb,sb,cb,wb;
int greenx[21]={1},bluex[21]={1};
double bottheta,theta2,slope;
int flag=1,flag1=1,flag2=1;
/* initialize camera */
capture = cvCaptureFromCAM(0);
//cvSetCaptureProperty( capture, CV_CAP_PROP_FRAME_WIDTH, 1024 );
//cvSetCaptureProperty( capture, CV_CAP_PROP_FRAME_HEIGHT, 720 );
/* always check */
if ( !capture )
{
fprintf( stderr, "Cannot open initialize webcam!\n" );
return 1;
}
/* create a window for the video */
cvNamedWindow( "image", CV_WINDOW_AUTOSIZE );
while( key != 'q' )
{
/* get a frame */
img = cvQueryFrame( capture );
/* always check */
if( !img ) break;
cvErode(img,img,NULL,1);
cvDilate(img,img,NULL,1);
cvErode(img,img,NULL,1);
cvDilate(img,img,NULL,1);
cvErode(img,img,NULL,1);
cvDilate(img,img,NULL,1);
img0=cvCreateImage(cvGetSize(img),IPL_DEPTH_8U,3);
imgblue=cvCreateImage(cvGetSize(img),IPL_DEPTH_8U,3);
imgpink=cvCreateImage(cvGetSize(img),IPL_DEPTH_8U,3);
imgyellow=cvCreateImage(cvGetSize(img),IPL_DEPTH_8U,3);
imggreen=cvCreateImage(cvGetSize(img),IPL_DEPTH_8U,3);
imgblack=cvCreateImage(cvGetSize(img),IPL_DEPTH_8U,3);
imgviolet=cvCreateImage(cvGetSize(img),IPL_DEPTH_8U,3);
cvCvtColor(img,img0,CV_BGR2HSV);
cvSetMouseCallback( "image", mouseHandler, img0 );
// cvThreshold(img0, img0, 85, 255, CV_THRESH_BINARY);
/*cvErode(img0,img0,NULL,1);
cvDilate(img0,img0,NULL,1);
cvErode(img0,img0,NULL,1);
cvDilate(img0,img0,NULL,1);
cvErode(img0,img0,NULL,1);
cvDilate(img0,img0,NULL,1);*/
/* display curent frame */
cvShowImage( "image", img );
pinkx=pink(img0,1);
yellowx=yellow(img0,1);
for(i=20;i>0;i--)
{
bluex[i]=bluex[i-1];
}
bluex[0]=blue(img0,1);
for(i=20;i>0;i--)
{
greenx[i]=greenx[i-1];
}
greenx[0]=green(img0,1);
violetx=violet(img0,1);
black(img,1);
uchar* datablack=(uchar*)imgblack->imageData;
hb=img0->height;
wb=img0->width;
cb=img0->nChannels;
sb=img0->widthStep;
uchar* datablue=(uchar*)imgblue->imageData;
uchar* datayellow=(uchar*)imgyellow->imageData;
pinky=pink(img0,0);
yellowy=yellow(img0,0);
bluey=blue(img0,0);
greeny=green(img0,0);
violety=violet(img0,0);
cvShowImage( "imageblue", imgblue );
cvShowImage( "imagepink", imgpink );
cvShowImage( "imageyellow", imgyellow );
cvShowImage( "imagegreen", imggreen );
cvShowImage( "imageblack", imgblack );
cvShowImage( "imageviolet", imgviolet );
botx=pinkx-violetx;
boty=pinky-violety;
bottheta=atan((double)boty/(double)botx)*180/PI;
if(pinky>giy && pinky<gfy && pinkx < 225&& flag2==1){
if(greenx[0]-greenx[20]>1 && greenx[0]>gix ){
com->sendChar('F');
printf("1");
flag2=0;
}
else{
com->sendChar('s');
printf("stop");
}
}
else if(bottheta<45 && bottheta >-45 && pinkx>violetx && flag==1){//right
flag1=1;
printf("right\n");
if(datablack[pinky*sb+(pinkx+l)*cb+0]!=0)
{
if(bottheta >20)
{
com->sendChar('l');
printf("R\n");
}
else if (bottheta <-20)
{
com->sendChar('r');
printf("L\n");
}
else if(bottheta >10)
{
com->sendChar('r');
printf("r\n");
}
else if (bottheta <-10)
{
com->sendChar('l');
printf("l\n");
}
else
{
com->sendChar('F');
printf("F\n");
}
}
else if(datablack[pinky*sb+(pinkx+l)*cb+0]==0)
{
if((pinky-m)>0){
if(datablack[(pinky-m)*sb+(pinkx)*cb+0]!=0)
{
com->sendChar('L');
printf("Left\n");
flag=0;
}
else if((pinky+m)<479){
if(datablack[(pinky+m)*sb+(pinkx)*cb+0]!=0)
{
com->sendChar('R');
printf("Right\n");
flag=0;
}
}
}
}
}
else if((bottheta>45 || 180+bottheta <135) && pinky > violety && flag1==1){//back
flag=1;
printf("back\n");
if((pinky+n)<479){
if(datablack[(pinky+n)*sb+(pinkx)*cb+0]!=0&&(pinky+l)<479)
{
if(bottheta <75 && bottheta>45)
{
com->sendChar('r');
printf("R\n");
}
else if (180+bottheta >105&&180+bottheta <135)
{
com->sendChar('l');
printf("L\n");
}
else if(bottheta <85&&bottheta>45)
{
com->sendChar('e');
printf("r\n");
}
else if (180+bottheta >95 && 180+bottheta <135)
{
com->sendChar('d');
printf("l\n");
}
else
{
com->sendChar('F');
printf("F\n");
}
}
else if(datablack[(pinky+n)*sb+(pinkx)*cb+0]==0)
{
if((pinkx-m)>0){
if(datablack[(pinky)*sb+(pinkx-m)*cb+0]!=0)
{
com->sendChar('R');
printf("Right\n");
flag1=0;
}
else if(datablack[(pinky)*sb+(pinkx+m)*cb+0]!=0)
{
com->sendChar('L');
printf("Left\n");
flag1=0;
}
}
}
}
}
else if(180+bottheta>135 && 180+bottheta < 225 && pinkx<violetx && flag==1){//left
flag1=1;
printf("left\n");
if((pinkx-l) >0){
if(datablack[pinky*sb+(pinkx-l)*cb+0]!=0)
{
if(180+bottheta <160)
{
com->sendChar('r');
printf("R\n");
}
else if (180+bottheta >200)
{
com->sendChar('l');
printf("L\n");
}
else if(180+bottheta <170)
{
com->sendChar('e');
printf("r\n");
}
else if (180+bottheta>190)
{
com->sendChar('d');
printf("l\n");
}
else
{
com->sendChar('F');
printf("F\n");
}
}
else if(datablack[pinky*sb+(pinkx-l)*cb+0]==0)
{
if((pinky-m)>0&&pinky+m<479){
if(datablack[(pinky-m)*sb+(pinkx)*cb+0]!=0)
{
com->sendChar('R');
printf("Right\n");
flag=0;
}
else if(datablack[(pinky+m)*sb+(pinkx)*cb+0]!=0)
{
com->sendChar('L');
printf("Left\n");
flag=0;
}
}
}
}
}
else if((180+bottheta > 225 || bottheta < -45) && pinky < violety && flag1==1){//straight
flag=1;
printf("straight\n");
if((pinky-l)>0 || pinky<125){
if(datablack[(pinky-l)*sb+(pinkx)*cb+0]!=0)
{
if((180+bottheta <250) && (180+bottheta > 225))
{
com->sendChar('r');
printf("R\n");
}
else if ((bottheta > -70) && (bottheta < -45))
{
com->sendChar('l');
printf("L\n");
}
else if((180+bottheta <260) && (180+bottheta > 225))
{
com->sendChar('e');
printf("r\n");
}
else if ((bottheta > -80) && (bottheta < -45))
{
com->sendChar('d');
printf("l\n");
}
else if((pinkx > bix && pinky < biy)==0)
{
com->sendChar('F');
printf("F\n");
}
else{
if(((datablue[(pinky-40)*sb+(pinkx-30)*cb+0]!=0)&&(bluex[20]-bluex[0]<0)))
{
com->sendChar('F');
printf("blueforward\n");
}
else{
com->sendChar('s');
printf("bluesstop\n");
}
}
}
else if(datablack[(pinky-l)*sb+(pinkx)*cb+0]==0)
{
if((pinkx-l)> 0){
if(datablack[(pinky)*sb+(pinkx+m)*cb+0]!=0)
{
com->sendChar('R');
printf("Right\n");
flag1=0;
}
else if(datablack[(pinky)*sb+(pinkx-m)*cb+0]!=0)
{
com->sendChar('L');
printf("Left\n");
flag1=0;
}
}
}
}
}
key = cvWaitKey(1);
cvReleaseImage(&img0);
//cvReleaseImage(&img);
cvReleaseImage(&imgblue);
cvReleaseImage(&imgpink);
cvReleaseImage(&imgyellow);
cvReleaseImage(&imggreen);
cvReleaseImage(&imgblack);
cvReleaseImage(&imgviolet);
}
/* free memory */
cvDestroyWindow( "image" );
cvDestroyWindow( "imageblue");
cvDestroyWindow( "imagepink");
cvDestroyWindow( "imageyellow");
cvDestroyWindow( "imageblack");
cvDestroyWindow( "imagegreen");
cvDestroyWindow( "imageviolet");
cvReleaseCapture( &capture );
com->disconnect();
return 0;
}
