Add files via upload

Model/point_attempt.py

@@ -14,6 +14,9 @@
 pub3 = rospy.Publisher("/imu_joint3_controller/command", Float64, queue_size=10)
 rate = rospy.Rate(10) # 10hz
 
+req_x = 10
+req_y = 10
+
 erp=0
 epp=0
 eyp=0
@@ -39,17 +42,25 @@
 vel_x = 0
 vel_y = 0
 vel_z = 0
+index = 0
+sum_d = 0
+pre_d = 0
+req_x1 = 0
+req_y1 = 0
+
 
 def get_pos(info):
+
 	global pos_x
 	global pos_y
 	global pos_z
-
+	global req_x1
+	global req_y1
 	global vel_x
 	global vel_y
 	global vel_z
-	pos_x= info.pose[1].position.x
-	pos_y= info.pose[1].position.y
+	pos_x= info.pose[1].position.x - req_x1
+	pos_y= info.pose[1].position.y - req_y1
 	pos_z= info.pose[1].position.z
 
 	vel_x= info.twist[1].linear.x
@@ -58,8 +69,10 @@ def get_pos(info):
 
 
 def callback(data):
+	near = []
 	global erp
 	global epp
+	global near
 	global eyp
 	global pos_x
 	global pos_y
@@ -72,17 +85,23 @@ def callback(data):
 	global set_r
 	global set_p
 	global set_y
+	global req_x1
+	global req_y1
 	global t0
+	global index
 	global sumr
 	global sump
 	global sumy
 	global pos1
 	global pos2
 	global pos3
+	global sum_d
+	global pre_d
+
 	#rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.orientation)
 
 
-	print("POSNS",pos_x,pos_y,pos_z)
+	#print("POSNS",pos_x,pos_y,pos_z)
 	#a_x = data.linear_acceleration.x
 	#a_y = data.linear_acceleration.y
 	#a_z = data.linear_acceleration.z
@@ -118,14 +137,36 @@ def callback(data):
 	w3= (0.333)*(wz+ ((1/1.414)*((sin(yaw)*((1.732*wx)+wy))+(cos(yaw)*(-wx+(1.732*wy))))))
 
 
+	des_ang = atan2(req_y,req_x)
+	num_points = int(sqrt(req_x**2 + req_y**2))
 
+	dist=0
+	for i in range(1,num_points+1):
+		x = cos(des_ang)*i
+		y = sin(des_ang)*i
+		near.append([x,y])
+
+	print(near,"NUM")
+	req_x1 = near[index][0]
+	req_y1 = near[index][1]
+	print("REQ",req_x1,req_y1)
+	if sqrt(pos_x**2 + pos_y**2)<0.3 and index!=len(near)-1:
+		index+=1
 
 	#print("POSN",pos_x,pos_y)
 	d = sqrt(pos_x**2 + pos_y**2)
 	ang = atan2(pos_y,pos_x)
-	set_r = abs(cos(ang))*(1.0*d/180)*3.1417
-	set_p = abs(sin(ang))*(1.0*d/180)*3.1417
-	print("POSN",pos_x,pos_y)
+
+	kp1 = 1
+	kp2 = 1.25
+	kp3 = 0.00001
+
+	xx = kp1*d + kp2*(d-pre_d) + kp3*sum_d
+	print("XXXX",xx)
+
+	set_r = abs(cos(ang))*(1.0*xx/180)*3.1417
+	set_p = abs(sin(ang))*(1.0*xx/180)*3.1417
+	#print("POSN",pos_x,pos_y)
 
 	if abs(roll-set_r)<0.034:
 		sum_r = 0
@@ -145,8 +186,8 @@ def callback(data):
 		set_r = 0.034
 	if vel_y<-0.2:
 		set_r = -0.034
-	print("VELS",vel_x,vel_y)
-	print("SETS",set_r,set_p)
+	#print("VELS",vel_x,vel_y)
+	#print("SETS",set_r,set_p)
 
 
 	#print(w_x,w_y,w_z,"Ball")
@@ -158,6 +199,22 @@ def callback(data):
 	sumr+=eroll
 	sump+=epitch
 	sumy+=eyaw
+	sum_d+=d
+	pre_d = d
+
+	if d<0.2:
+		sum_d = 0
+
+	if set_r>0.034:
+		set_r = 0.034
+	if set_r<-0.034:
+		set_r = -0.034
+	if set_p>0.034:
+		set_p = 0.034
+	if set_p<-0.034:
+		set_p = -0.034
+
+	print("SET",set_r,set_p)
 
 	pub1.publish(w2)
 	pub2.publish(w3)