Update lab56func.cpp

Lab5/lab56func.cpp

@@ -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");