diff --git a/MobileRobot/AugReaMarker/CamMark/build/camtomar/CMakeFiles/camtomar.dir/CXX.includecache b/MobileRobot/AugReaMarker/CamMark/build/camtomar/CMakeFiles/camtomar.dir/CXX.includecache index b514746d669fc7dc69298962db3a6c2d4d59298c..9d5c4b4f54e39af0a4c6395865fab0378e5c7641 100644 --- a/MobileRobot/AugReaMarker/CamMark/build/camtomar/CMakeFiles/camtomar.dir/CXX.includecache +++ b/MobileRobot/AugReaMarker/CamMark/build/camtomar/CMakeFiles/camtomar.dir/CXX.includecache @@ -130,12 +130,6 @@ opencv2/contrib/contrib.hpp VisionControl.h - -/home/faridalijani/thesis/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp -iostream -- -VisionControl.h -- - /opt/ros/hydro/include/XmlRpcDecl.h ros/macros.h - diff --git a/MobileRobot/AugReaMarker/CamMark/build/camtomar/CMakeFiles/camtomar.dir/src/VisionControl.cpp.o b/MobileRobot/AugReaMarker/CamMark/build/camtomar/CMakeFiles/camtomar.dir/src/VisionControl.cpp.o index dc892a180231eb55831b43fe8871cf7d626ae69e..ec1763a300aa1b59cdb5b4fe50bd3db51a160c23 100644 Binary files a/MobileRobot/AugReaMarker/CamMark/build/camtomar/CMakeFiles/camtomar.dir/src/VisionControl.cpp.o and b/MobileRobot/AugReaMarker/CamMark/build/camtomar/CMakeFiles/camtomar.dir/src/VisionControl.cpp.o differ diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h index 324817a0dfc4e284e8cfb7f338c33da4e213cb80..befd277b2d1e11ed573249e070e69cc769afbbea 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h @@ -93,11 +93,10 @@ private: static const string trackbarWindowName; - void Controller(double Reference, double CamPose, double dt, int index); - //void Controller(double Reference[6], double RobPose[6], double dt); + void Controller(double RefX, double MarPoseX, double refY, double MarPoseY, double refYAW, double MarPoseYAW, double dt); + + void dock(double VelX, double VelY, double omegaZ); - void dock(double VelX, double VelY, double VelZ, double omegaX, double omegaY, double omegaZ, double omegaW); - //void dock(double speed[6]); public: ImageConverter(); @@ -119,26 +118,41 @@ public: void myhandler(int value); - static double prev_error; - - static double int_error; - - static double curr_error; - - static double diff; - - static double p_term; - - static double d_term; - - static double i_term; - - static double control_signal; - - + static double prop_gain; static double integ_gain; static double deriv_gain; + + // ---- CONTROLL PARAMETERS ------ // + static double prev_errorX; + static double int_errorX; + static double curr_errorX; + static double diffX; + static double p_termX; + static double d_termX; + static double i_termX; + static double control_signalX; + + static double prev_errorY; + static double int_errorY; + static double curr_errorY; + static double diffY; + static double p_termY; + static double d_termY; + static double i_termY; + static double control_signalY; + + static double prev_errorYAW; + static double int_errorYAW; + static double curr_errorYAW; + static double diffYAW; + static double p_termYAW; + static double d_termYAW; + static double i_termYAW; + static double control_signalYAW; + // ---- CONTROLL PARAMETERS ------ // + + static double zeroMin,zeroMax,eps; double marpose[6]; diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ index 63c911ab444207345bc4ce9da9bb89e557bfa49e..ee062152358fbb77bdcff65de6bef288e88d0496 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ @@ -87,16 +87,15 @@ private: static float TheMarkerSize; void ContStart(); - static bool update_images,MarKerPoseObtained,found,seekingMarker; + static bool update_images,found; bool readArguments ( int argc,char **argv ); static const string trackbarWindowName; - void Controller(double Reference, double CamPose, double dt, int index); - //void Controller(double Reference[6], double RobPose[6], double dt); + void Controller(double RefX, double MarPoseX, double refY, double MarPoseY, double refYAW, double MarPoseYAW, double dt); - void dock(double VelX, double VelY, double VelZ, double omegaX, double omegaY, double omegaZ, double omegaW); + void dock(double VelX, double VelY, double omegaZ); //void dock(double speed[6]); public: @@ -119,26 +118,41 @@ public: void myhandler(int value); - static double prev_error; - - static double int_error; - - static double curr_error; - - static double diff; - - static double p_term; - - static double d_term; - - static double i_term; - - static double control_signal; - - + static double prop_gain; static double integ_gain; static double deriv_gain; + + // ---- CONTROLL PARAMETERS ------ // + static double prev_errorX; + static double int_errorX; + static double curr_errorX; + static double diffX; + static double p_termX; + static double d_termX; + static double i_termX; + static double control_signalX; + + static double prev_errorY; + static double int_errorY; + static double curr_errorY; + static double diffY; + static double p_termY; + static double d_termY; + static double i_termY; + static double control_signalY; + + static double prev_errorYAW; + static double int_errorYAW; + static double curr_errorYAW; + static double diffYAW; + static double p_termYAW; + static double d_termYAW; + static double i_termYAW; + static double control_signalYAW; + // ---- CONTROLL PARAMETERS ------ // + + static double zeroMin,zeroMax,eps; double marpose[6]; diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp index 31ca1847bb69f18f260f6400e2378abd6d06529a..d3fb9faacecd172f053a28695759ef342cc0023a 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp @@ -43,6 +43,10 @@ using namespace cv; using namespace aruco; using namespace std; +double ImageConverter::prop_gain = .01; +double ImageConverter::integ_gain = .5; +double ImageConverter::deriv_gain = 0; + float ImageConverter::TheMarkerSize = -1; int ImageConverter::Thresh1_min = 5; @@ -62,20 +66,34 @@ bool ImageConverter::found = false; int ImageConverter::ThresParam1 = 0; int ImageConverter::ThresParam2 = 0; - -double ImageConverter::prev_error; -double ImageConverter::curr_error; -double ImageConverter::int_error; -double ImageConverter::diff; - -double ImageConverter::p_term = 0; -double ImageConverter::i_term = 0; -double ImageConverter::d_term = 0; -double ImageConverter::control_signal; - -double ImageConverter::prop_gain = 5; -double ImageConverter::integ_gain = 3; -double ImageConverter::deriv_gain = 0; +// ---- CONTROLL PARAMETERS ------ // +double ImageConverter::prev_errorX; +double ImageConverter::curr_errorX; +double ImageConverter::int_errorX; +double ImageConverter::diffX; +double ImageConverter::p_termX = 0; +double ImageConverter::i_termX = 0; +double ImageConverter::d_termX = 0; +double ImageConverter::control_signalX; + +double ImageConverter::prev_errorY; +double ImageConverter::curr_errorY; +double ImageConverter::int_errorY; +double ImageConverter::diffY; +double ImageConverter::p_termY = 0; +double ImageConverter::i_termY = 0; +double ImageConverter::d_termY = 0; +double ImageConverter::control_signalY; + +double ImageConverter::prev_errorYAW; +double ImageConverter::curr_errorYAW; +double ImageConverter::int_errorYAW; +double ImageConverter::diffYAW; +double ImageConverter::p_termYAW = 0; +double ImageConverter::i_termYAW = 0; +double ImageConverter::d_termYAW = 0; +double ImageConverter::control_signalYAW; +// ---- CONTROLL PARAMETERS ------ // double ImageConverter::zeroMax = .0000000000000000001; double ImageConverter::zeroMin = -.0000000000000000001; @@ -210,9 +228,9 @@ void ImageConverter::ProgStart(int argc,char** argv) const std::string vsa = "http://192.168.0.101:8080/video?x.mjpeg"; // -- publishing video stream with Android Camera-- - TheVideoCapturer.open(vsa); + //TheVideoCapturer.open(vsa); - //TheVideoCapturer.open(0); + TheVideoCapturer.open(1); // Check video is open if (!TheVideoCapturer.isOpened()) @@ -409,85 +427,61 @@ void ImageConverter::ContStart() void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) { -RefPose[0] = -.1; -RefPose[1] = .01; +RefPose[0] = -.0957; +RefPose[1] = .00740; RefPose[2] = .35; -RefPose[3] = 75.5; +RefPose[3] = -.68952; - ROS_INFO_STREAM(" Xmar = " << CamFB->pose.position.x << " m. \n"); - ROS_INFO_STREAM(" Xref = " << RefPose[0] << " m. \n"); + //ROS_INFO_STREAM(" Xmar = " << CamFB->pose.position.x << " m. \n"); + //ROS_INFO_STREAM(" Xref = " << RefPose[0] << " m. \n"); - ROS_INFO_STREAM(" Ymar = " << CamFB->pose.position.y << " m. \n"); - ROS_INFO_STREAM(" Yref = " << RefPose[1] << " m. \n"); + + // in Marker coordinate sys. + + // z => X robot (thrust) + // y => -Y robot (left - right) + // x => Z robot (NOT applicabale in our case!) + + // correspondingly ... + // roll in Marker coordinate => yaw in Robot coordinate ROS_INFO_STREAM(" Zmar = " << CamFB->pose.position.z << " m. \n"); ROS_INFO_STREAM(" Zref = " << RefPose[2] << " m. \n"); + ROS_INFO_STREAM(" Ymar = " << CamFB->pose.position.y << " m. \n"); + ROS_INFO_STREAM(" Yref = " << RefPose[1] << " m. \n"); + ROS_INFO_STREAM(" rollmar = " << CamFB->pose.orientation.x << " rad. \n"); ROS_INFO_STREAM(" rollref = " << RefPose[3] << " rad. \n"); ROS_INFO_STREAM(" ------------------------------------------------------------- \n"); - if (/*(abs(RefPose[0] - CamFB->pose.position.x) <= eps) &&*/ + if ( (abs(RefPose[1] - CamFB->pose.position.y) <= eps) && // Y - (abs(RefPose[2] - CamFB->pose.position.z) <= eps)) // Z + (abs(RefPose[2] - CamFB->pose.position.z) <= eps) && // Z + (abs(RefPose[3] - CamFB->pose.orientation.x) <= eps) // Yaw + ) { - if (((RefPose[3] - CamFB->pose.orientation.x) <= eps) /*&& // Yaw - ((RefPose[4] - CamFB->pose.orientation.y) <= eps) && - ((RefPose[5] - CamFB->pose.orientation.z) <= eps) && - ((RefPose[6] - CamFB->pose.orientation.w) <= eps)*/) - { + ROS_INFO("------------------------ Dock is completed ! ---------------------- \n "); keepMoving = false; - - }else if (abs(RefPose[3] - CamFB->pose.orientation.x) > eps) - { - ROS_INFO(" Calculating control signal For ROLL! \n "); - Controller(RefPose[3],CamFB->pose.orientation.x,.01,6); - - }/* else if (abs(RefPose[4] - CamFB->pose.orientation.y) > eps) - { - ROS_INFO(" Calculating control signal For PITCH! \n "); - Controller(RefPose[4],CamFB->pose.orientation.y,.01,5); - - } else if (abs(RefPose[5] - CamFB->pose.orientation.z) > eps) - { - ROS_INFO(" Calculating control signal For YAW! \n "); - Controller(RefPose[5],CamFB->pose.orientation.z,.01,6); - - } else if (abs(RefPose[6] - CamFB->pose.orientation.w > eps) - { - ROS_INFO(" Calculating control signal For W! \n "); - Controller(RefPose[6],CamFB->pose.orientation.w,.01,7); - }*/ - - - } /*else if (abs(RefPose[0] - CamFB->pose.position.x) > eps) + + } else { - ROS_INFO(" Calculating control signal For X-direction! \n "); - Controller(RefPose[0],CamFB->pose.position.x,.01,1); - - } */else if (abs(RefPose[1] - CamFB->pose.position.y) > eps) - { - ROS_INFO(" Calculating control signal For Y-direction! \n "); - Controller(RefPose[1],CamFB->pose.position.y,.01,2); - - } else if (abs(RefPose[2] - CamFB->pose.position.z) > eps) - { - ROS_INFO(" Calculating control signal For Z-direction! \n "); - Controller(RefPose[2],CamFB->pose.position.z,.01,1); + ROS_INFO(" Calculating control signal ! \n "); + Controller(RefPose[2], CamFB->pose.position.z, RefPose[1], CamFB->pose.position.y, RefPose[3], CamFB->pose.orientation.x,.01); } } -//void ImageConverter::Controller(double Reference[i], double RobPose[i], double dt) -void ImageConverter::Controller(double Reference, double CamPose, double dt, int index) +void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, double MarPoseY, double RefYAW, double MarPoseYAW, double dt) { + // -----------------X--------------------- // // e(t) = setpoint - actual value; - curr_error = Reference - CamPose; + curr_errorX = RefX - MarPoseX; // Integrated error - int_error += curr_error * dt; + int_errorX += curr_errorX * dt; /* // -- windup gaurd -- if (int_error < ) @@ -496,66 +490,87 @@ void ImageConverter::Controller(double Reference, double CamPose, double dt, int {}*/ // differentiation - diff = ((curr_error - prev_error) / dt); + diffX = ((curr_errorX - prev_errorX) / dt); // scalling - p_term = prop_gain * curr_error; - i_term = integ_gain * int_error; - d_term = deriv_gain * diff; + p_termX = prop_gain * curr_errorX; + i_termX = integ_gain * int_errorX; + d_termX = deriv_gain * diffX; // control signal - control_signal = p_term + i_term + d_term; + control_signalX = p_termX + i_termX + d_termX; // save the current error as the previous one // for the next iteration. - prev_error = curr_error; + prev_errorX = curr_errorX; - //dock(control_signal); + // -----------------Y--------------------- // + // e(t) = setpoint - actual value; + curr_errorY = RefY - MarPoseY; - if (index == 1) - { - dock(control_signal, 0, 0, 0, 0, 0, 0); - } else if (index == 2) - { - dock(0, -control_signal, 0, 0, 0, 0, 0); - } else if (index == 3) - { - dock(0, 0, control_signal, 0, 0, 0, 0); - }else if (index == 4) - { - dock(0, 0, 0, control_signal, 0, 0, 0); - } else if (index == 5) - { - dock(0, 0, 0, 0, control_signal, 0, 0); - } else if (index == 6) - { - dock(0, 0, 0, 0, 0, control_signal, 0); - } /*else if (index == 7) - { - dock(0, 0, 0, 0, 0, 0, control_signal); - }*/ + // Integrated error + int_errorY += curr_errorY * dt; + /* + // -- windup gaurd -- + if (int_error < ) + {} + else if () + {}*/ + + // differentiation + diffY = ((curr_errorY - prev_errorY) / dt); + + // scalling + p_termY = prop_gain * curr_errorY; + i_termY = integ_gain * int_errorY; + d_termY = deriv_gain * diffY; + + // control signal + control_signalY = p_termY + i_termY + d_termY; + + + // save the current error as the previous one + // for the next iteration. + prev_errorY = curr_errorY; + + // -------------------YAW--------------------------// + // e(t) = setpoint - actual value; + curr_errorYAW = RefYAW - MarPoseYAW; + + // Integrated error + int_errorYAW += curr_errorYAW * dt; + /* + // -- windup gaurd -- + if (int_error < ) + {} + else if () + {}*/ + + // differentiation + diffYAW = ((curr_errorYAW - prev_errorYAW) / dt); + + // scalling + p_termYAW = prop_gain * curr_errorYAW; + i_termYAW = integ_gain * int_errorYAW; + d_termYAW = deriv_gain * diffYAW; + + // control signal + control_signalYAW = p_termYAW + i_termYAW + d_termYAW; + + + // save the current error as the previous one + // for the next iteration. + prev_errorYAW = curr_errorYAW; + + dock(-control_signalX, -control_signalY, -control_signalYAW); } - -//void ImageConverter::dock(double speed[6]) -void ImageConverter::dock(double VelX, double VelY, double VelZ, double omegaX, double omegaY, double omegaZ, double omegaW) +void ImageConverter::dock(double VelX, double VelY, double omegaZ) { geometry_msgs::Twist msg; - /*msg.linear.x = speed[0]; - msg.linear.y = speed[1]; - msg.linear.z = speed[2]; - - msg.angular.x = speed[3]; - msg.angular.y = speed[4]; - msg.angular.z = speed[5];*/ - msg.linear.x = VelX; msg.linear.y = VelY; - msg.linear.z = VelZ; - - msg.angular.x = omegaX; - msg.angular.y = omegaY; msg.angular.z = omegaZ; commandPub.publish(msg); diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ index 47de5d7146659798a1b6f7bff084fccdb6a090fb..1868461d136e8ae7c4b09b3fe0a03a80b758c3bb 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ @@ -43,6 +43,10 @@ using namespace cv; using namespace aruco; using namespace std; +double ImageConverter::prop_gain = .01; +double ImageConverter::integ_gain = .5; +double ImageConverter::deriv_gain = 0; + float ImageConverter::TheMarkerSize = -1; int ImageConverter::Thresh1_min = 5; @@ -62,20 +66,34 @@ bool ImageConverter::found = false; int ImageConverter::ThresParam1 = 0; int ImageConverter::ThresParam2 = 0; - -double ImageConverter::prev_error; -double ImageConverter::curr_error; -double ImageConverter::int_error; -double ImageConverter::diff; - -double ImageConverter::p_term = 0; -double ImageConverter::i_term = 0; -double ImageConverter::d_term = 0; -double ImageConverter::control_signal; - -double ImageConverter::prop_gain = 15; -double ImageConverter::integ_gain = 3; -double ImageConverter::deriv_gain = 0; +// ---- CONTROLL PARAMETERS ------ // +double ImageConverter::prev_errorX; +double ImageConverter::curr_errorX; +double ImageConverter::int_errorX; +double ImageConverter::diffX; +double ImageConverter::p_termX = 0; +double ImageConverter::i_termX = 0; +double ImageConverter::d_termX = 0; +double ImageConverter::control_signalX; + +double ImageConverter::prev_errorY; +double ImageConverter::curr_errorY; +double ImageConverter::int_errorY; +double ImageConverter::diffY; +double ImageConverter::p_termY = 0; +double ImageConverter::i_termY = 0; +double ImageConverter::d_termY = 0; +double ImageConverter::control_signalY; + +double ImageConverter::prev_errorYAW; +double ImageConverter::curr_errorYAW; +double ImageConverter::int_errorYAW; +double ImageConverter::diffYAW; +double ImageConverter::p_termYAW = 0; +double ImageConverter::i_termYAW = 0; +double ImageConverter::d_termYAW = 0; +double ImageConverter::control_signalYAW; +// ---- CONTROLL PARAMETERS ------ // double ImageConverter::zeroMax = .0000000000000000001; double ImageConverter::zeroMin = -.0000000000000000001; @@ -409,85 +427,61 @@ void ImageConverter::ContStart() void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) { -RefPose[0] = -.1; -RefPose[1] = .01; +RefPose[0] = -.0957; +RefPose[1] = .00740; RefPose[2] = .35; -RefPose[3] = 75.5; +RefPose[3] = -.68952; - ROS_INFO_STREAM(" Xmar = " << CamFB->pose.position.x << " m. \n"); - ROS_INFO_STREAM(" Xref = " << RefPose[0] << " m. \n"); + //ROS_INFO_STREAM(" Xmar = " << CamFB->pose.position.x << " m. \n"); + //ROS_INFO_STREAM(" Xref = " << RefPose[0] << " m. \n"); - ROS_INFO_STREAM(" Ymar = " << CamFB->pose.position.y << " m. \n"); - ROS_INFO_STREAM(" Yref = " << RefPose[1] << " m. \n"); + + // in Marker coordinate sys. + + // z => X robot (thrust) + // y => -Y robot (left - right) + // x => Z robot (NOT applicabale in our case!) + + // correspondingly ... + // roll in Marker coordinate => yaw in Robot coordinate ROS_INFO_STREAM(" Zmar = " << CamFB->pose.position.z << " m. \n"); ROS_INFO_STREAM(" Zref = " << RefPose[2] << " m. \n"); + ROS_INFO_STREAM(" Ymar = " << CamFB->pose.position.y << " m. \n"); + ROS_INFO_STREAM(" Yref = " << RefPose[1] << " m. \n"); + ROS_INFO_STREAM(" rollmar = " << CamFB->pose.orientation.x << " rad. \n"); ROS_INFO_STREAM(" rollref = " << RefPose[3] << " rad. \n"); ROS_INFO_STREAM(" ------------------------------------------------------------- \n"); - if (/*(abs(RefPose[0] - CamFB->pose.position.x) <= eps) &&*/ + if ( (abs(RefPose[1] - CamFB->pose.position.y) <= eps) && // Y - (abs(RefPose[2] - CamFB->pose.position.z) <= eps)) // Z + (abs(RefPose[2] - CamFB->pose.position.z) <= eps) && // Z + (abs(RefPose[3] - CamFB->pose.orientation.x) <= eps) // Yaw + ) { - if (((RefPose[3] - CamFB->pose.orientation.x) <= eps) /*&& // Yaw - ((RefPose[4] - CamFB->pose.orientation.y) <= eps) && - ((RefPose[5] - CamFB->pose.orientation.z) <= eps) && - ((RefPose[6] - CamFB->pose.orientation.w) <= eps)*/) - { + ROS_INFO("------------------------ Dock is completed ! ---------------------- \n "); keepMoving = false; - - }else if (abs(RefPose[3] - CamFB->pose.orientation.x) > eps) - { - ROS_INFO(" Calculating control signal For ROLL! \n "); - Controller(RefPose[3],CamFB->pose.orientation.x,.01,6); - - }/* else if (abs(RefPose[4] - CamFB->pose.orientation.y) > eps) - { - ROS_INFO(" Calculating control signal For PITCH! \n "); - Controller(RefPose[4],CamFB->pose.orientation.y,.01,5); - - } else if (abs(RefPose[5] - CamFB->pose.orientation.z) > eps) - { - ROS_INFO(" Calculating control signal For YAW! \n "); - Controller(RefPose[5],CamFB->pose.orientation.z,.01,6); - - } else if (abs(RefPose[6] - CamFB->pose.orientation.w > eps) - { - ROS_INFO(" Calculating control signal For W! \n "); - Controller(RefPose[6],CamFB->pose.orientation.w,.01,7); - }*/ - - - } /*else if (abs(RefPose[0] - CamFB->pose.position.x) > eps) + + } else { - ROS_INFO(" Calculating control signal For X-direction! \n "); - Controller(RefPose[0],CamFB->pose.position.x,.01,1); - - } */else if (abs(RefPose[1] - CamFB->pose.position.y) > eps) - { - ROS_INFO(" Calculating control signal For Y-direction! \n "); - Controller(RefPose[1],CamFB->pose.position.y,.01,2); - - } else if (abs(RefPose[2] - CamFB->pose.position.z) > eps) - { - ROS_INFO(" Calculating control signal For Z-direction! \n "); - Controller(RefPose[2],CamFB->pose.position.z,.01,1); + ROS_INFO(" Calculating control signal ! \n "); + Controller(RefPose[2], CamFB->pose.position.z, RefPose[1], CamFB->pose.position.y, RefPose[3], CamFB->pose.orientation.x,.01); } } -//void ImageConverter::Controller(double Reference[i], double RobPose[i], double dt) -void ImageConverter::Controller(double Reference, double CamPose, double dt, int index) +void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, double MarPoseY, double RefYAW, double MarPoseYAW, double dt) { + // -----------------X--------------------- // // e(t) = setpoint - actual value; - curr_error = Reference - CamPose; + curr_errorX = RefX - MarPoseX; // Integrated error - int_error += curr_error * dt; + int_errorX += curr_errorX * dt; /* // -- windup gaurd -- if (int_error < ) @@ -496,66 +490,87 @@ void ImageConverter::Controller(double Reference, double CamPose, double dt, int {}*/ // differentiation - diff = ((curr_error - prev_error) / dt); + diffX = ((curr_errorX - prev_errorX) / dt); // scalling - p_term = prop_gain * curr_error; - i_term = integ_gain * int_error; - d_term = deriv_gain * diff; + p_termX = prop_gain * curr_errorX; + i_termX = integ_gain * int_errorX; + d_termX = deriv_gain * diffX; // control signal - control_signal = p_term + i_term + d_term; + control_signalX = p_termX + i_termX + d_termX; // save the current error as the previous one // for the next iteration. - prev_error = curr_error; + prev_errorX = curr_errorX; - //dock(control_signal); + // -----------------Y--------------------- // + // e(t) = setpoint - actual value; + curr_errorY = RefY - MarPoseY; - if (index == 1) - { - dock(control_signal, 0, 0, 0, 0, 0, 0); - } else if (index == 2) - { - dock(0, -control_signal, 0, 0, 0, 0, 0); - } else if (index == 3) - { - dock(0, 0, control_signal, 0, 0, 0, 0); - }else if (index == 4) - { - dock(0, 0, 0, control_signal, 0, 0, 0); - } else if (index == 5) - { - dock(0, 0, 0, 0, control_signal, 0, 0); - } else if (index == 6) - { - dock(0, 0, 0, 0, 0, control_signal, 0); - } /*else if (index == 7) - { - dock(0, 0, 0, 0, 0, 0, control_signal); - }*/ + // Integrated error + int_errorY += curr_errorY * dt; + /* + // -- windup gaurd -- + if (int_error < ) + {} + else if () + {}*/ + + // differentiation + diffY = ((curr_errorY - prev_errorY) / dt); + + // scalling + p_termY = prop_gain * curr_errorY; + i_termY = integ_gain * int_errorY; + d_termY = deriv_gain * diffY; + + // control signal + control_signalY = p_termY + i_termY + d_termY; + + + // save the current error as the previous one + // for the next iteration. + prev_errorY = curr_errorY; + + // -------------------YAW--------------------------// + // e(t) = setpoint - actual value; + curr_errorYAW = RefYAW - MarPoseYAW; + + // Integrated error + int_errorYAW += curr_errorYAW * dt; + /* + // -- windup gaurd -- + if (int_error < ) + {} + else if () + {}*/ + + // differentiation + diffYAW = ((curr_errorYAW - prev_errorYAW) / dt); + + // scalling + p_termYAW = prop_gain * curr_errorYAW; + i_termYAW = integ_gain * int_errorYAW; + d_termYAW = deriv_gain * diffYAW; + + // control signal + control_signalYAW = p_termYAW + i_termYAW + d_termYAW; + + + // save the current error as the previous one + // for the next iteration. + prev_errorYAW = curr_errorYAW; + + dock(-control_signalX, -control_signalY, -control_signalYAW); } - -//void ImageConverter::dock(double speed[6]) -void ImageConverter::dock(double VelX, double VelY, double VelZ, double omegaX, double omegaY, double omegaZ, double omegaW) +void ImageConverter::dock(double VelX, double VelY, double omegaZ) { geometry_msgs::Twist msg; - /*msg.linear.x = speed[0]; - msg.linear.y = speed[1]; - msg.linear.z = speed[2]; - - msg.angular.x = speed[3]; - msg.angular.y = speed[4]; - msg.angular.z = speed[5];*/ - msg.linear.x = VelX; msg.linear.y = VelY; - msg.linear.z = VelZ; - - msg.angular.x = omegaX; - msg.angular.y = omegaY; msg.angular.z = omegaZ; commandPub.publish(msg); diff --git a/MobileRobot/AugReaMarker/CamMark/devel/lib/camtomar/camtomar b/MobileRobot/AugReaMarker/CamMark/devel/lib/camtomar/camtomar index 37cb90a332aeda3501ade35e4881744c6442c1c8..c97d3b798cf20e95bfa133fb2fca7ea6b646bf8d 100755 Binary files a/MobileRobot/AugReaMarker/CamMark/devel/lib/camtomar/camtomar and b/MobileRobot/AugReaMarker/CamMark/devel/lib/camtomar/camtomar differ diff --git a/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp b/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp index 4c90b19eb7a2005a6eafb391f4bd966b86989a68..7b5dbffc159d2f18839fad624b74375d749232ac 100644 --- a/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp +++ b/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp @@ -254,10 +254,10 @@ int main(int argc,char **argv){ } // Print other rectangles that contains no valid markers - /** for (unsigned int i=0;i<MDetector.getCandidates().size();i++) { + for (unsigned int i=0;i<MDetector.getCandidates().size();i++) { aruco::Marker m( MDetector.getCandidates()[i],999); m.draw(TheInputImageCopy,cv::Scalar(255,0,0)); - }*/ + } // Draw a 3d cube in each marker if there is 3d info if (TheCameraParameters.isValid()){ diff --git a/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp~ b/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp~ index a168d8c91060306f44e96f8d3cfb7398fdce1393..4c90b19eb7a2005a6eafb391f4bd966b86989a68 100644 --- a/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp~ +++ b/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp~ @@ -125,8 +125,15 @@ int main(int argc,char **argv){ if (readArguments(argc,argv)==false) { return 0; } + + // IP address for raw3_lund + const std::string vsa = "http://192.168.0.101:8080/video?x.mjpeg"; - TheVideoCapturer.open(1); + // -- publishing video stream with Android Camera-- + TheVideoCapturer.open(vsa); + + + //TheVideoCapturer.open(1); // Check video is open if (!TheVideoCapturer.isOpened()) { cerr<<"Could not open video"<<endl; diff --git a/MobileRobot/AugReaMarker/ar_raw_detection/build/ar_det/CMakeFiles/ar_det.dir/src/ar_detection.cpp.o b/MobileRobot/AugReaMarker/ar_raw_detection/build/ar_det/CMakeFiles/ar_det.dir/src/ar_detection.cpp.o index a98ea80625f6ce742851db55d2e8a58eca16cc32..5432255b2c7a9608c246b223cdd85279282cc6ec 100644 Binary files a/MobileRobot/AugReaMarker/ar_raw_detection/build/ar_det/CMakeFiles/ar_det.dir/src/ar_detection.cpp.o and b/MobileRobot/AugReaMarker/ar_raw_detection/build/ar_det/CMakeFiles/ar_det.dir/src/ar_detection.cpp.o differ diff --git a/MobileRobot/AugReaMarker/ar_raw_detection/devel/lib/ar_det/ar_det b/MobileRobot/AugReaMarker/ar_raw_detection/devel/lib/ar_det/ar_det index 372c999c17a1eb60758e24d3413d35a9b9bd1948..f16c23cca75131daac0504890eb0355073b866f6 100755 Binary files a/MobileRobot/AugReaMarker/ar_raw_detection/devel/lib/ar_det/ar_det and b/MobileRobot/AugReaMarker/ar_raw_detection/devel/lib/ar_det/ar_det differ diff --git a/MobileRobot/Controller/PID/build/dock/CMakeFiles/dock.dir/src/dock.cpp.o b/MobileRobot/Controller/PID/build/dock/CMakeFiles/dock.dir/src/dock.cpp.o index 00a461ea1e92ab72127c7cafbb9f2fd7e06b9dde..d69e4b28ed6bff6038ab856d3bc7c55c705c7b1c 100644 Binary files a/MobileRobot/Controller/PID/build/dock/CMakeFiles/dock.dir/src/dock.cpp.o and b/MobileRobot/Controller/PID/build/dock/CMakeFiles/dock.dir/src/dock.cpp.o differ diff --git a/MobileRobot/Controller/PID/devel/lib/dock/dock b/MobileRobot/Controller/PID/devel/lib/dock/dock index 19e590c92ab00f26d0a46a9f264bfb3e84adf0e1..1acc90e1eeb0f363c1deb79f549dc8ac82561d4f 100755 Binary files a/MobileRobot/Controller/PID/devel/lib/dock/dock and b/MobileRobot/Controller/PID/devel/lib/dock/dock differ diff --git a/MobileRobot/Controller/PID/dock/src/dock.cpp b/MobileRobot/Controller/PID/dock/src/dock.cpp index 5743eb43e77a6168a17121cdba1a38610c562f20..cd75ca610f6bfa95bbe26c8db02b2792496c34b2 100644 --- a/MobileRobot/Controller/PID/dock/src/dock.cpp +++ b/MobileRobot/Controller/PID/dock/src/dock.cpp @@ -35,7 +35,6 @@ RobotDocking::RobotDocking() : ac("Robot_move", true) //PosSub = node.subscribe("/visualize_pose_ekf",1,&RobotDocking::RobCurrPose,this); - } diff --git a/MobileRobot/Controller/PID/dock/src/dock.cpp~ b/MobileRobot/Controller/PID/dock/src/dock.cpp~ index 2b5513180acd64439ee7f1975d84180942d16948..5743eb43e77a6168a17121cdba1a38610c562f20 100644 --- a/MobileRobot/Controller/PID/dock/src/dock.cpp~ +++ b/MobileRobot/Controller/PID/dock/src/dock.cpp~ @@ -45,7 +45,7 @@ void RobotDocking::ProgStart() while (ros::ok() && keepMoving) { - //dock(FORWARD_SPEED_MPS); + dock(FORWARD_SPEED_MPS); ros::spinOnce(); rate.sleep(); }