Commit fd33c5b8 authored by muskula2's avatar muskula2
Browse files

Update lab56func.cpp

parent 592d601d
......@@ -328,6 +328,13 @@ void ImageConverter::position_callback(const ece470_ur3_driver::positions::Const
void ImageConverter::suction_callback(const ur_msgs::IOStates::ConstPtr& msg)
{
SuctionValue = msg->analog_in_states[0].state;
if (SuctionValue > 2.0) {
get_block = 0;
}
else {
get_block = 1;
}
}
......@@ -801,7 +808,10 @@ void ImageConverter::onClick(int event,int x, int y, int flags, void* userdata)
if (leftclickdone == 1) {
leftclickdone = 0; // code started
if(get_block){
// ros::Rate loop_rate(SPIN_RATE); // Initialize the rate to publish to ur3/command
suction_off(srv_SetIO, srv, loop_rate);
get_block = 0;
// put your right click code here
//Store the parameters transfer btw Camera frame and world frame
......@@ -811,8 +821,8 @@ void ImageConverter::onClick(int event,int x, int y, int flags, void* userdata)
double y_c =(x-O_c)/beta;
double x_w = x_c*cos(theta) + y_c*sin(theta) - Tx*cos(theta) - Ty*sin(theta);
double y_w = -1 * x_c*sin(theta) + y_c*cos(theta) + Tx*sin(theta) -Ty*cos(theta);
x_w += 0.015;
y_w -= 0.035;
x_w += 0.02;
y_w -= 0.055;
ROS_INFO_STREAM("left click: (" << x_w << ", " << y_w << ")"); //the point you clicked
......@@ -848,10 +858,6 @@ void ImageConverter::onClick(int event,int x, int y, int flags, void* userdata)
ROS_INFO_STREAM("Ready for new point");
//Move to desired position
// move_arm(x_w, y_w, z_w);
driver_msg.destination=lab_invk(x_w, y_w, z_w,-90);
......@@ -883,9 +889,6 @@ void ImageConverter::onClick(int event,int x, int y, int flags, void* userdata)
ROS_INFO_STREAM("Ready for new point");
suction_on(srv_SetIO, srv, loop_rate);
//Move to home position
// move_arm(0, 0, 0.2);
......@@ -917,9 +920,6 @@ void ImageConverter::onClick(int event,int x, int y, int flags, void* userdata)
}
ROS_INFO_STREAM("Ready for new point");
}
leftclickdone = 1; // code finished
} else {
......@@ -976,11 +976,6 @@ void ImageConverter::onClick(int event,int x, int y, int flags, void* userdata)
}
ROS_INFO_STREAM("Ready for new point");
//Move to desired position
// move_arm(x_w, y_w, z_w);
driver_msg.destination=lab_invk(x_w, y_w, z_w,-90);
......@@ -1056,9 +1051,6 @@ void ImageConverter::onClick(int event,int x, int y, int flags, void* userdata)
// move_arm(0, 0, 0.2);
}
rightclickdone = 1; // code finished
} else {
ROS_INFO_STREAM("Previous Right Click not finshed, IGNORING this Click");
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment