From a23d3d8db1b977e0962674a64996d3b49ddfda9b Mon Sep 17 00:00:00 2001
From: Farid Alijani <farid.alijani@student.lut.fi>
Date: Wed, 2 Mar 2016 22:58:34 +0100
Subject: [PATCH] y direction movement is still wrong
---
.../CamMark/camtomar/include/VisionControl.h | 4 +-
.../CamMark/camtomar/include/VisionControl.h~ | 3 +
.../CamMark/camtomar/src/VisionControl.cpp | 87 ++++++++++---------
3 files changed, 50 insertions(+), 44 deletions(-)
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
index 545a6ce6..42d85258 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
@@ -122,8 +122,8 @@ public:
static double Pos_Px,Pos_Ix,Pos_Dx;
static double Pos_Py,Pos_Iy,Pos_Dy;
- //static double S_yPos_P,S_yPos_I,S_yPos_D;
- //static double L_yPos_P,L_yPos_I,L_yPos_D;
+ static double S_yPos_P,S_yPos_I,S_yPos_D;
+ static double L_yPos_P,L_yPos_I,L_yPos_D;
static double S_Ang_P,S_Ang_I,S_Ang_D;
static double L_Ang_P,L_Ang_I,L_Ang_D;
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
index e1e6b02f..545a6ce6 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
@@ -122,6 +122,9 @@ public:
static double Pos_Px,Pos_Ix,Pos_Dx;
static double Pos_Py,Pos_Iy,Pos_Dy;
+ //static double S_yPos_P,S_yPos_I,S_yPos_D;
+ //static double L_yPos_P,L_yPos_I,L_yPos_D;
+
static double S_Ang_P,S_Ang_I,S_Ang_D;
static double L_Ang_P,L_Ang_I,L_Ang_D;
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index 30278f8a..20e7ad16 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
@@ -42,29 +42,25 @@ using namespace cv;
using namespace aruco;
using namespace std;
-
double ImageConverter::Pos_Px = .00005;
-double ImageConverter::Pos_Ix = 0.002;
+double ImageConverter::Pos_Ix = 0.0015;
double ImageConverter::Pos_Dx = 0;
+double ImageConverter::Pos_Py = 2.3 * ImageConverter::Pos_Px;
+double ImageConverter::Pos_Iy = 2.3 * ImageConverter::Pos_Ix;
+double ImageConverter::Pos_Dy = 2.3 * ImageConverter::Pos_Dx;
-double ImageConverter::Pos_Py = .39 * ImageConverter::Pos_Px;
-double ImageConverter::Pos_Iy = .39 * ImageConverter::Pos_Ix;
-double ImageConverter::Pos_Dy = .39 * ImageConverter::Pos_Dx;
-
-/*
-double ImageConverter::S_yPos_P = .16 * ImageConverter::Pos_Px;
-double ImageConverter::S_yPos_I = .16 * ImageConverter::Pos_Ix;
-double ImageConverter::S_yPos_D = .16 * ImageConverter::Pos_Dx;
+double ImageConverter::S_yPos_P = 2.3 * ImageConverter::Pos_Px;
+double ImageConverter::S_yPos_I = 2.3 * ImageConverter::Pos_Ix;
+double ImageConverter::S_yPos_D = 2.3 * ImageConverter::Pos_Dx;
-double ImageConverter::L_yPos_P = .3 * ImageConverter::Pos_Px;
-double ImageConverter::L_yPos_I = .3 * ImageConverter::Pos_Ix;
-double ImageConverter::L_yPos_D = .3 * ImageConverter::Pos_Dx;
-*/
+double ImageConverter::L_yPos_P = 15 * ImageConverter::Pos_Px;
+double ImageConverter::L_yPos_I = 15 * ImageConverter::Pos_Ix;
+double ImageConverter::L_yPos_D = 15 * ImageConverter::Pos_Dx;
-double ImageConverter::S_Ang_P = .065 * ImageConverter::Pos_Px;
-double ImageConverter::S_Ang_I = .065 * ImageConverter::Pos_Ix;
-double ImageConverter::S_Ang_D = .065 * ImageConverter::Pos_Dx;
+double ImageConverter::S_Ang_P = .062 * ImageConverter::Pos_Px;
+double ImageConverter::S_Ang_I = .062 * ImageConverter::Pos_Ix;
+double ImageConverter::S_Ang_D = .062 * ImageConverter::Pos_Dx;
double ImageConverter::L_Ang_P = .7 * ImageConverter::Pos_Px;
double ImageConverter::L_Ang_I = .7 * ImageConverter::Pos_Ix;
@@ -72,8 +68,8 @@ double ImageConverter::L_Ang_D = .7 * ImageConverter::Pos_Dx;
float ImageConverter::TheMarkerSize = .1; // Default marker size
-int ImageConverter::Thresh1_min = 20;
-int ImageConverter::Thresh2_min = 20;
+int ImageConverter::Thresh1_min = 10;
+int ImageConverter::Thresh2_min = 10;
int ImageConverter::Thresh1_max = 300;
int ImageConverter::Thresh2_max = 300;
@@ -120,8 +116,8 @@ double ImageConverter::control_signalYAW;
double ImageConverter::zeroMax = .0000000000000000001;
double ImageConverter::zeroMin = -.0000000000000000001;
-double ImageConverter::Pz_eps = .005;
-double ImageConverter::Py_eps = .007;
+double ImageConverter::Pz_eps = .001;
+double ImageConverter::Py_eps = .005;
double ImageConverter::A_eps = (CV_PI/180) * 1; // 1 deg.
@@ -140,6 +136,9 @@ double ImageConverter::A_eps = (CV_PI/180) * 1; // 1 deg.
keepMoving = true;
commandPub = nh_.advertise<geometry_msgs::Twist>("/base_controller/command",1);
Sub = nh_.subscribe("/marker_pose",1,&ImageConverter::camCB,this);
+
+
+
}
@@ -446,7 +445,7 @@ void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB)
{
RefPose[0] = -.0957;
-RefPose[1] = -0.0203153;
+RefPose[1] = -0.0193642;
RefPose[2] = 0.307922;
RefPose[3] = 0.705028;
@@ -516,7 +515,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
diffX = ((curr_errorX - prev_errorX) / dt);
// scalling
- p_termX = Pos_Px * curr_errorX;
+ p_termX = Pos_Px * curr_errorX;
i_termX = Pos_Ix * int_errorX;
d_termX = Pos_Dx * diffX;
@@ -532,8 +531,8 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
control_signalX = 0;
}
// -----------------Y--------------------- //
-
- if(abs(RefY - MarPoseY) > Py_eps)
+
+ if((RefY - MarPoseY) <= -Py_eps || (RefY - MarPoseY) >= Py_eps)
{
// e(t) = setpoint - actual value;
curr_errorY = RefY - MarPoseY;
@@ -555,31 +554,35 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
i_termY = Pos_Iy * int_errorY;
d_termY = Pos_Dy * diffY;
- /*int y_offset = 5;
- if (curr_errorY < y_offset && curr_errorY > -y_offset) // -5 < err < +5
+ /*double x_offset = .25;
+ if (abs(curr_errorY) >= abs(curr_errorX)) // -5 < err < +5
{
- ROS_INFO_STREAM(" INSIDE boundry of " << y_offset << " m. \n");
+ ROS_INFO_STREAM("robot movement in Y-direction is dominant! \n");
+ control_signalX = 0;
// scalling
- p_termYAW = S_yPos_P * curr_errorY;
- i_termYAW = S_yPos_I * int_errorY;
- d_termYAW = S_yPos_D * diffY;
- } else if (curr_errorY > y_offset || curr_errorYAW < y_offset) // err > +5 or err < -5
+ p_termY = L_yPos_P * curr_errorY;
+ i_termY = L_yPos_I * int_errorY;
+ d_termY = L_yPos_D * diffY;
+ } else
{
- ROS_INFO_STREAM(" OUTSIDE boundry of " << y_offset << " m. \n");
+ ROS_INFO_STREAM("robot movement in X-direction is dominant! \n");
// scalling
- p_termYAW = L_yPos_P * curr_errorY;
- i_termYAW = L_yPos_I * int_errorY;
- d_termYAW = L_yPos_D * diffY;
+ p_termY = S_yPos_P * curr_errorY;
+ i_termY = S_yPos_I * int_errorY;
+ d_termY = S_yPos_D * diffY;
}*/
// control signal
control_signalY = p_termY + i_termY + d_termY;
-
-
+
+ // robot & marker coordinates conversion
+ control_signalY = - control_signalY;
+
// save the current error as the previous one
// for the next iteration.
- prev_errorY = curr_errorY;
- } else
+ prev_errorY = curr_errorY;
+
+ }else if ((RefY - MarPoseY) < Py_eps && (RefY - MarPoseY) > -Py_eps)
{
control_signalY = 0;
}
@@ -644,7 +647,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
//ROS_INFO_STREAM("RAW Control signalYAW = "<< control_signalYAW <<". \n");
ROS_INFO_STREAM(" ------------------------------------------------------------- \n");
- dock(-control_signalX, -control_signalY, control_signalYAW);
+ dock(-control_signalX, control_signalY, control_signalYAW);
}
@@ -692,4 +695,4 @@ void ImageConverter::move2docking(double VelX_est, double VelY_est, double omega
ROS_INFO_STREAM(" Current ESTIMATED speed of robot: \n" << msg << ".\n");
}
-// ---- Controller part ----------- END --------
+// ---- Controller part -------- END ------
--
GitLab