From 3822cfc78c14d153b9e65a0749a4900cb25eabdd Mon Sep 17 00:00:00 2001
From: Farid Alijani <farid.alijani@student.lut.fi>
Date: Thu, 21 Apr 2016 21:54:40 +0200
Subject: [PATCH] PID controller modified
---
.../CamMark/camtomar/include/VisionControl.h | 38 +-
.../CamMark/camtomar/include/VisionControl.h~ | 41 +--
.../CamMark/camtomar/src/VisionControl.cpp | 339 ++++--------------
.../CamMark/camtomar/src/VisionControl.cpp~ | 21 +-
.../CamMark/camtomar/src/run_vis_cont.cpp | 8 +-
.../CamMark/camtomar/src/run_vis_cont.cpp~ | 8 +-
.../Machine_Learning/Practice/Q_Learning | Bin 14442 -> 14443 bytes
.../Machine_Learning/Practice/Q_learning.cpp | 10 +-
.../Machine_Learning/Practice/Q_learning.cpp~ | 14 +-
MobileRobot/Machine_Learning/Practice/iANDj | Bin 0 -> 9425 bytes
.../Practice/iANDj_publishe.cpp | 67 ++++
.../Practice/iANDj_publishe.cpp~ | 67 ++++
12 files changed, 248 insertions(+), 365 deletions(-)
create mode 100755 MobileRobot/Machine_Learning/Practice/iANDj
create mode 100644 MobileRobot/Machine_Learning/Practice/iANDj_publishe.cpp
create mode 100644 MobileRobot/Machine_Learning/Practice/iANDj_publishe.cpp~
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
index 0adcc11b..48e600a4 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
@@ -36,7 +36,7 @@ using namespace cv;
using namespace aruco;
using namespace std;
-class VisCont
+class PID4Docking
{
private:
@@ -98,9 +98,9 @@ private:
public:
- VisCont();
+ PID4Docking();
- ~VisCont();
+ ~PID4Docking();
Mat getCurrentImage();
@@ -159,42 +159,10 @@ public:
static double control_signalYAW;
// ---- CONTROLL PARAMETERS ------ //
-
- // ---- Q_LEARNING PARAMETERS ------ //
-
static double x_new,y_new,theta_new; // inp (y,theta)
static const double y_up,y_dwn,theta_up,theta_dwn;
static double Kp_y,Ki_y,Kd_y; // actions (Kp,Ki,Kd)
- static int R_indx_i, R_indx_j, P_indx_i, P_indx_j;
-
-
- static const int row, col;
- static double gamma;
- static double alpha;
- static double R_step;
-
- static double R[7][7];
- static double Q[7][7];
-
- static double Q_curr_state,Q_next_state;
- static double reward;
- static int counter;
- static int Time_Reward;
- static double sample;
-
- void Q_Learning(double y, double theta);
- void print_Q();
-
- void print_R();
-
- // ---- Q_LEARNING PARAMETERS ------ //
-
-
- // ---- TIMER PARAMETERS ------ //
- static int msec,sec,min,hr;
- // ---- TIMER PARAMETERS ------ //
-
static double docking_counter;
static double dock_started,dock_finished,docking_duration;
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
index 20512a5a..79669717 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
@@ -36,7 +36,7 @@ using namespace cv;
using namespace aruco;
using namespace std;
-class VisCont
+class PID4Docking
{
private:
@@ -98,9 +98,9 @@ private:
public:
- VisCont();
+ PID4Docking();
- ~VisCont();
+ ~PID4Docking();
Mat getCurrentImage();
@@ -159,42 +159,10 @@ public:
static double control_signalYAW;
// ---- CONTROLL PARAMETERS ------ //
-
- // ---- Q_LEARNING PARAMETERS ------ //
-
static double x_new,y_new,theta_new; // inp (y,theta)
static const double y_up,y_dwn,theta_up,theta_dwn;
static double Kp_y,Ki_y,Kd_y; // actions (Kp,Ki,Kd)
- static int R_indx_i, R_indx_j, P_indx_i, P_indx_j;
-
-
- static const int row, col;
- static double gamma;
- static double alpha;
- static double R_step;
-
- static double R[7][7];
- static double Q[7][7];
-
- static double Q_curr_state,Q_next_state;
- static double reward;
- static int counter;
- static int Time_Reward;
- static double sample;
-
- void Q_Learning(double y, double theta);
- void print_Q();
-
- void print_R();
-
- // ---- Q_LEARNING PARAMETERS ------ //
-
-
- // ---- TIMER PARAMETERS ------ //
- static int msec,sec,min,hr;
- // ---- TIMER PARAMETERS ------ //
-
static double docking_counter;
static double dock_started,dock_finished,docking_duration;
@@ -204,10 +172,9 @@ public:
static double Py_eps,Pz_eps,A_eps;
static double x_thresh_undock,y_thresh_undock,theta_thresh_undock;
static double rand_X_SM,rand_A_esp,rand_Y_esp;
- static double speed_reducer;
+ static double speed_reducer_X,speed_reducer_Y,speed_reducer_theta;
double marpose[6];
double camPose[6];
static const double RefPose[4];
-};
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index 34650155..94c0e546 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
@@ -42,140 +42,113 @@
#include<VisionControl.h>
+
+
using namespace cv;
using namespace aruco;
using namespace std;
-float VisCont::TheMarkerSize = .1; // Default marker size
+float PID4Docking::TheMarkerSize = .1; // Default marker size
-int VisCont::Thresh1_min = 10;
-int VisCont::Thresh2_min = 10;
+int PID4Docking::Thresh1_min = 10;
+int PID4Docking::Thresh2_min = 10;
-int VisCont::Thresh1_max = 300;
-int VisCont::Thresh2_max = 300;
+int PID4Docking::Thresh1_max = 300;
+int PID4Docking::Thresh2_max = 300;
-const string VisCont::trackbarWindowName = "Trackbars";
+const string PID4Docking::trackbarWindowName = "Trackbars";
-int VisCont::ThePyrDownLevel = 0;
+int PID4Docking::ThePyrDownLevel = 0;
-bool VisCont::update_images = true;
+bool PID4Docking::update_images = true;
-bool VisCont::found = false;
-bool VisCont::Go2RandomPose = false;
+bool PID4Docking::found = false;
+bool PID4Docking::Go2RandomPose = false;
-int VisCont::ThresParam1 = 0;
-int VisCont::ThresParam2 = 0;
+int PID4Docking::ThresParam1 = 0;
+int PID4Docking::ThresParam2 = 0;
// ---- CONTROLL PARAMETERS ------ //
-double VisCont::prev_errorX, VisCont::curr_errorX, VisCont::int_errorX, VisCont::diffX;
+double PID4Docking::prev_errorX, PID4Docking::curr_errorX, PID4Docking::int_errorX, PID4Docking::diffX;
-double VisCont::p_termX = 0;
-double VisCont::i_termX = 0;
-double VisCont::d_termX = 0;
+double PID4Docking::p_termX = 0;
+double PID4Docking::i_termX = 0;
+double PID4Docking::d_termX = 0;
-double VisCont::prev_errorY, VisCont::curr_errorY, VisCont::int_errorY, VisCont::diffY;
+double PID4Docking::prev_errorY, PID4Docking::curr_errorY, PID4Docking::int_errorY, PID4Docking::diffY;
-double VisCont::p_termY = 0;
-double VisCont::i_termY = 0;
-double VisCont::d_termY = 0;
+double PID4Docking::p_termY = 0;
+double PID4Docking::i_termY = 0;
+double PID4Docking::d_termY = 0;
-double VisCont::prev_errorYAW,VisCont::curr_errorYAW,VisCont::int_errorYAW,VisCont::diffYAW;
+double PID4Docking::prev_errorYAW,PID4Docking::curr_errorYAW,PID4Docking::int_errorYAW,PID4Docking::diffYAW;
-double VisCont::p_termYAW = 0;
-double VisCont::i_termYAW = 0;
-double VisCont::d_termYAW = 0;
+double PID4Docking::p_termYAW = 0;
+double PID4Docking::i_termYAW = 0;
+double PID4Docking::d_termYAW = 0;
-double VisCont::control_signalX, VisCont::control_signalY, VisCont::control_signalYAW;
+double PID4Docking::control_signalX, PID4Docking::control_signalY, PID4Docking::control_signalYAW;
// ---- CONTROLL PARAMETERS ------ //
// ---- Ref. Values for Android Camera ---- //
-//const double VisCont::RefPose[4] = {-.0957, 0.00638817 /* Y_ref*/ , 0.308857 /* X_ref*/ , 0.17 /* theta_ref*/};
+//const double PID4Docking::RefPose[4] = {-.0957, 0.00638817 /* Y_ref*/ , 0.308857 /* X_ref*/ , 0.17 /* theta_ref*/};
// ---- Ref. Values for Logitech Camera ---- //
-const double VisCont::RefPose[4] = {-.0957, -0.0271482 /* Y_ref*/ , 0.266214 /* X_ref*/ , -0.535344 /* theta_ref*/};
-
+const double PID4Docking::RefPose[4] = {-.0957, -0.0295876 /* Y_ref*/ , 0.218695 /* X_ref*/ , -0.637745 /* theta_ref*/};
// ---------------- PID gains---------------- //
-double VisCont::Kp_y = .7; //.7
-double VisCont::Ki_y = .005;//.005
-double VisCont::Kd_y = .2;
+double PID4Docking::Kp_y = .7; //.7
+double PID4Docking::Ki_y = .001;//.005
+double PID4Docking::Kd_y = .2;
-double VisCont::Kp_theta = .1 * Kp_y; // .08 * Kp_y
-double VisCont::Ki_theta = .07 * Ki_y;
-double VisCont::Kd_theta = .00005 * Kd_y;
+double PID4Docking::Kp_theta = .2 * Kp_y;
+double PID4Docking::Ki_theta = .6 * Ki_y; // .07 * Ki_y
+double PID4Docking::Kd_theta = .00005 * Kd_y;
// ---------------- PID gains---------------- //
-double VisCont::TT_S,VisCont::TT_E;
+double PID4Docking::TT_S,PID4Docking::TT_E;
// random pose initialized
-const double VisCont::y_up = .3;
-const double VisCont::y_dwn = -.1;
-
-const double VisCont::theta_dwn = -.7 /*-RefPose[3]*/;
-const double VisCont::theta_up = .7 /*RefPose[3]*/;
-
-double VisCont::x_new,VisCont::y_new,VisCont::theta_new;
-
-// ---------------- Q_LEARNING PARAMETERS ---------------- //
-
-double VisCont::gamma = .8;
-double VisCont::alpha = .1;
-double VisCont::R_step = 200;
+const double PID4Docking::y_up = .3;
+const double PID4Docking::y_dwn = -.1;
-const int VisCont::row = 5/*(y_up - y_dwn) * 10000*/;
-const int VisCont::col = 5/*(theta_up - theta_dwn) * 1000*/;
+const double PID4Docking::theta_dwn = -.7 /*-RefPose[3]*/;
+const double PID4Docking::theta_up = .7 /*RefPose[3]*/;
-double VisCont::Q[7][7] = {0};
-double VisCont::R[7][7] = {0};
+double PID4Docking::x_new,PID4Docking::y_new,PID4Docking::theta_new;
-double VisCont::Q_next_state;
-int VisCont::counter = 1;
-int VisCont::Time_Reward;
-double VisCont::sample;
+double PID4Docking::dock_started,PID4Docking::dock_finished,PID4Docking::docking_duration;
+double PID4Docking::zeroMax = .0000000000000000001;
+double PID4Docking::zeroMin = -.0000000000000000001;
-int VisCont::R_indx_i = 0;
-int VisCont::R_indx_j = 2;
-
-int VisCont::P_indx_i = 1;
-int VisCont::P_indx_j = 1;
-// ---------------- Q_LEARNING PARAMETERS ---------------- //
-
-double VisCont::dock_started,VisCont::dock_finished,VisCont::docking_duration;
-double VisCont::zeroMax = .0000000000000000001;
-double VisCont::zeroMin = -.0000000000000000001;
-
-double VisCont::speed_reducer_X = 1;
-double VisCont::speed_reducer_Y = 1;
-double VisCont::speed_reducer_theta = 1;
+double PID4Docking::speed_reducer_X = 1;
+double PID4Docking::speed_reducer_Y = 1;
+double PID4Docking::speed_reducer_theta = 1;
// ------ offsets X, Y, theta for Docking ---------
-double VisCont::Pz_eps = .001;
-double VisCont::Py_eps = .005;
-double VisCont::A_eps = (CV_PI/180) * 6; // 1 deg.
+double PID4Docking::Pz_eps = .001;
+double PID4Docking::Py_eps = .002;
+double PID4Docking::A_eps = (CV_PI/180) * 1; // 1 deg.
-double VisCont::safety_margin_X = .2; // safety margin X axis in docking process : 12 cm
+double PID4Docking::safety_margin_X = .15; // safety margin X axis in docking process : 18 cm
// ------ offsets X, Y, theta for Undocking ---------
-double VisCont::x_thresh_undock = .02;
-double VisCont::y_thresh_undock = .015;
-double VisCont::theta_thresh_undock = (CV_PI/180) * 3;
+double PID4Docking::x_thresh_undock = .02;
+double PID4Docking::y_thresh_undock = .015;
+double PID4Docking::theta_thresh_undock = (CV_PI/180) * 3;
-double VisCont::docking_counter = 1;
+double PID4Docking::docking_counter = 1;
-VisCont::VisCont()
+PID4Docking::PID4Docking()
{
keepMoving = true;
/* initialize random seed: */
- //srand (time(NULL));
-
-
- R[R_indx_i][R_indx_j] = 50; // reward
- R[P_indx_i][P_indx_j] = -60; // punishment
+ srand (time(NULL));
x_rand_SM = RefPose[2] + .55; // 55 cm spreading domain in the x-axis while moving towards the random pose
@@ -183,25 +156,25 @@ VisCont::VisCont()
MarPose_pub = node_vis.advertise<geometry_msgs::PoseStamped>("/marker_pose", 100);
commandPub = node_cont.advertise<geometry_msgs::Twist>("/base_controller/command",1);
- MarPose_Sub = node_vis.subscribe("/marker_pose",1,&VisCont::camCB,this);
+ MarPose_Sub = node_vis.subscribe("/marker_pose",1,&PID4Docking::camCB,this);
}
- VisCont::~VisCont()
+ PID4Docking::~PID4Docking()
{
}
- Mat VisCont::getCurrentImage()
+ Mat PID4Docking::getCurrentImage()
{
return img;
}
-void VisCont::cvTackBarEvents(int value,void* ptr)
+void PID4Docking::cvTackBarEvents(int value,void* ptr)
{
- VisCont* ic = (VisCont*)(ptr);
+ PID4Docking* ic = (PID4Docking*)(ptr);
ic-> myhandler(value);
}
-void VisCont::myhandler(int value)
+void PID4Docking::myhandler(int value)
{
if (Thresh1_min<3) Thresh1_min=3;
@@ -227,14 +200,14 @@ void VisCont::myhandler(int value)
//imshow("INPUT IMAGE",TheInputImageCopy);
//imshow("THRESHOLD IMAGE",MDetector.getThresholdedImage());
}
-void VisCont::createTrackbars()
+void PID4Docking::createTrackbars()
{
namedWindow(trackbarWindowName, 0);
createTrackbar("ThresParam 1", trackbarWindowName, &Thresh1_min, Thresh1_max, cvTackBarEvents, this);
createTrackbar("ThresParam 2", trackbarWindowName, &Thresh2_min, Thresh2_max, cvTackBarEvents, this);
}
-void VisCont::imageCb(const sensor_msgs::ImageConstPtr& msg)
+void PID4Docking::imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
try
@@ -249,7 +222,7 @@ void VisCont::imageCb(const sensor_msgs::ImageConstPtr& msg)
img = cv_ptr->image;
}
-bool VisCont::readArguments ( int argc,char **argv )
+bool PID4Docking::readArguments ( int argc,char **argv )
{
if (argc<2) {
cerr<< "Invalid number of arguments!\n" <<endl;
@@ -266,7 +239,7 @@ bool VisCont::readArguments ( int argc,char **argv )
return true;
}
-void VisCont::ProgStart(int argc,char** argv)
+void PID4Docking::ProgStart(int argc,char** argv)
{
// Show images, press "SPACE" to diable image
// rendering to save CPU time
@@ -357,10 +330,10 @@ if (TheVideoCapturer.retrieve(TheInputImage))
x_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[1];
z_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[2];
- cv::Mat R(3,3,cv::DataType<float>::type);
+ Mat R(3,3,cv::DataType<float>::type);
// You need to apply cv::Rodrigues() in order to obatain angles wrt to camera coords
- cv::Rodrigues(TheMarkers[0].Rvec,R);
+ Rodrigues(TheMarkers[0].Rvec,R);
// ----------- Euler angle -----------------//
float roll1 = -asin(R.at<float>(2,0));
@@ -459,15 +432,13 @@ if (key == ' ')
}
ros::spinOnce();
- //rate.sleep();
-TT_E = ros::Time::now().toSec();
-ROS_INFO_STREAM("start = "<< TT_S << " , end = " << TT_E << " , duration = " << (TT_E - TT_S) <<" \n");
-
- }
+ TT_E = ros::Time::now().toSec();
+ ROS_INFO_STREAM(" visualization while loop duration = " << (TT_E - TT_S) <<" \n");
+ }
}
-void VisCont::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) // subscriber
+void PID4Docking::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) // subscriber
{
camPose[0] = CamFB->pose.position.x; // not important!!!
camPose[1] = CamFB->pose.position.y; // y pose
@@ -539,9 +510,6 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation
}else
{
Controller(RefPose[2], camPose[2], RefPose[1], camPose[1], RefPose[3], camPose[3],.1);
- //Q_Learning(camPose[1],camPose[3]); // performing Q_Learning...
- //print_R();
-
}
} else
{
@@ -550,7 +518,7 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation
}
}
-void VisCont::Controller(double RefX, double MarPoseX, double RefY, double MarPoseY, double RefYAW, double MarPoseYAW, double dt)
+void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double MarPoseY, double RefYAW, double MarPoseYAW, double dt)
{
ROS_INFO_STREAM("--------------------- Controller started ----------------------\n ");
@@ -659,11 +627,10 @@ void VisCont::Controller(double RefX, double MarPoseX, double RefY, double MarPo
//ROS_INFO_STREAM("Control signalYAW = "<< control_signalYAW <<". \n");
ROS_INFO_STREAM(" ---------------------- Controller ended ----------------------- \n");
- //dock(-control_signalX, control_signalY, control_signalYAW);
dock(control_signalX, control_signalY, control_signalYAW);
}
-void VisCont::dock(double VelX, double VelY, double omegaZ)
+void PID4Docking::dock(double VelX, double VelY, double omegaZ)
{
ROS_INFO(".... REAL .... !");
geometry_msgs::Twist msg;
@@ -677,7 +644,7 @@ void VisCont::dock(double VelX, double VelY, double omegaZ)
ROS_INFO_STREAM(" Current speed of robot: \n" << msg << ".\n");
}
-void VisCont::move2docking(double VelX_est, double VelY_est, double omegaZ_est)
+void PID4Docking::move2docking(double VelX_est, double VelY_est, double omegaZ_est)
{
ROS_INFO_STREAM(" Zmar = " << camPose[2] << " m. \n");
@@ -709,7 +676,7 @@ void VisCont::move2docking(double VelX_est, double VelY_est, double omegaZ_est)
}
// ---- Controller part -------- END ------
-void VisCont::GenerateRandomVal()
+void PID4Docking::GenerateRandomVal()
{
// ---------------- PID gains ------------------
@@ -724,7 +691,7 @@ void VisCont::GenerateRandomVal()
theta_new = ((double) rand() / (RAND_MAX)) * (theta_up - theta_dwn) + theta_dwn; // will be used for Q_Learning
}
-void VisCont::RandomPose(double X_rand, double Y_rand, double theta_rand)
+void PID4Docking::RandomPose(double X_rand, double Y_rand, double theta_rand)
{
ROS_INFO_STREAM(" Xr = " << X_rand << ", Yr = " << Y_rand << ", Thetar = " << theta_rand << " rad ~ " << theta_rand * (180/CV_PI) << " deg\n");
ROS_INFO_STREAM(" -------------------------------------------------------------- \n");
@@ -827,153 +794,3 @@ geometry_msgs::Twist msg_new;
commandPub.publish(msg_new);
}
-
-void VisCont::Q_Learning(double y, double theta)
-{
-
-/* while ( iterations <= it_ )
- {
- if (user_action == 1 && y_new != 0) // North
- {
- reward = R[y_new][theta_new];
- Q_next_state = Q[y_new - 1][theta_new];
-
- sample = reward + gamma * Q_next_state;
- Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);
- Q_curr_state = Q_next_state;
- i--;
- counter++;
- } else if (user_action == 1 && y_new == 0) // North
- {
- cout << "You can't go further up!\n";
- } else if (user_action == 3 && i < (row - 1)) // South, i < row
- {
- reward = R[y_new][theta_new];
- Q_next_state = Q[y_new + 1][theta_new];
-
-
-
-
- sample = reward + gamma * Q_next_state;
- Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);
- Q_curr_state = Q_next_state;
- i++;
- counter++;
- } else if (user_action == 3 && y_new >= (row - 1)) // South
- {
- cout << "You can't go further down!\n";
- } else if (user_action == 2 && theta_new < (col - 1)) // East
- {
- reward = R[y_new][theta_new];
- Q_next_state = Q[i][j + 1];
-
- sample = reward + gamma * Q_next_state;
- Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);
- Q_curr_state = Q_next_state;
- j++;
- counter++;
- } else if (user_action == 2 && j >= (col - 1)) // East, j > col
- {
- cout << "You can't go further right!\n";
- } else if (user_action == 4 && j != 0 ) // West
- {
- reward = R[y_new][theta_new];
- Q_next_state = Q[y_new][theta_new - 1];
-
- sample = reward + gamma * Q_next_state;
- Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);
- Q_curr_state = Q_next_state;
- j--;
- counter++;
- } else if (user_action == 4 && theta_new == 0) // West, j = 1
- {
- cout << "You can't go further left!\n";
- } else
- {
- cout << "\nGenerating random pose in grid for 1st. time!\n";
- generate_rand();
- }
-
- // + Reward
- if (y_new == R_indx_i && theta_new == R_indx_j)
- {
- Time_Reward = -counter;
- cout << " Time Reward = "<< Time_Reward << "\n";
-
- if(abs(Time_Reward) <= R_step)
- {
-
- cout << "\n Goal is achieved <= " << R_step << " time steps\n";
- reward = R[y_new][theta_new];
- Q_next_state = 0;
-
- sample = reward + gamma * Q_next_state;
- Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);
- } else
- {
- cout << "\n Goal is achieved > " << R_step << " time steps => time_punishment\n";
- reward = -1; // ???
- Q_next_state = 0;
-
- sample = reward + gamma * Q_next_state;
- Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);
- }
-
- counter = 0;
- } else if (y_new == P_indx_i && theta_new == P_indx_j) // (-) Reward => Punishment
- {
- cout << "\n Failed to achieve a goal! \n";
-
- reward = R[y_new][theta_new];
- Q_next_state = 0;
-
- sample = reward + gamma * Q_next_state;
- Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);
- }
-
- cout << "\n Q_value = " << Q_curr_state << " , actions N(1), E(2), S(3), W(4) : ";
- user_action = ((double) rand() / (RAND_MAX)) * (5 - 1) + 1; // Generating random user_action
- printf(" ramdom user action = %i \n",user_action);
- //cin >> user_action;
- }*/
-}
-
-
-void VisCont::print_R()
-{
- cout << " R = \n";
- for(int i = 0; i <= (row - 1); i++)
- {
- for(int j = 0; j <= (col - 1); j++)
- {
- cout << setw(col - 1) << R[i][j];
- if(j < col - 1)
- {
- cout << " , ";
- }
- } // j
- cout << "\n";
- } // i
- cout << "\n";
-}
-
-
-void VisCont::print_Q()
-{
- cout << " Q = \n";
- for(int i = 0; i <= (row - 1); i++)
- {
- for(int j = 0; j <= (col - 1); j++)
- {
- cout << setw(col - 1) << Q[i][j];
- if(j < col - 1)
- {
- cout << " , ";
- }
- } // j
- cout << "\n";
- } // i
- cout << "\n";
-}
-
-
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
index 9d8c3410..34650155 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
@@ -98,12 +98,12 @@ const double VisCont::RefPose[4] = {-.0957, -0.0271482 /* Y_ref*/ , 0.266214 /*
// ---------------- PID gains---------------- //
-double VisCont::Kp_y = .5; //.6
-double VisCont::Ki_y = .05;
-double VisCont::Kd_y = .3;
+double VisCont::Kp_y = .7; //.7
+double VisCont::Ki_y = .005;//.005
+double VisCont::Kd_y = .2;
double VisCont::Kp_theta = .1 * Kp_y; // .08 * Kp_y
-double VisCont::Ki_theta = .002 * Ki_y;
+double VisCont::Ki_theta = .07 * Ki_y;
double VisCont::Kd_theta = .00005 * Kd_y;
// ---------------- PID gains---------------- //
@@ -153,10 +153,10 @@ double VisCont::speed_reducer_theta = 1;
// ------ offsets X, Y, theta for Docking ---------
double VisCont::Pz_eps = .001;
-double VisCont::Py_eps = .004;
+double VisCont::Py_eps = .005;
double VisCont::A_eps = (CV_PI/180) * 6; // 1 deg.
-double VisCont::safety_margin_X = .18; // safety margin X axis in docking process : 12 cm
+double VisCont::safety_margin_X = .2; // safety margin X axis in docking process : 12 cm
// ------ offsets X, Y, theta for Undocking ---------
double VisCont::x_thresh_undock = .02;
@@ -533,7 +533,7 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation
)
{
ROS_INFO("y & theta fixed successfully, MOVING STRAIGHT AHEAD ... \n");
- speed_reducer_X = .1;
+ speed_reducer_X = .08;
Controller(RefPose[2], camPose[2], RefPose[1], RefPose[1], RefPose[3], RefPose[3],.1);
}
}else
@@ -571,10 +571,10 @@ void VisCont::Controller(double RefX, double MarPoseX, double RefY, double MarPo
// control signal
control_signalX = p_termX + i_termX + d_termX;
prev_errorX = curr_errorX;*/
- control_signalX = speed_reducer_X * 0.08;
+ control_signalX = speed_reducer_X * 0.1;
} else
{
- control_signalX = 1e-4;
+ control_signalX = 5e-5;
}
// -----------------Y--------------------- //
@@ -613,7 +613,8 @@ void VisCont::Controller(double RefX, double MarPoseX, double RefY, double MarPo
{
control_signalY = 0;
}
- // -------------------YAW--------------------------//
+
+ // -------------------YAW--------------------------//
if(abs(abs(RefYAW) - abs(MarPoseYAW)) > A_eps)
{
// e(t) = setpoint - actual value;
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp
index 6c52b0ba..fbdef348 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp
@@ -5,12 +5,8 @@ int main(int argc, char** argv)
{
ros::init(argc, argv, "aruco_tf_publisher");
- VisCont imgConv;
- imgConv.ProgStart(argc,argv);
-
-
+ PID4Docking controller;
+ controller.ProgStart(argc,argv);
return 0;
-
-
}
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp~
index 6c52b0ba..98e8f518 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/run_vis_cont.cpp~
@@ -5,12 +5,12 @@ int main(int argc, char** argv)
{
ros::init(argc, argv, "aruco_tf_publisher");
- VisCont imgConv;
- imgConv.ProgStart(argc,argv);
+ PID4Docking controller;
+ controller.ProgStart(argc,argv);
+/*PID4Docking cont;
+cont.ContStart();*/
return 0;
-
-
}
diff --git a/MobileRobot/Machine_Learning/Practice/Q_Learning b/MobileRobot/Machine_Learning/Practice/Q_Learning
index 0d0d3255a39c1b2f6342705481ca0d3e9dd01f59..63742263fbc07be2b1b8928dd5f6787fced58998 100755
GIT binary patch
delta 290
zcmaD=@Va0_3zNvixP(<J*lPts6Xt#0{bWmn;D&_C%`=!Ds4<@2T&Y>jAu7lq#bBV8
zzyJaqAcBE`;lkvPhT@D*CMz0=OMYNxU;wFMP==`KV1y|8G&#^noRMp?ps_fk?Bqrx
zaY=a=1_l9!kN-2Era{y(FsMx4Xe2JF&jK<>l3|Jh#2gl=w8i9yM&gn-EDQ`n47?1#
zMI6B5XTc-`g9pfb$tbAFOQ3q8k_?F;S;lo>^1$RmV{yi#n>&qfF)IYd=cQJZ#21$&
fmZUPoM;V%AI{LX}#RnASr<5kA7L`qIGJgR8393aS
delta 289
zcmaD|@Ty=#3zJCFmnV$dOw&c8gBT~L9#Odc^$U0I<{3;6)EG}~uGFmN5EWwJWtb}F
zzyJbVj3APM;o9VnhT@EmCo39>OTK4jU=U=GVqj2C084g2)qI>BXe7?aIa$zHoKa?S
zqmj6z918=30K>=s8EO!fVC4)949b%?8i`Blv4G5xWSF7=k!68On@@gdBra*q!oUD>
z$!`&eOU^=N-9hF{MnX+q0@VwZWJmzXGOh)a`zIF~i!&bC+-ZD^Sw23>&^SFcFSRJK
dBsIP$F)yWzAuv8KwW1`xxFoS8b#k-$3jk#&M8*IB
diff --git a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp
index 1e1d2a7d..850990ed 100644
--- a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp
+++ b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp
@@ -39,7 +39,7 @@ double sample;
void print_R();
void print_Q();
-void generate_rand();
+void iANDj_Generator();
int main()
{
@@ -115,7 +115,7 @@ int main()
} else
{
cout << "\nGenerating random pose in grid for 1st. time!\n";
- generate_rand();
+ iANDj_Generator();
}
// + Reward
@@ -145,7 +145,7 @@ int main()
counter = 0;
print_Q();
- generate_rand();
+ iANDj_Generator();
iterations++;
} else if (i == P_indx_i && j == P_indx_j) // - Reward => Punishment
{
@@ -158,7 +158,7 @@ int main()
Q[i][j] = ((1 - alpha) * Q[i][j]) + (alpha * sample);
print_Q();
- generate_rand();
+ iANDj_Generator();
iterations++;
//Goal = true;
}
@@ -206,7 +206,7 @@ void print_Q()
cout << "\n";
}
-void generate_rand()
+void iANDj_Generator()
{
// Generate Random Pose for current state (position)
diff --git a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~ b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~
index 4ed5d34d..d67d1189 100644
--- a/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~
+++ b/MobileRobot/Machine_Learning/Practice/Q_learning.cpp~
@@ -27,8 +27,8 @@ double Q_curr_state = Q[i][j];
double reward;
-int R_indx_i = row - row;
-int R_indx_j = .5 * col;
+int R_indx_i = 5/*row - row*/;
+int R_indx_j = 3/*.5 * col*/;
int P_indx_i = row - 2;
int P_indx_j = col - 1;
@@ -39,7 +39,7 @@ double sample;
void print_R();
void print_Q();
-void generate_rand();
+void iANDj_Producer();
int main()
{
@@ -115,7 +115,7 @@ int main()
} else
{
cout << "\nGenerating random pose in grid for 1st. time!\n";
- generate_rand();
+ iANDj_Producer();
}
// + Reward
@@ -145,7 +145,7 @@ int main()
counter = 0;
print_Q();
- generate_rand();
+ iANDj_Producer();
iterations++;
} else if (i == P_indx_i && j == P_indx_j) // - Reward => Punishment
{
@@ -158,7 +158,7 @@ int main()
Q[i][j] = ((1 - alpha) * Q[i][j]) + (alpha * sample);
print_Q();
- generate_rand();
+ iANDj_Producer();
iterations++;
//Goal = true;
}
@@ -206,7 +206,7 @@ void print_Q()
cout << "\n";
}
-void generate_rand()
+void iANDj_Producer()
{
// Generate Random Pose for current state (position)
diff --git a/MobileRobot/Machine_Learning/Practice/iANDj b/MobileRobot/Machine_Learning/Practice/iANDj
new file mode 100755
index 0000000000000000000000000000000000000000..8d9a6c4c77a9d0c73d53f74f81a235afbeaf7bcd
GIT binary patch
literal 9425
zcmb<-^>JfjWMqH=CI&kO5bpw;16T+`GB7L<1arZJ1A_$vCxZinJcA4a8v_FaD+2=q
zOq~Oi1*3m}3}awmfYBUa6Brnn85kH?7#J8Vm>>ccOb`=bv<yU;0Y*cO0lN)kA5<E}
zW{}t%9uUdE0HYZg6u<%?{UEn~;01FTKJX@h88Es6>W&W}M}cJ^`ar@UeF}VFK?Vgr
zh&~v70Aw%&0|ShP`440a2#0_SU|?Vff%*?dyFiR%fYBheAfbS#B`F|w0XK-pz)-*q
z2^Sb`0}*C`(IB-Tp}?mlDIj-(*u-F%JA<J1;R+W4sQY0w)VmD&IhjdjCi*!kx;dG7
zr4_mr7G}C;CVIvBdPd-Q6<}Zh$CbNZDA+k5V?gecfX2HB10)TA<QI7D4aqZ@ern^w
z7c-l`nAhYx&1?mw4^F5VAbkuB0!)mIEDRtqkbztb3=GB7*qJn3ulk72;Bf|tVZ)gW
z3=Be8BtmeAgWL^?c~njE46u}hD!v+rIk#|#pT!|wj6>X;fq_AgL5iV40Fv$`pb=mU
zRlfn6PhqZ#hpJy72vIKpRnL%<nN(bolB}%_iX$_I`1thP{Ji+$lEk8t_;`kRud?`{
z)bz~alGGySoW$bd)MAGCD8Jwmi_HAu_@u<*R1?p<%n}zv*D{9qsNfRg<jg#<g3O{~
z*Ps-zm`QSeX$e>`Kd0C=6D*cf>=|5QXqcRlSQKAUl$cpk?3wK98Wdb&Xq=Q-oS7V-
zUtCg@nwaaE91`yuY#JZnoeVYs&0c4ay*Zgl$uP$!S0u(KmZVl>mN3M}gCw9n$xY16
zV{rHJbaIY2(lgRCg|H(c;*Ir87#J9s7(nR|L^CrmgLyC!Mg~TP!wd`zu=p>P%H#y4
z&^OS41<8Yg5+n~2D$$(80W#+Yh{M3Z@PHo@-k`J!O1n^Dc)WqiELhqF$-&|WBo58$
zAYlduXgsq+(>_Fyfq?@_9O4Ivi~y22$SjB`1A_#TILJ*faRnrCkR32_4J2_;-hzo6
zAc@1u43L~h^BayAyBQc5UU)GwFua(`!oc9se1zlhf74l<3Jm{M=Wr@8@XI?e{8t6>
zGeBBiKKTFt|9{m<oC*vXprCwt0nG0L@j+gCc>v6B0`WmX_;Lf7Uj^cWin*5y!2BW*
zALP}S6Tti|5Fg~lmks~_|2KT=*?BR}qw|qR^OFw&A)dz%T;OtG@Hl?Z0Zbj=!tTKE
zV*3C8|G!L_0P=%JH|tFf1qP4S10_uVFMz!;0U3mPbUqCRDQvK9;8tK@DD~bgz?Z<_
z(al=Lt-#>X_?iR6JNTQ~qxm(Xhvki8l{9{N7f@gvVD#u@4dqr~_%Cu4tf-sS9i+wb
zM$w~Vtj63542&S_qGJ!IrKRcd%eR2Sh{2=Twt-E7fdOQ1iO>#^7|3lLvHK<X5*YqJ
zh&?=E0!Yp=1mr4(<E}42v2xt?4T$>0FTh&Op}_EoUw}6oOvfDj#INQ1<`aM1!B6~x
zt`|TmGQlzeypdoUqT&Ti#f^Oi+zt%;LCN#Qf`9-2?>ixsz_1@w`n+%iajoF8O(5<C
zxUBNO|NoD>J^(rRWg`Ov*bUFwL2l7<eej82K=c<ANHF5iC;mv+3u*is7eDdGfapUY
z*Sc~kFr@M8o#3B*pyfb`!zX@-Cf=hEO$Wi6KnkaTjf^-5Vhcbt9ps-5(!|~E`k<5b
z4W|ObYvE?s2aL_N4><Ve9cVeg-&*h=<hCa$Zi@yRq~-bq<hD47HDI^hKzG|hP7Jq!
zH1TReG=bfA1LPhKu#phA<v=tc-1elCH3aImCydRtPmtZ#Eo;fCz~IuYs>i9ou+IY?
z#PWat|99zp=Fxfl#g;$+|L^nXc3=Q;*#G|j-+2$jnhO$+gbIK7^Z!32#WWvLh<1#L
zJq$`I9?fqAI&1%Qmi}n2{li$Q)mZ!E|9=LCQpNo@gc2BDRD(2uYLQ(I0tpQJKw=<+
zTmJn2e-f;JA4v6nP=<Z6_W%F?pah7NK|DHhRBrfmrl>sd=+#}$tia&Yt2zTjb>^tN
z@aZg3`QX!8qw>R}Ge+ftNAnvCAI+zZoqs($zj++|%k0^C{l(_r|NncmzAce4ybUTZ
zd^&&mbbfQ`eDC;Q<c>%4VMY(j*QKvKx?@y0Ji2pK1bn(vR3v=5b5s<3x=U0ve7b8?
z3_O~TSRCe;Zve&b=fg;jn*hlxpq#?t(ap-itia&W{NsO#<nAB52@KGD`kLea1&?lK
zkVNT6XpV%G4KMzK+y*MuKy4XN5yim3rBJD0t6;Cd#lWRdl95`H2<K%g*eWn6B<7_k
zWPw;*4B)m3a+%M-@W7r?n%NrEb^{ey4}SgsAHl%D;PU7He~{zz{{8>If`Nfy-M|0;
zPcSerT=@6@{|g2NhJ^qB|HJBWP`e4_PsXYs2F3~jMrj^)jtPtm3@Qu^;I?wZFNj$T
zd;)HK5?=h=<s1zR_EOeb#wto6eIRw9Hs_w-|Nm=&yyVCy(8lD<%f^)4%+kZme29^U
z9aNNn+TA$}3=I4J{{Ihdfx^^PBjkG+7#QyT{r_JSWH3xV03r{vXAP)c1SM6Fd|(f2
z8(Tb|Lo<6nQy+6L3sVFi$R3b?t}rk#c>MeSALJa6(osyi5Rl^n$0h@)>I7k!O&~T1
z!}u^7l<z@obQ;uF0*OPrTMQP^b||!a#UKIgw@88%F)+aUThdTIDDFYbumAq%gXkI1
z{t>8R2k~DpL);0<Cm{X_s64D~!UA;=tnCEqSAf)k(;|og#h``+i1`BAK7*w(P(A~R
zgVG;}hP8#nK^z7Kh6$_?`(ff9P(IAv9#D0#{0+1J-+zdIIH2nPL-{a!KS23!SRm#@
zjiEUO>#w3)lm?BrGAP{!rKdsZWl(w>ls*Qf(akXigU9>9l<{MG2z?pKzijXB>};i=
z5t>w*S5m6)lbM%Tl&@f-XQpSYYgh^vG}VOg85s16D|1T{lNj`hONt<L28@-NSCU#(
zz@V3xUy`cl=;Wzel9&$VrRSCEC6#98r08a*FzA7J8HvRi40<V*dBv5v5W1uYB2$)H
zRGgWghr)?3V$dr}%}E4lfU*j5N*MGq^D;{q^h#1IN*MIgGV?MS^osIR5=#;p^inh8
z(~1&vQ{yvIiV!@Aj`+l)qQpuDz2uVoqGATU6bO}-Tm*JVVs2(KgI;=m37F6W+XFE(
zskoRyFF8LqH#M&WJ&4KTgYp|FtH9D9Y<vYa?gA?3kqri^Mdm9rFff4f08Bq@`~o(v
zf!<dJHPn&y8=~nif#%~9sDYq*0$Dw}eo$TY?f?ILsB#8aIWYq&U;xz&D>q>I6DrK$
z$H2e<DkGrE8DQne38;Vq)We{12E>Hvhvnxm=r{#bIRmU*dI2q1VC4|ZK9CtOHi(X6
zU;vf1aQDOV{Trx9VfMh>53>hGXEQJ`fXWmYA6Bk#KsCTh5s>>qYGCdM*$u)~(D4?K
z9EcAq$2_3=VdWT<%K+04a~G&r0&*%$KdfAw0M!61M?qXrU4X8?5896fxesJNte&ud
z3c&0E$$>D)E)WgFooM<4pyf{hR6nTQ1XBy5Vf}L$Jq1laY+UmKRKEwBg)n^}vp{$j
zntoWlCIL!R3=9m=VL^~mFh(<uVF6S>Ec`T}^`Zt;KP>#gni&}2;Sb}3!WWdLVESR>
zfUxmFP+JnF7DS`lzYWd)4N&_xK<$T(Z-DfIFf9H+Y!E&G^&>R7!|QR_cp<uabp9Er
z{V@HYhBwIEAJB#ZdU&Jjzl^3I)RPA3=YR%0A^o6nI8gY(><9ItLHZ@2`qAw}=ig^w
z0FQCQ^uy{=*my8(+!vah;PD4@7pVP?Y(H$A=m9^-6`1i2vmZvkMe{$b9rFUJKLBD3
zoP_Cv(cjSYTR_KiET9gAm7g#JVESPEKhQJ@^FL@L03??I)t>-Mm>@wcW2B6XpuQ6W
z1G+sh{aR3cFbWh_AU2E!O*_Es28qM)B&hvdFojSFG%k1y8qJAdl?l*>=?_rl#=yWJ
z$H2e<8X<)FA671b+IQIOk5B{~!%&GPiEckc6?T%Dfsp}LA0w*(jZDJoSyXXm23Y-x
zD$WA07g5Do;q@J=I2*hkLltLdfYnc^;v5XHdIwdUlL1zrpo(+B+s~-t-0*$^syGh=
ztXxMG=VgGE*Qnxr@csp=I6nidd_@%(fX}0#iVHHp%0pCfA$U25DlW_bE5A_1MHpb^
z5~{c;1FXD36&HigBcY0m!^e?P#U&VE`5skVk^z>VQN<DS1*qbn)*!M7ByTb?@G|^?
z)~hgCP`+bf`1l{jhtZ(+A|nH4IDqmoBLikQSb)nP%y4i8kH+&dD6qp?Fc2G}!Rn<L
zHV8q+(_#533o5<?Dh|u16;N{~pqYcq{h;;?$o&k^c|3@%3=9*&<_j{+;6ZiIe6TpC
z`$6+Qpm2Kt?N7tZIS6iVVTK!MjDwLuiUBmf22u<2*JG&r?{Pxxg^&yk!r*d~pCJZX
zU&HEkX{dMtnz%9}Xf}eE0oK2V^@B~o>M_#~Xw)7wGlrg?qrnctO#gXcb1>6wIU{IZ
z3^N@zF=C&$n!yON6%+!fcs&mB<6v=Kh6~VehLz>lVd7}%_8!<hsAhwBZ*hn-GC|x4
z8psBzgQY8WCXhQZ%Nbs<I7lTbHo_quh(kOZY_BAP2edwh*;@)#4;qn0wqX`j99ay+
zz7IABQSQOg$vY;{Toz`${06I+WJo}>7c?OcnrA!!&0jEcn3y5q1DX&3*#{ekl!S_d
zrUyXcF!xJ?#F-=+K!eF3aTwMBi;FOTCQo2uAlj4}6wbU1py?S9AJpCl(XPxO_kaAK
z0d*%x41@zg<}gSx96@tuFjV~ms5nS12uDE0(anhmn<L2(0W}}y{#2-V0GfCn)SaOD
zBape^u~X0-G*}!<y_TGqlM|1aV`WG#Dk&~0O-s{DW{8h>4)G6)_wfu40ZqNeyP)vA
zoROqb^5fHU@{<yC;=w}*@rk7s49WSq1v#lDsVRD9rpAU0Ac^?QlnM~fm;o{(ky4tQ
zTM4tr)z1ZNja!hTuWLMv2eJmnM%V%#uK?SU>FDQ@6<<)Al#^MUk*b$mPyn9L^|6SL
z&&&r8io{2m7{-I<T}v{P<3S>si8+~7i6xo&dGU#PDe)<(#U(|h$zZ-gCTKp`$G9lJ
zoFP8S$2d7ZhaukG$KT1(CqBNoG%4OQJ`*nB@8%Zl8WJDk=;Y%X&kzqb8#160pPH9~
zDgd6qW{7u*^mFv}bY=jJqJW%K1e%D3OjA2JfaimubHJcUZSeForf?>BDjQ84GP#^w
zTpAA^{DGN`6tkeP#WH&io*ZC+>cX5!2hC_>wF>N^c+fmHR2n*w9UqUF^G1~c+Z3Nt
z4w;Zgn>h!~*h7`WLm;?>jA?l^Lp=TCi%U{6^Wsa3Q$bOJ6!4Ytr3DP2=ngIcF9|@^
z2}+hJ#rg3WiFqkGsSNRu)DKIE;Hi92XoJ!dLwtNvaWOQ=vlt+ep9->xp%Uz0h|fz4
M7(l_r08S?i0I2DNi~s-t
literal 0
HcmV?d00001
diff --git a/MobileRobot/Machine_Learning/Practice/iANDj_publishe.cpp b/MobileRobot/Machine_Learning/Practice/iANDj_publishe.cpp
new file mode 100644
index 00000000..eef33e1a
--- /dev/null
+++ b/MobileRobot/Machine_Learning/Practice/iANDj_publishe.cpp
@@ -0,0 +1,67 @@
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <ctime>
+#include <cstdlib>
+
+using namespace std;
+
+
+int i,j;
+double y,theta;
+
+ const int row = 9;
+ const int col = 9;
+
+
+
+double theta_dwn = -.6;
+double theta_up = .6;
+double y_dwn = -.3;
+double y_up = .3;
+
+
+int main()
+{
+ int sample_rate_y = col -1;
+ int sample_rate_theta = row -1;
+
+ double div_theta = (theta_up - theta_dwn)/sample_rate_theta;
+ double div_y = (y_up - y_dwn)/sample_rate_y;
+
+cout << "\n y = ? \n" ;
+ cin >> y;
+
+
+
+cout << "\n theta = ? \n" ;
+ cin >> theta;
+
+
+
+ for(int k = 0; k <= (row - 1); k++)
+ {
+ if(theta >= (theta_dwn - ((.5 + k) * div_theta)) && theta < (theta_dwn + ((.5 + k) * div_theta)))
+ {
+ i = k;
+ break;
+ }
+ }
+
+ for(int l = 0; l <= (col - 1); l++)
+ {
+ if(y >= (y_dwn - ((.5 + l) * div_y)) && y < (y_dwn + ((.5 + l) * div_y)))
+ {
+ j = l;
+ break;
+ }
+ }
+
+cout << "\n i = " << i << " and j = " << j <<"\n";
+
+
+return 0;
+
+
+
+}
diff --git a/MobileRobot/Machine_Learning/Practice/iANDj_publishe.cpp~ b/MobileRobot/Machine_Learning/Practice/iANDj_publishe.cpp~
new file mode 100644
index 00000000..7035b08d
--- /dev/null
+++ b/MobileRobot/Machine_Learning/Practice/iANDj_publishe.cpp~
@@ -0,0 +1,67 @@
+#include <stdio.h>
+#include <iostream>
+#include <iomanip>
+#include <ctime>
+#include <cstdlib>
+
+using namespace std;
+
+
+int i,j;
+double y,theta;
+
+ const int row = 7;
+ const int col = 7;
+
+
+
+double theta_dwn = -.6;
+double theta_up = .6;
+double y_dwn = -.3;
+double y_up = .3;
+
+
+int main()
+{
+ int sample_rate_y = col -1;
+ int sample_rate_theta = row -1;
+
+ double div_theta = (theta_up - theta_dwn)/sample_rate_theta;
+ double div_y = (y_up - y_dwn)/sample_rate_y;
+
+cout << "\n y = ? \n" ;
+ cin >> y;
+
+
+
+cout << "\n theta = ? \n" ;
+ cin >> theta;
+
+
+
+ for(int k = 0; k <= (row - 1); k++)
+ {
+ if(theta >= (theta_dwn - ((.5 + k) * div_theta)) && theta < (theta_dwn + ((.5 + k) * div_theta)))
+ {
+ i = k;
+ break;
+ }
+ }
+
+ for(int l = 0; l <= (col - 1); l++)
+ {
+ if(y >= (y_dwn - ((.5 + l) * div_y)) && y < (y_dwn + ((.5 + l) * div_y)))
+ {
+ j = l;
+ break;
+ }
+ }
+
+cout << "\n i = " << i << " and j = " << j <<"\n";
+
+
+return 0;
+
+
+
+}
--
GitLab