diff --git a/Digraph/GoToBall b/Digraph/GoToBall
deleted file mode 100644
index f4add715..00000000
--- a/Digraph/GoToBall
+++ /dev/null
@@ -1,20 +0,0 @@
-digraph GoToBall {
-	failed [label="behavior::failed" shape=ellipse]
-	start [label="behavior::start" shape=diamond]
-	cancelled [label="behavior::cancelled" shape=ellipse]
-	completed [label="behavior::completed" shape=ellipse]
-	subgraph cluster_0 {
-		graph [label="behavior::running" style=dotted]
-		setup [label="role.GoToBall::setup" shape=ellipse]
-		course_approach [label="role.GoToBall::course_approach" shape=ellipse]
-		fine_approach [label="role.GoToBall::fine_approach" shape=ellipse]
-	}
-	start -> setup [label=immediately decorate=True]
-	setup -> failed [label=failed decorate=True]
-	setup -> course_approach [label=setup decorate=True]
-	setup -> fine_approach [label=ball_in_vicinity decorate=True]
-	course_approach -> failed [label=failed decorate=True]
-	course_approach -> fine_approach [label=complete decorate=True]
-	fine_approach -> failed [label=failed decorate=True]
-	fine_approach -> completed [label=complete decorate=True]
-}
diff --git a/Digraph/GoToBall.png b/Digraph/GoToBall.png
index cad6459b..ecaef90c 100644
Binary files a/Digraph/GoToBall.png and b/Digraph/GoToBall.png differ
diff --git a/Digraph/Goalie b/Digraph/Goalie
index 83c39d53..ead07023 100644
--- a/Digraph/Goalie
+++ b/Digraph/Goalie
@@ -8,10 +8,23 @@ digraph Goalie {
 		protect [label="tactics.Goalie::protect" shape=ellipse]
 		peace [label="tactics.Goalie::peace" shape=ellipse]
 		clear [label="tactics.Goalie::clear" shape=ellipse]
+		block [label="tactics.Goalie::block" shape=ellipse]
 	}
+	start -> clear [label="direct clear" decorate=True]
 	start -> peace [label=immediately decorate=True]
-	protect -> clear [label="save now" decorate=True]
-	protect -> peace [label=peace decorate=True]
+	start -> protect [label="direct protect" decorate=True]
+	start -> block [label="direct block" decorate=True]
+	protect -> failed [label=failed decorate=True]
+	protect -> clear [label="clear now" decorate=True]
+	protect -> peace [label=relax decorate=True]
+	protect -> block [label="block now" decorate=True]
+	peace -> failed [label=failed decorate=True]
+	peace -> clear [label="clear now" decorate=True]
 	peace -> protect [label="ball is valid" decorate=True]
-	clear -> peace [label=peace decorate=True]
+	peace -> block [label="opponent shot" decorate=True]
+	clear -> failed [label=failed decorate=True]
+	block -> failed [label=failed decorate=True]
+	block -> clear [label="clear now" decorate=True]
+	block -> peace [label=relax decorate=True]
+	block -> protect [label="remain vigilant" decorate=True]
 }
diff --git a/belief_state/src/node_class.cpp b/belief_state/src/node_class.cpp
index 62d64fc8..d9b106af 100755
--- a/belief_state/src/node_class.cpp
+++ b/belief_state/src/node_class.cpp
@@ -18,11 +18,11 @@
 #include "kalman_filter.h"
 
 //const int BALL_AT_CORNER_THRESH	= 20; 
-const int HALF_FIELD_MAXX		= 3000; 
-const int HALF_FIELD_MAXY		= 2000;
+const int HALF_FIELD_MAXX		= 6000; 
+const int HALF_FIELD_MAXY		= 4500;
 const float MAX_DRIBBLE_R		= 3;
-const int DBOX_WIDTH			= 600;
-const int DBOX_HEIGHT			= 600;
+const int DBOX_WIDTH			= 2400;
+const int DBOX_HEIGHT			= 1200;
 const int MAX_QUEUE_SZ			= 5;
 short int isteamyellow = 0;
 
diff --git a/coordinated-pass.py b/coordinated-pass.py
new file mode 100644
index 00000000..466471fe
--- /dev/null
+++ b/coordinated-pass.py
@@ -0,0 +1,114 @@
+import rospy,sys
+from utils.geometry import Vector2D
+from utils.functions import *
+from krssg_ssl_msgs.msg import point_2d
+from krssg_ssl_msgs.msg import BeliefState
+from krssg_ssl_msgs.msg import gr_Commands
+from krssg_ssl_msgs.msg import gr_Robot_Command
+from krssg_ssl_msgs.msg import BeliefState
+from role import  GoToBall, GoToPoint, KickToPoint, pass_and_receive, receiver, passer
+from multiprocessing import Process
+from kubs import kubs
+from krssg_ssl_msgs.srv import bsServer
+from math import atan2,pi
+from utils.functions import *
+from tactics import Goalie
+import multiprocessing
+import threading
+#pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000)
+
+
+import memcache
+shared = memcache.Client(['127.0.0.1:11211'],debug=True)
+
+
+def function(id_1,id_2,state,pub):
+	kub1 = kubs.kubs(id_1,state,pub)
+	# print(id_1)
+	kub1.update_state(state)
+	kub2 = kubs.kubs(id_2,state,pub)
+	# print(id_2)
+	kub2.update_state(state)
+	#print(kub1.kubs_id)
+	p_fsm = passer.PassReceive()
+	r_fsm = receiver.PassReceive()
+	# g_fsm = GoToPoint.GoToPoint()
+	if dist(kub1.state.ballPos,kub1.get_pos())< dist(kub2.state.ballPos,kub2.get_pos()):
+		p_fsm.add_kub(kub1)
+		p_fsm.add_kub(kub2)
+		p_fsm.spin()
+	else:
+		r_fsm.add_kub(kub1)
+		r_fsm.add_kub(kub2)
+		r_fsm.spin()
+	# g_fsm.add_point(point=kub.state.ballPos,orient=normalize_angle(pi+atan2(state.ballPos.y,state.ballPos.x-3000)))
+	#g_fsm.add_theta(theta=normalize_angle(pi+atan2(state.ballPos.y,state.ballPos.x+3000)))
+	# g_fsm.as_graphviz()
+	# g_fsm.write_diagram_png()
+	#print('something before spin')
+
+def main1(process_id):
+	pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000)
+	rospy.init_node('node' + str(process_id),anonymous=False)
+
+	while True:
+	    state = None
+        # state=shared.get('state')
+	    rospy.wait_for_service('bsServer',)
+	    getState = rospy.ServiceProxy('bsServer',bsServer)
+	    try:
+	            state = getState(state)
+	    except rospy.ServiceException, e:
+	            print("chutiya")		
+	    if state:
+	            #print('lasknfcjscnajnstate',state.stateB.homePos)
+	            #p2 = multiprocessing.Process(target=function2, args=(2,state.stateB, )) 
+	            # print("process 1")
+	            function(1,2,state.stateB,pub)
+	            #p2.start()
+	            #p1.join()
+	            #p2.join()
+	           # print('chal ja')
+	            # break
+	    #rospy.spin()	
+
+def main2(process_id):
+	pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000)
+	rospy.init_node('node' + str(process_id),anonymous=False)
+
+	while True:
+	    state = None
+        # state=shared.get('state')
+	    rospy.wait_for_service('bsServer',)
+	    getState = rospy.ServiceProxy('bsServer',bsServer)
+	    try:
+	            state = getState(state)
+	    except rospy.ServiceException, e:
+	            print("chutiya")		
+	    if state:
+	            #print('lasknfcjscnajnstate',state.stateB.homePos)
+	            #p2 = multiprocessing.Process(target=function2, args=(2,state.stateB, )) 
+	            # print("process 2")
+	            function(2,1,state.stateB,pub)
+	            #p2.start()
+	            #p1.join()
+	            #p2.join()
+	           # print('chal ja')
+	            # break
+	    #rospy.spin()	
+
+
+#print str(kub.kubs_id) + str('***********')
+p1 = multiprocessing.Process(target=main1, args=(1,))
+p2 = multiprocessing.Process(target=main2, args=(2,))
+p1.start()
+p2.start()
+p1.join()
+p2.join()
+#main1()
+
+# rospy.Subscriber('/belief_state', BeliefState, BS_callback, queue_size=1000)
+
+
+
+
diff --git a/ompl_planner/include/MotionPlanner.h b/ompl_planner/include/MotionPlanner.h
index 1683518a..e15d976a 100644
--- a/ompl_planner/include/MotionPlanner.h
+++ b/ompl_planner/include/MotionPlanner.h
@@ -40,8 +40,8 @@ using namespace std;
 
 #define BS_TO_OMPL 0.1
 #define OMPL_TO_BS 10
-#define HALF_FIELD_MAXX 4500
-#define HALF_FIELD_MAXY 3000
+#define HALF_FIELD_MAXX 6000
+#define HALF_FIELD_MAXY 4500
 #define HALF_FIELD_MAXX_OMPL HALF_FIELD_MAXX*BS_TO_OMPL
 #define HALF_FIELD_MAXY_OMPL HALF_FIELD_MAXY*BS_TO_OMPL
 #define self_radius 270*BS_TO_OMPL
diff --git a/role/KickToPoint.py b/role/KickToPoint.py
index a98cb1f1..ac60a5b9 100644
--- a/role/KickToPoint.py
+++ b/role/KickToPoint.py
@@ -16,8 +16,7 @@ class KickToPoint(behavior.Behavior):
 	class State(Enum):
 		normal = 1
 		setStance = 2
-		turnAround = 3
-		GoAndKick = 4
+		GoAndKick = 3
 
 	def __init__(self,target,continuous=False):
 
@@ -25,12 +24,14 @@ def __init__(self,target,continuous=False):
 
 		self.name = "KickToPoint"
 
-		self.power = 7.0
-
 		self.target_point = target 
 
 		self.go_at = None 
 
+		self.power = 0
+
+		self.goto_kick=False;
+
 		self.setStanceThresh = 1.5*BOT_RADIUS
 
 		self.ball_dist_thresh = BOT_BALL_THRESH
@@ -42,9 +43,6 @@ def __init__(self,target,continuous=False):
 
 		self.add_state(KickToPoint.State.setStance,
 			behavior.Behavior.State.running)
-
-		self.add_state(KickToPoint.State.turnAround,
-			behavior.Behavior.State.running)
 		
 		self.add_state(KickToPoint.State.GoAndKick,
 			behavior.Behavior.State.running)
@@ -56,24 +54,15 @@ def __init__(self,target,continuous=False):
 			KickToPoint.State.setStance,lambda: self.setstance(),'far_away')
 
 		self.add_transition(KickToPoint.State.normal,
-			KickToPoint.State.turnAround,lambda: self.turnAroundDirect(),'near_enough')
-
-		self.add_transition(KickToPoint.State.normal,
-			KickToPoint.State.GoAndKick,lambda:self.GoAndKickDirect(),'very_close')
-
-		self.add_transition(KickToPoint.State.setStance,
-			KickToPoint.State.turnAround,lambda:self.turnaround(),'turn')
-
-		self.add_transition(KickToPoint.State.turnAround,
-			KickToPoint.State.GoAndKick,lambda: self.goandkick(),'kick')
+			KickToPoint.State.GoAndKick,lambda: self.GoAndKickDirect(),'very_close')
 
 		self.add_transition(KickToPoint.State.GoAndKick,
 			behavior.Behavior.State.completed,lambda: self.at_ball_pos(),'kicked!!!')
 
 		self.add_transition(KickToPoint.State.setStance,
-			behavior.Behavior.State.failed,lambda: self.behavior_failed,'failed')
+			KickToPoint.State.GoAndKick,lambda: self.goto_kick,'kicking')
 
-		self.add_transition(KickToPoint.State.turnAround,
+		self.add_transition(KickToPoint.State.setStance,
 			behavior.Behavior.State.failed,lambda: self.behavior_failed,'failed')
 
 		self.add_transition(KickToPoint.State.GoAndKick,
@@ -94,18 +83,14 @@ def add_target_theta(self):
 	def setstance(self):
 		global DIST_THRESH
 		#print("hello")
-		theta = angle_diff(self.target_point, self.kub.state.ballPos)
+		theta = angle_diff(self.kub.state.ballPos,self.target_point)
 		go_at = getPointToGo(self.kub.state.ballPos, theta) 
-		return go_at.dist(self.get_pos_as_vec2d(self.kub.get_pos())) >= DIST_THRESH*0.85
+		print "##########################"
+		return go_at.dist(self.get_pos_as_vec2d(self.kub.get_pos())) >= DIST_THRESH*1.00
 
-	def turnaround(self):
-		return not self.bot_moving() #and not self.facing_the_target() and self.near_targetBall_line() and self.one_more()
-
-	def turnAroundDirect(self):
-		return not self.setstance() and not self.bot_moving()
 
 	def GoAndKickDirect(self):
-		return self.facing_the_target() and not self.setstance()
+		return not self.setstance()
 
 	def goandkick(self):
 		print("facing_the_target : ", self.facing_the_target())
@@ -126,7 +111,11 @@ def bot_moving(self):
 			return True
 		return False
 
+	def kicking_power(self):
+		return math.sqrt(dist(self.target_point,self.kub.state.ballPos)/6400.0)*5.0
+	
 	def ball_nearby(self, thresh = BOT_RADIUS*1.5):
+
 		if bot_in_front_of_ball(self.kub, thresh):
 			return True
 		else :
@@ -135,7 +124,7 @@ def ball_nearby(self, thresh = BOT_RADIUS*1.5):
 	def facing_the_target(self):
 		print(" theta1 : ", self.theta)
 		print(" bot theta : ", self.kub.get_theta())
-		if vicinity_theta( self.theta, normalize_angle(self.kub.get_theta()), thresh=0.30 ):
+		if vicinity_theta( self.theta, self.kub.get_theta(), thresh=0.30 ):
 			return True
 		else :
 			return False
@@ -144,118 +133,61 @@ def at_ball_pos(self):
 		error = 10
 		return vicinity_points(self.kub.get_pos(),self.kub.state.ballPos,thresh=BOT_BALL_THRESH+error)
 
-
-	# def on_enter_setStance(self):
-	# 	global start_time
-	# 	start_time = rospy.Time.now()
-	# 	start_time = start_time.secs + 1.0*start_time.nsecs/pow(10,9)
-
-	def on_enter_setStance(self):
-		print("entered setstance")
-		theta = angle_diff(self.target_point, self.kub.state.ballPos)
-		self.go_at = getPointToGo(self.kub.state.ballPos, theta)
-		_GoToPoint_.init(self.kub, self.go_at, self.theta)
-		pass
-
 	def reset(self):
 		global start_time
 		start_time = rospy.Time.now()
 		start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)
 
+	def on_enter_normal(self):
+		pass
+
 	def execute_normal(self):
 		pass	
 
-	# def execute_setStance(self):
-	# 	global start_time
-	# 	t = rospy.Time.now()
-	# 	t = t.secs + 1.0*t.nsecs/pow(10,9)
-	# 	#print(" t - start = ",t-start_time)
-	# 	theta = angle_diff(self.target_point, self.kub.state.ballPos)
-	# 	go_at = getPointToGo(self.kub.state.ballPos, theta)
-	# 	#print("goo", go_at)
-	# 	[vx, vy, vw, REPLANNED] = Get_Vel(start_time, t, self.kub.kubs_id, Vector2D(go_at.x, go_at.y), self.kub.state.homePos, self.kub.state.awayPos, True)
-	# 		#vx, vy, vw, replanned
-	# 	#print("-------------------REPLANNED = ",REPLANNED)
-	# 	if(REPLANNED):
-	# 		self.reset()
-		
-	# 	# print("kubs_id = ",kub.kubs_id)
-	# 	curPos = self.kub.get_pos()
-	# 	#if vicinity_points(go_at, curPos, 4) == False:
-	# 	try:   
-	# 		#print("vx = ",vx)
-	# 		#print("vy = ",vy)
-	# 		self.kub.move(vx, vy)
-	# 		#print(vw)
-	# 		#print("homePos")
-	# 		self.kub.turn(vw)
-	# 		self.kub.execute()
-	# 	except Exception as e:
-	# 		print("In except",e)
-	# 		pass
-	# 	print("exiting")
+	def on_exit_normal(self):
+		pass
+	def getPointToGo1(self,point, theta):
+		x = point.x - (1.5 * BOT_RADIUS) * (math.cos(theta))
+		y = point.y - (1.5 * BOT_RADIUS) * (math.sin(theta))
+		return Vector2D(int(x), int(y))
+
+	def on_enter_setStance(self):
+		print("entered setstance hello")
+		self.theta = normalize_angle(angle_diff( self.kub.state.ballPos,self.target_point))
+		self.power = self.kicking_power()
+		self.go_at = self.getPointToGo1(self.kub.state.ballPos, self.theta)
+		_GoToPoint_.init(self.kub, self.go_at, self.theta)
+		pass
+
 
 	def execute_setStance(self):
 		global DIST_THRESH
-		print("executing setstance")
+		print("executing setstance fnfnfj")
 		start_time = rospy.Time.now()
-		start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)   
+		start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)  
+		print "-----------------------##########-"
 		generatingfunction = _GoToPoint_.execute(start_time,DIST_THRESH,True)
+		
 		for gf in generatingfunction:
 			self.kub,target_point = gf
 			# self.target_point = getPointBehindTheBall(self.kub.state.ballPos,self.theta)
 			theta = angle_diff(self.target_point, self.kub.state.ballPos)
-			self.go_at = getPointToGo(self.kub.state.ballPos, theta)
-			if not vicinity_points(self.go_at,target_point,thresh=BOT_BALL_THRESH):
-				self.behavior_failed = True
-				break
-
-
-	def on_enter_turnAround(self):
-		print("entered turnAround")
-		print(self.get_pos_as_vec2d(self.kub.get_pos()).dist(self.get_pos_as_vec2d(self.kub.state.ballPos)))
-		self.theta = normalize_angle(angle_diff(self.kub.state.ballPos,self.target_point))
-		_turnAround_.init(self.kub, self.theta )
-		pass
-
-	# def execute_turnAround(self):
-	# 	print("TURNING")
-	# 	#print(data.homePos[BOT_ID].theta)
-	# 	#print(theta)
-	# 	theta = angle_diff(self.target_point, self.kub.state.ballpos)
-	# 	totalAngle = theta
-	# 	MAX_w = (MAX_BOT_OMEGA+MIN_BOT_OMEGA)/1.2
-	# 	# theta_left = float(homePos[kub_id].theta-totalAngle)
-	# 	theta_lft = normalize_angle(normalize_angle(self.kub.state.homePos[self.kub.kubs_id].theta)-totalAngle)*-1.0+3.1412
-	# 	vw = (theta_lft/2*math.pi)*MAX_w
-	# 	# print "totalAngle",radian_2_deg(totalAngle)
-	# 	# print "theta_left ",radian_2_deg(theta_lft),theta_lft
-	# 	# print "homePos theta ",radian_2_deg(normalize_angle(homePos[kub_id].theta))
-	# 	# print "omega ",vw
-	# 	if abs(vw)<1*MIN_BOT_OMEGA:
-	# 		vw = 1*MIN_BOT_OMEGA*(1 if vw>0 else -1)
-
-	# 	if abs(theta_lft)<ROTATION_FACTOR/2:
-	# 		vw = 0.0
-
-	# 	print "Omega return",vw
-	# 	self.kub.reset()
-	# 	self.kub.turn(vw)
-	# 	self.kub.execute()
-
-	def execute_turnAround(self):
-		print("executing turnaround")
-		start_time = rospy.Time.now()
-		start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)   
-		generatingfunction = _turnAround_.execute(start_time)
-		for gf in generatingfunction:
-			self.kub,final_theta = gf
-			# self.target_point = getPointBehindTheBall(self.kub.state.ballPos,self.theta)
-			if not vicinity_theta(self.theta,final_theta,thresh=0.1):
-				self.behavior_failed = True
-				print("failed")
+			print(self.theta)
+			self.kub.kick(self.kicking_power())
+			#print theta,"------------------------"
+			self.go_at = self.getPointToGo1(self.kub.state.ballPos, theta)
+			# #print vicinity_points(self.go_at,target_point,thresh=BOT_BALL_THRESH)
+			# if(dist(self.go_at,self.kub.get_pos())<BOT_BALL_THRESH):
+			# 	self.behavior_failed = True
+			# # 	break
+			if not vicinity_points(self.go_at, target_point,thresh=BOT_RADIUS*2.0):
+				print "Finally"
+				# self.behavior_failed = True 
 				break
-
+			#print self.go_at.x,"1",self.go_at.y,"abc",self.kub.get_pos().x,"2",self.kub.get_pos().y
+		print("main nikal gya")
+		self.goto_kick = True;
+			
 
 	def on_enter_GoAndKick(self):
 		print("entered GoAndKick")
@@ -263,17 +195,6 @@ def on_enter_GoAndKick(self):
 		_GoToPoint_.init(self.kub, self.kub.state.ballPos, theta)
 		pass
 
-	# def on_enter_GoAndKick(self):
-	# 	print("entered GoAndKick")
-	# 	global alpha, buffer_dist, start_time
-	# 	start_time = rospy.Time.now()
-	# 	start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9) 
-	# 	alpha = atan2(self.kub.state.ballPos.y - self.kub.get_pos().y,self.kub.state.ballPos.x - self.kub.get_pos().x ) - self.kub.state.homePos[self.kub.kubs_id].theta
-	# 	buffer_dist = self.get_pos_as_vec2d(self.kub.get_pos()).dist(self.get_pos_as_vec2d(self.kub.state.ballPos))*abs(sin(alpha))
-	# 	theta = self.kub.get_pos().theta
-	# 	_GoToPoint_.init(self.kub, self.kub.state.ballPos, theta)
-	# 	pass
-
 	def execute_GoAndKick(self):
 		print("ha ma pahuch gaya")
 		start_time = rospy.Time.now()
@@ -288,33 +209,19 @@ def execute_GoAndKick(self):
 				self.behavior_failed = True
 				break
 
-	# def execute_GoAndKick(self):
-	# 	print("ha ma pahuch gaya")
-	# 	global alpha, buffer_dist, start_time
-	# 	time = rospy.Time.now()
-	# 	time = 1.0*time.secs + 1.0*time.nsecs/pow(10,9)
-	# 	self.kub.kick(self.power)
-	# 	self.kub.execute()	
-
-
-		
-
 	def on_exit_GoAndKick(self):
 		print("kicking bolo all is well")
-		#self.kub.kick(self.power)
 		self.kub.reset()
 		self.kub.kick(self.power)
 		self.kub.execute()
+		self.goto_kick=False
 		pass
 
-	def on_enter_normal(self):
-		pass
-
-	def on_exit_normal(self):
-		pass
+	
 
 	def on_exit_setStance(self):
-		print("exiting setstance")
+		print("main firse nikl gya")
+		self.goto_kick = True;
 		pass
 
 	def on_exit_turnAround(self):
diff --git a/role/_GoToPoint_.py b/role/_GoToPoint_.py
index a8fe7f87..9dc1fc4d 100644
--- a/role/_GoToPoint_.py
+++ b/role/_GoToPoint_.py
@@ -74,7 +74,7 @@ def execute(startTime,DIST_THRESH,avoid_ball=False):
             kub.state = getState(prev_state).stateB
         except rospy.ServiceException, e:
             print("Error ", e)
-        print(kub.state)
+        # print(kub.state)
         if not(prev_state == kub.state):
             prev_state = kub.state
 
diff --git a/role/_turnAround_.py b/role/_turnAround_.py
index e71b185c..aea52c23 100644
--- a/role/_turnAround_.py
+++ b/role/_turnAround_.py
@@ -52,7 +52,7 @@ def reset():
     start_time = rospy.Time.now()
     start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)
     
-def execute(startTime):
+def execute(startTime,thresh=2*ROTATION_FACTOR):
     global start_time,FIRST_CALL,FLAG_turn,kub,prev_state
 
     # print DIST_THRESH
@@ -87,8 +87,8 @@ def execute(startTime):
 
             #print("rotate : ", rotate)
             #print(" not rotate : ", kub.state.homePos[kub.kubs_id].theta)
-            print("diff : ",abs(normalize_angle(kub.state.homePos[kub.kubs_id].theta-rotate)))
-            if (abs(normalize_angle(kub.state.homePos[kub.kubs_id].theta-rotate))<2*ROTATION_FACTOR):
+            # print("diff : ",abs(normalize_angle(kub.state.homePos[kub.kubs_id].theta-rotate)))
+            if (abs(normalize_angle(kub.state.homePos[kub.kubs_id].theta-rotate))<thresh):
                 kub.turn(0)
                 print("Angle completed")
                 FLAG_turn = True
diff --git a/role/pass_and_receive.py b/role/pass_and_receive.py
new file mode 100644
index 00000000..46c86969
--- /dev/null
+++ b/role/pass_and_receive.py
@@ -0,0 +1,371 @@
+import sys,rospy
+import composite_behavior
+import behavior
+from utils.config import *
+import time
+from enum import Enum
+import logging
+from role import pass_receive
+from role import GoToBall, GoToPoint, KickToPoint, _GoToPoint_,_turnAround_
+from utils.geometry import *
+from utils.functions import *
+from krssg_ssl_msgs.srv import bsServer
+import math
+
+rospy.wait_for_service('bsServer',)
+getState = rospy.ServiceProxy('bsServer',bsServer)
+
+class PassReceive(behavior.Behavior):
+
+	class State(Enum):
+
+		setup = 1
+		passer = 2
+		waiting = 3
+		kicking = 4
+		receiver = 5
+		going_to_receiving_point = 6
+		receiving = 7
+		received = 8
+
+	def __init__(self):
+		super(PassReceive,self).__init__()
+
+		self.kubs = []
+		self.passer = None
+		self.receiver = None
+		self.receiving_point = None
+		self.pass_angle = None
+		self.behavior_failed = False
+		self.ball_kicked = False
+
+		for state in PassReceive.State:
+			self.add_state(state,behavior.Behavior.State.running)
+
+		self.add_transition(behavior.Behavior.State.start,PassReceive.State.setup,
+			lambda:True,"immediately")
+		self.add_transition(PassReceive.State.setup, PassReceive.State.kicking,
+			lambda:self.passer_present() and self.kub_0_is_passer() and self.ready_to_kick(),"Direct kick")
+		self.add_transition(PassReceive.State.setup,PassReceive.State.passer,
+			lambda:self.passer_present() and self.kub_0_is_passer() and not self.ready_to_kick(),"passer found")
+		self.add_transition(PassReceive.State.passer,PassReceive.State.waiting,
+			lambda:self.receiver_present(),"passer ready")
+		self.add_transition(PassReceive.State.waiting,PassReceive.State.kicking,
+			lambda:self.receiver_is_ready(),"kicking")
+		self.add_transition(PassReceive.State.kicking,PassReceive.State.receiving,
+			lambda:self.ball_kicked,"ball kicked")
+		self.add_transition(PassReceive.State.setup,PassReceive.State.receiver,
+			lambda:self.receiver_present() and self.kub_0_is_receiver(),"receiver found")
+		self.add_transition(PassReceive.State.receiver,PassReceive.State.going_to_receiving_point,
+			lambda:self.ball_moving_to_receive_point(),"moving to receiving point")
+		self.add_transition(PassReceive.State.going_to_receiving_point,PassReceive.State.receiving,
+			lambda:self.reciever_at_receiving_point(),"ball incoming")
+		self.add_transition(PassReceive.State.receiving,PassReceive.State.received,
+			lambda:self.ball_with_receiver() or not self.ball_moving(),"ball received")
+		self.add_transition(PassReceive.State.received,behavior.Behavior.State.completed,
+			lambda:True,"one pass completed")
+
+		for state in PassReceive.State:
+			self.add_transition(state,behavior.Behavior.State.failed,
+				lambda:self.behavior_failed,"failed")
+
+	def passer_present(self):
+		return self.passer != None
+
+	def receiver_present(self):
+		return self.receiver != None
+
+	def kub_0_is_passer(self):
+		return self.passer.kubs_id == self.kubs[0].kubs_id
+
+	def kub_0_is_receiver(self):
+		return self.receiver.kubs_id == self.kubs[0].kubs_id		
+
+	def receiver_is_ready(self):
+		return math.fabs(math.fabs(normalize_angle(self.receiver.get_theta()-self.pass_angle))-math.pi) < deg_2_radian(10.0)
+
+	def ready_to_kick(self):
+		return vicinity_points(getPointBehindTheBall(Vector2D(self.passer.state.ballPos),self.pass_angle), Vector2D(self.passer.get_pos()), thresh = 2*BOT_RADIUS)
+
+	def ball_moving_to_receive_point(self):
+		if self.ball_moving():
+			ball_vel = Vector2D()
+			ball_vel.x = self.receiver.state.ballVel.x
+			ball_vel.y = self.receiver.state.ballVel.y
+			return math.fabs(ball_vel.angle()-self.pass_angle) < math.pi/2.0
+		return False
+
+	def ball_with_receiver(self):
+		return self.receiver.has_ball()
+
+	def reciever_at_receiving_point(self):
+		if vicinity_points(self.receiver.get_pos(),self.receiving_point,0.5*BOT_RADIUS):
+			return True
+		return False
+
+	def ball_moving(self):
+		if magnitute(self.receiver.state.ballVel) <= 50 or vicinity_points(self.receiver.state.homePos[self.passer.kubs_id],self.receiver.state.ballPos,200):
+			return False
+		return True
+
+	def add_kub(self,kub):
+		self.kubs += [kub]
+
+	def kicking_power(self):
+		return math.sqrt(dist(self.receiving_point,self.passer.state.ballPos)/6400.0)*5.0
+
+	def calculate_receiving_point(self):
+		ball_pos = Vector2D(self.receiver.state.ballPos.x,self.receiver.state.ballPos.y)
+		ball_vel = Vector2D(self.receiver.state.ballVel.x,self.receiver.state.ballVel.y)
+		self.pass_line = Line(point1=ball_pos,angle=ball_vel.angle())
+		receiver_pos = Vector2D(self.receiver.get_pos().x,self.receiver.get_pos().y)
+		line2 = Line(point1=receiver_pos,angle = normalize_angle(ball_vel.angle()+math.pi/2.0))
+		# print(magnitute(ball_vel))
+		return self.pass_line.intersection_with_line(line2)
+
+	def on_enter_setup(self):
+		#print("Entering setup")
+		pass
+
+	def execute_setup(self):
+		if len(self.kubs) != 2:
+			self.behavior_failed = True
+			return
+		if dist(self.kubs[0].state.ballPos,self.kubs[0].get_pos())< dist(self.kubs[0].state.ballPos,self.kubs[1].get_pos()):
+			self.passer = self.kubs[0]
+			self.receiver = self.kubs[1]
+			self.receiving_point = Vector2D(self.kubs[1].get_pos())
+		else:
+			self.passer = self.kubs[1]
+			self.receiver = self.kubs[0]
+			self.receiving_point = Vector2D(self.kubs[0].get_pos())
+		self.pass_angle = angle_diff(self.receiver.state.ballPos,self.receiving_point)
+		# self.pass_angle = normalize_angle(self.pass_angle)
+		state = None
+		state = getState(state)
+		state = state.stateB
+		self.passer.update_state(state)
+		self.receiver.update_state(state)
+		print("receiver id is",self.receiver.kubs_id)
+
+	def on_exit_setup(self):
+		#print("exiting setup")
+		pass
+
+	def on_enter_passer(self):
+		print("Entering passer")
+		# print("#############################   GOING TO POINT ###################################")
+		target_point = getPointBehindTheBall(Vector2D(self.passer.state.ballPos),normalize_angle(self.pass_angle+math.pi))
+		_GoToPoint_.init(self.passer,target_point,self.passer.get_theta())
+		pass
+
+	def execute_passer(self):
+		if vicinity_points(Vector2D(self.passer.get_pos()),Vector2D(self.passer.state.ballPos),4.5*BOT_RADIUS):
+			pass
+		# target_point = getPointBehindTheBall(self.passer.state.ballPos,normalize_angle(self.pass_angle+math.pi))
+		# g_fsm = GoToPoint.GoToPoint()
+		# g_fsm.add_kub(self.passer)
+		# g_fsm.add_point(target_point,self.pass_angle)
+		# g_fsm.avoid_ball = True
+		# g_fsm.spin()
+		# self.behavior_failed = g_fsm.behavior_failed
+		print(self.passer.kubs_id)
+		if self.ready_to_kick():
+			pass
+		start_time = rospy.Time.now()
+		start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)
+		generatingfunction = _GoToPoint_.execute(start_time,BOT_RADIUS,avoid_ball=True)
+		for gf in generatingfunction:
+			self.passer,target_point = gf
+			if not vicinity_points(target_point,getPointBehindTheBall(Vector2D(self.passer.state.ballPos),normalize_angle(self.pass_angle+math.pi)),BOT_RADIUS):
+				self.behavior_failed = True
+				break
+
+	def on_exit_passer(self):
+		print("exiting passer")
+		pass
+
+	def on_enter_waiting(self):
+		print("Enter waiting")
+		self.pass_angle = angle_diff(self.receiver.state.ballPos,self.receiver.get_pos())
+		_turnAround_.init(self.passer,self.pass_angle)
+		pass
+
+	def execute_waiting(self):
+		# print("/////////////////////// waiting //////////////////")
+		if not vicinity_theta(self.passer.get_theta(),self.pass_angle,thresh=0.05):
+			start_time = rospy.Time.now()
+			start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)   
+			generatingfunction = _turnAround_.execute(start_time,0.1)
+			for gf in generatingfunction:
+				self.passer,final_theta = gf
+				# self.target_point = getPointBehindTheBall(self.kub.state.ballPos,self.theta)
+				if not vicinity_theta(self.pass_angle,final_theta,thresh=0.1):
+					self.behavior_failed = True
+					break
+		# print("[[[[[[[[[[[[[[[[[[[[[[[[[[[oriented]]]]]]]]]]]]]]]]]]]]]]]]]]]]]")
+		print("in waiting, oriented")
+		while True:
+			state = None
+			state = getState(state)
+			state = state.stateB
+			self.passer.update_state(state)
+			self.receiver.update_state(state)
+			if self.receiver_is_ready():
+				break
+
+	def on_exit_waiting(self):
+		print("exiting waiting")
+		pass
+
+	def on_enter_kicking(self):
+		print("Enter kicking")
+		self.kick_power = self.kicking_power()
+		_GoToPoint_.init(self.passer,Vector2D(self.passer.state.ballPos),self.passer.get_theta())
+		pass
+
+	def execute_kicking(self):
+		# g_fsm = KickToPoint.KickToPoint(self.receiving_point)
+		# g_fsm.power = self.kick_power
+		# state = None
+		# state = getState(state)
+		# state = state.stateB
+		# self.passer.update_state(state)
+		# self.receiver.update_state(state)
+		# g_fsm.add_kub(self.passer)
+		# g_fsm.add_theta(self.pass_angle)
+		# print("***************************** KICKING **************************")
+		# g_fsm.spin()
+		# # self.behavior_failed = g_fsm.behavior_failed
+		# if g_fsm.state == behavior.Behavior.State.completed:
+		# 	self.ball_kicked = True
+		# else:
+		# 	self.behavior_failed = True
+		print(self.kicking_power())
+		start_time = rospy.Time.now()
+		start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)  
+		generatingfunction = _GoToPoint_.execute(start_time,BOT_RADIUS)
+		for gf in generatingfunction:
+			self.passer,ballPos = gf
+			self.passer.kick(self.kicking_power())
+			if not vicinity_points(ballPos,Vector2D(self.passer.state.ballPos),thresh=BOT_RADIUS):
+				self.behavior_failed = True
+				break
+
+	def on_exit_kicking(self):
+		print("exiting kicking")
+		self.passer.reset()
+		self.passer.execute()
+		# # if not self.behavior_failed:
+		# self.ball_kicked = True
+		# # else:
+		# # 	print("noooooooooooooooooooooooooooooooooooooooooooooooooooooo!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
+		pass
+
+	def on_enter_receiver(self):
+		print("Entering receiver")
+		_turnAround_.init(self.receiver,normalize_angle(self.pass_angle+math.pi))
+		pass
+
+	def execute_receiver(self):
+		# target_point = Vector2D(self.receiver.get_pos().x,self.receiver.get_pos().y)
+		# g_fsm = GoToPoint.GoToPoint()
+		# g_fsm.add_kub(self.receiver)
+		# g_fsm.add_point(target_point,normalize_angle(self.pass_angle+math.pi))
+		# g_fsm.spin()
+		# self.behavior_failed = g_fsm.behavior_failed
+		print(self.receiver.kubs_id)
+		start_time = rospy.Time.now()
+		start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)   
+		generatingfunction = _turnAround_.execute(start_time,0.1)
+		for gf in generatingfunction:
+			self.receiver,final_theta = gf
+			# self.target_point = getPointBehindTheBall(self.kub.state.ballPos,self.theta)
+			if not vicinity_theta(angle_diff(Vector2D(self.receiver.get_pos()),Vector2D(self.receiver.state.ballPos)),final_theta,thresh=0.1):
+				self.behavior_failed = True
+				break
+
+	def on_exit_receiver(self):
+		print("exiting receiver")
+		pass
+
+	def on_enter_going_to_receiving_point(self):
+		print("going to receiving point")
+		if not vicinity_points(self.receiving_point,self.calculate_receiving_point(),thresh=0.5*BOT_RADIUS):
+			self.receiving_point = self.calculate_receiving_point()
+		_GoToPoint_.init(self.receiver,self.receiving_point,self.receiver.get_theta())
+		pass
+
+	def execute_going_to_receiving_point(self):
+		start_time = rospy.Time.now()
+		start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)
+		generatingfunction = _GoToPoint_.execute(start_time,BOT_RADIUS)
+		for gf in generatingfunction:
+			self.receiver,receiving_point = gf
+			if not vicinity_points(self.calculate_receiving_point(),receiving_point,BOT_RADIUS):
+				self.behavior_failed = True
+				break
+
+		# g_fsm = GoToPoint.GoToPoint()
+		# state = None
+		# state = getState(state)
+		# state = state.stateB
+		# self.passer.update_state(state)
+		# self.receiver.update_state(state)
+		# g_fsm.add_kub(self.receiver)
+		# g_fsm.add_point(self.receiving_point,self.receiver.get_theta())
+		# print(self.receiving_point.x, self.receiving_point.y)
+		# g_fsm.spin()
+		# self.behavior_failed = g_fsm.behavior_failed
+
+		# while True:
+		# 	state = None
+		# 	state = getState(state)
+		# 	state = state.stateB
+		# 	self.passer.update_state(state)
+		# 	self.receiver.update_state(state)
+		# 	self.receiver.dribble = True
+		# 	self.receiver.execute()
+		# 	if self.receiver.has_ball():
+		# 		break
+
+	def on_exit_going_to_receiving_point(self):
+		print("reached receiving point")
+		pass
+
+	def on_enter_receiving(self):
+		print("entered receiving")
+		pass
+
+	def execute_receiving(self):
+		while True:
+			state = None
+			state = getState(state).stateB
+			self.receiver.update_state(state)
+			self.receiver.dribble(True)
+			self.receiver.execute()
+			if self.ball_with_receiver() or not self.ball_moving():
+				break
+
+	def on_exit_receiving(self):
+		print("exiting receiving")
+		pass
+
+	def on_enter_received(self):
+		print("Entered received")
+		self.receiver.reset()
+		pass
+
+	def execute_received(self):
+		self.receiver.execute()
+
+	def on_exit_received(self):
+		print("exiting received")
+		pass
+		
+
+
+
+
+
+
diff --git a/run_goalie.py b/run_goalie.py
index dd2b24b2..a2a4920a 100644
--- a/run_goalie.py
+++ b/run_goalie.py
@@ -1,4 +1,4 @@
-KUB_ID = 0
+KUB_ID = 1
 
 import rospy,sys
 from utils.geometry import Vector2D
diff --git a/tactics/Goalie.py b/tactics/Goalie.py
index d9012a70..88181c3e 100644
--- a/tactics/Goalie.py
+++ b/tactics/Goalie.py
@@ -60,7 +60,7 @@ def __init__(self,continuous=False):
 
 		self.add_transition(Goalie.State.block, Goalie.State.clear, lambda: self._block_to_clear(),"clear now")
 
-		self.add_transition(Goalie.State.block, Goalie.State.protect, lambda: self._peace_to_protect() and not self._peace_to_block,"remain vigilant")
+		self.add_transition(Goalie.State.block, Goalie.State.protect, lambda: self._peace_to_protect() and not self._peace_to_block(),"remain vigilant")  #make changes here
 		
 		self.add_transition(Goalie.State.block, Goalie.State.peace, lambda: self.peace_out(),"relax")
 		
@@ -93,7 +93,7 @@ def peace_out(self):
 			print e
 		if state:
 			state = state.stateB
-		if state.ballPos.x > 0:
+		if state.ballPos.x >= 0 and opponent_bot_with_ball(state) == None:
 			return True
 		return False
 
@@ -116,8 +116,9 @@ def _peace_to_protect(self):
 			print e
 		if state:
 			state = state.stateB
-		return  (opponent_bot_with_ball(state) != None) or ((state.ballVel.x < -self.MIN_VEL or state.ballPos.x<=0) and \
-			not (state.ballPos.x < -HALF_FIELD_MAXX + 2*OUR_GOAL_MAXX))
+		# return  (opponent_bot_with_ball(state) != None) or ((state.ballVel.x < -self.MIN_VEL or state.ballPos.x<=0) and \
+		# 	not (state.ballPos.x < -HALF_FIELD_MAXX + 2*OUR_GOAL_MAXX))
+		return state.ballPos.x < 0 and opponent_bot_with_ball(state) == None
 
 	def _peace_to_block(self):
 		state = None
@@ -127,7 +128,8 @@ def _peace_to_block(self):
 			print e
 		if state:
 			state = state.stateB
-		return  (opponent_bot_with_ball(state) != None) and ((state.ballPos.x < -HALF_FIELD_MAXX and not state.ballPos.x < -HALF_FIELD_MAXX + 2*OUR_GOAL_MAXX) or (abs(state.ballPos.y) > OUR_GOAL_MAXY and state.ballPos.x < -HALF_FIELD_MAXX + 2*OUR_GOAL_MAXX))
+		# return  (opponent_bot_with_ball(state) != None) and ((state.ballPos.x < 0 and not state.ballPos.x < -HALF_FIELD_MAXX + 2*OUR_GOAL_MAXX) or (abs(state.ballPos.y) > OUR_GOAL_MAXY and state.ballPos.x < -HALF_FIELD_MAXX + 2*OUR_GOAL_MAXX)) #make changes here
+		return ball_moving_towards_our_goal(state) or opponent_bot_with_ball(state) != None
 
 	def _protect_to_clear(self):
 		state = None
@@ -149,10 +151,7 @@ def _block_to_clear(self):
 			state = state.stateB
 		return state.ballPos.x < -HALF_FIELD_MAXX + 2*OUR_GOAL_MAXX and abs(state.ballPos.y) < OUR_GOAL_MAXY
 
-	# note that execute_running() gets called BEFORE any of the execute_SUBSTATE methods gets called
-
-	def execute_peace(self):
-		print("Sab moh maya hain")
+	def peace_expected(self):
 		state = None
 		try:
 			state = getState(state)
@@ -160,33 +159,96 @@ def execute_peace(self):
 			print e
 		if state:
 			state = state.stateB
+
+		self.kub.update_state(state)
+
 		if abs(state.ballPos.y) < OUR_GOAL_MAXY:
-			expected_y = state.ballPos.y
+				expected_y = state.ballPos.y
 		else:
 			if state.ballPos.y < 0:
 				expected_y = OUR_GOAL_MINY
 			else:
 				expected_y = OUR_GOAL_MAXY
-		print("In Peace :", state.ballPos.y)
+
 		point = Vector2D(-HALF_FIELD_MAXX + 5*BOT_RADIUS, expected_y)
+		return point
+
+	def protect_expected(self):
+		state = None
+		try:
+			state = getState(state)
+		except rospy.ServiceException, e:
+			print e
+		if state:
+			state = state.stateB
+
+		self.kub.update_state(state)
+
+		if abs(state.ballVel.x) < 10:
+			if abs(state.ballPos.y) < OUR_GOAL_MAXY:
+				expected_y = state.ballPos.y
+			else:
+				if state.ballPos.y < 0:
+					expected_y = OUR_GOAL_MINY
+				else:
+					expected_y = OUR_GOAL_MAXY
+		else:
+			angle = state.ballVel.y/state.ballVel.x
+			expected_y = angle*(-HALF_FIELD_MAXX + 3*BOT_RADIUS - state.ballPos.x) + state.ballPos.y
+		point = Vector2D()
+
+		if expected_y < OUR_GOAL_MINY:
+			expected_y = OUR_GOAL_MINY
+		if expected_y > OUR_GOAL_MAXY:
+			expected_y = OUR_GOAL_MAXY
+		point = Vector2D(-HALF_FIELD_MAXX + 3*BOT_RADIUS, expected_y)
+		return point
+
+	def block_expected(self):
+		state = None
+		try:
+			state = getState(state)
+		except rospy.ServiceException, e:
+			print e
+		if state:
+			state = state.stateB
+
+		self.kub.update_state(state)
+
+		messi = opponent_bot_with_ball(state)
+		if messi != None:
+			angle = state.awayPos[messi].theta
+		else:
+			angle = atan2(state.ballVel.y, state.ballVel.x)
+		expected_y = (math.tan(angle))*(-HALF_FIELD_MAXX + BOT_RADIUS - state.ballPos.x) + state.ballPos.y
+		point = Vector2D()
+
+		if expected_y < OUR_GOAL_MINY:
+			expected_y = OUR_GOAL_MINY
+		if expected_y > OUR_GOAL_MAXY:
+			expected_y = OUR_GOAL_MAXY
+		point = Vector2D(-HALF_FIELD_MAXX + BOT_RADIUS, expected_y)
+		return point,angle
+
+	# note that execute_running() gets called BEFORE any of the execute_SUBSTATE methods gets called
+
+	def execute_peace(self):
+		print("Sab moh maya hain")
 		start_time = rospy.Time.now()
 		start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)   
 		generatingfunction = _GoToPoint_.execute(start_time,BOT_BALL_THRESH)
 		for gf in generatingfunction:
 			self.kub,aim = gf
-			print("peace gf", point.x + point.y)
-			if not vicinity_points(aim,point,thresh=BOT_RADIUS) or self.outOfField(state):
+			point = self.peace_expected()
+			# print("peace gf", point.x + point.y)
+			if not vicinity_points(aim,point,thresh=BOT_RADIUS):   #make changes here
 				self.behavior_failed = True
 				print("Bhenchod:")
 				break
 
-
-		pass
-
-
 	def execute_clear(self):
 		print("Clear kar raha hoon")
-		target = Vector2D(4000,0)
+		target = Vector2D(HALF_FIELD_MAXX,0)
 		state = None
 		try:
 			state = getState(state)
@@ -198,65 +260,30 @@ def execute_clear(self):
 		self.ktp.add_kub(self.kub)
 		self.ktp.add_theta(theta=normalize_angle(atan2(target.y - state.ballPos.y,target.x - state.ballPos.x)))
 		self.ktp.spin()
+		self.behavior_failed = self.ktp.behavior_failed
 
 	def execute_protect(self):
-		state = None
-		try:
-			state = getState(state)
-		except rospy.ServiceException, e:
-			print e
-		if state:
-			state = state.stateB
-		print("main rakshak hoon")
-		if state.ballVel.x < -10:  
-			angle = state.ballVel.y/state.ballVel.x
-			expected_y = angle*(-HALF_FIELD_MAXX + 3*BOT_RADIUS - state.ballPos.x) + state.ballPos.y
-		else:
-			if abs(state.ballPos.y) < OUR_GOAL_MAXY:
-				expected_y = state.ballPos.y
-			else:
-				if state.ballPos.y < 0:
-					expected_y = OUR_GOAL_MINY
-				else:
-					expected_y = OUR_GOAL_MAXY
-		point = Vector2D(-HALF_FIELD_MAXX + 3*BOT_RADIUS,expected_y)
 		start_time = rospy.Time.now()
 		start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)   
 		generatingfunction = _GoToPoint_.execute(start_time,BOT_BALL_THRESH)
 		for gf in generatingfunction:
 			self.kub,aim = gf
-			print("gf", point.x, point.y)
-			if not vicinity_points(aim,point,thresh=BOT_RADIUS) or self.outOfField(state):
+			point = self.protect_expected()
+			# print("gf", point.x, point.y)
+			if not vicinity_points(aim,point,thresh=BOT_RADIUS):
 				self.behavior_failed = True
 				print("Fail Fail")
 				break
 
 	def execute_block(self):
-		state = None
-		try:
-			state = getState(state)
-		except rospy.ServiceException, e:
-			print e
-		if state:
-			state = state.stateB
-		print("main de Gea hoon")
-		messi = opponent_bot_with_ball(state)
-		angle = state.awayPos[messi].theta
-		expected_y = (math.tan(angle))*(-HALF_FIELD_MAXX + BOT_RADIUS - state.ballPos.x) + state.ballPos.y
-		point = Vector2D()
-
-		if expected_y < OUR_GOAL_MINY:
-			expected_y = OUR_GOAL_MINY
-		if expected_y > OUR_GOAL_MAXY:
-			expected_y = OUR_GOAL_MAXY
-		point = Vector2D(-HALF_FIELD_MAXX + BOT_RADIUS,expected_y)
 		start_time = rospy.Time.now()
 		start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)   
 		generatingfunction = _GoToPoint_.execute(start_time,BOT_BALL_THRESH)
 		for gf in generatingfunction:
 			self.kub,aim = gf
-			print("gf", point.x, point.y)
-			if not vicinity_points(aim,point,thresh=BOT_RADIUS) or self.outOfField(state):
+			point,angle = self.block_expected()
+			# print("gf", point.x, point.y)
+			if not vicinity_points(aim,point,thresh=BOT_RADIUS):
 				self.behavior_failed = True
 				print("Fail Fail")
 				break
@@ -270,87 +297,27 @@ def on_exit_protect(self):
 	def on_exit_block(self):
 		pass
 	def on_enter_peace(self):
-		state = None
-		try:
-			state = getState(state)
-		except rospy.ServiceException, e:
-			print e
-		if state:
-			state = state.stateB
-		if abs(state.ballPos.y) < OUR_GOAL_MAXY:
-				expected_y = state.ballPos.y
-		else:
-			if state.ballPos.y < 0:
-				expected_y = OUR_GOAL_MINY
-			else:
-				expected_y = OUR_GOAL_MAXY
-
-		point = Vector2D(-HALF_FIELD_MAXX + 5*BOT_RADIUS, expected_y)
-		theta = self.kub.get_pos().theta
+		point = self.peace_expected()
+		theta = angle_diff(point,self.kub.state.ballPos)
 		_GoToPoint_.init(self.kub, point, theta)
+		print [point.x, point.y]
 		pass
 	def on_enter_clear(self):
 		pass
 	def on_enter_protect(self):
-		state = None
-		try:
-			state = getState(state)
-		except rospy.ServiceException, e:
-			print e
-		if state:
-			state = state.stateB
-		#expected_y = goalie_expected_y(state, self.kub.kubs_id)
-		if abs(state.ballVel.x) < 10:
-			if abs(state.ballPos.y) < OUR_GOAL_MAXY:
-				expected_y = state.ballPos.y
-			else:
-				if state.ballPos.y < 0:
-					expected_y = OUR_GOAL_MINY
-				else:
-					expected_y = OUR_GOAL_MAXY
-		else:
-			angle = state.ballVel.y/state.ballVel.x
-			expected_y = angle*(-HALF_FIELD_MAXX + 3*BOT_RADIUS - state.ballPos.x) + state.ballPos.y
-		point = Vector2D()
-
-		if expected_y < OUR_GOAL_MINY:
-			expected_y = OUR_GOAL_MINY
-		if expected_y > OUR_GOAL_MAXY:
-			expected_y = OUR_GOAL_MAXY
-		point = Vector2D(-HALF_FIELD_MAXX + 3*BOT_RADIUS, expected_y)
-		
+		point = self.protect_expected()
 		print("Entered Protect , going to", point.x, point.y)
-		theta = self.kub.get_pos().theta
+		theta = angle_diff(point,self.kub.state.ballPos)
 		_GoToPoint_.init(self.kub, point, theta)
+		print [point.x, point.y]
 		pass
 	def on_enter_block(self):
-		state = None
-		try:
-			state = getState(state)
-		except rospy.ServiceException, e:
-			print e
-		if state:
-			state = state.stateB
-		#expected_y = goalie_expected_y(state, self.kub.kubs_id)
-		messi = opponent_bot_with_ball(state)
-		angle = state.awayPos[messi].theta
-		expected_y = (math.tan(angle))*(-HALF_FIELD_MAXX + BOT_RADIUS - state.ballPos.x) + state.ballPos.y
-		point = Vector2D()
-
-		if expected_y < OUR_GOAL_MINY:
-			expected_y = OUR_GOAL_MINY
-		if expected_y > OUR_GOAL_MAXY:
-			expected_y = OUR_GOAL_MAXY
-		point = Vector2D(-HALF_FIELD_MAXX + BOT_RADIUS, expected_y)
-		
-		# if expected_y > 0:
-		# 	point = Vector2D(-HALF_FIELD_MAXX + 2*BOT_RADIUS,expected_y)
-		# else:
-		# 	point = Vector2D(-HALF_FIELD_MAXX + 2*BOT_RADIUS,expected_y)
+		point,angle = self.block_expected()
 		print("Entered Block , going to", point.x, point.y)
-		theta = angle + 3.141592654
+		theta = angle + math.pi
 		_GoToPoint_.init(self.kub, point, theta)
-	pass
+		print [point.x, point.y]
+		pass
 
 
 
diff --git a/test_Kick.py b/test_Kick.py
index bfc117e7..aa1e9000 100644
--- a/test_Kick.py
+++ b/test_Kick.py
@@ -12,49 +12,52 @@
 from krssg_ssl_msgs.srv import bsServer
 from math import atan2,pi
 from utils.functions import *
-pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000)
 
 
 
+import memcache
+shared = memcache.Client(['127.0.0.1:11211'],debug=True)
+
 
 
-def function(id_,state):
+def function(id_,state,pub):
 	kub = kubs.kubs(id_,state,pub)
 	kub.update_state(state)
 	print(kub.kubs_id)
-	target = Vector2D(0,0)
-	#ub.state.homePos[kub.kubs_id].theta += 3.1412
-	g_fsm = KickToPoint.KickToPoint(target)
+	# g_fsm = GoToBall.GoToBall()
 	# g_fsm = GoToPoint.GoToPoint()
+	g_fsm = KickToPoint.KickToPoint(Vector2D(-HALF_FIELD_MAXX, 0))
 	g_fsm.add_kub(kub)
+	theta = angle_diff(state.ballPos, Vector2D(-HALF_FIELD_MAXX, 0))
+	g_fsm.add_theta(theta)
 	# g_fsm.add_point(point=kub.state.ballPos,orient=normalize_angle(pi+atan2(state.ballPos.y,state.ballPos.x-3000)))
-	g_fsm.add_theta(theta=normalize_angle(atan2(target.y - state.ballPos.y,target.x - state.ballPos.x)))
-	#g_fsm.as_graphviz()
-	#g_fsm.write_diagram_png()
+	# g_fsm.add_theta(theta=normalize_angle(pi+atan2(state.ballPos.y,state.ballPos.x+3000)))
+	g_fsm.as_graphviz()
+	g_fsm.write_diagram_png()
+	#print('something before spin')
 	g_fsm.spin()
 	# 
 
 
 
-#print str(kub.kubs_id) + str('***********')
+pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000)
 rospy.init_node('node',anonymous=False)
-start_time = rospy.Time.now()
-start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)   
-
-# rospy.Subscriber('/belief_state', BeliefState, BS_callback, queue_size=1000)
 
 while True:
-	state=None
+	state = None
+	#state=shared.get('state')
 	rospy.wait_for_service('bsServer',)
 	getState = rospy.ServiceProxy('bsServer',bsServer)
 	try:
 		state = getState(state)
 	except rospy.ServiceException, e:
-		print e	
+		print("chutiya")		
 	if state:
-		function(1,state.stateB)
+		#print('lasknfcjscnajnstate',state.stateB.homePos)
+		function(1,state.stateB,pub)
+		#print('chal ja')
 		# break
-rospy.spin()
+rospy.spin()	
 
 
 
diff --git a/test_point.py b/test_point.py
index 1d8992e6..61b1f40b 100644
--- a/test_point.py
+++ b/test_point.py
@@ -25,7 +25,7 @@ def function(id_,state):
 	g_fsm = GoToPoint.GoToPoint()
 	# g_fsm = GoToPoint.GoToPoint()
 	g_fsm.add_kub(kub)
-	g_fsm.add_point(point=Vector2D(state.homePos[id_].x,state.homePos[id_].y),orient=normalize_angle(pi+atan2(state.ballPos.y-state.homePos[id_].y,state.ballPos.x-state.homePos[id_].x)))
+	g_fsm.add_point(point=Vector2D(-5000.0,0))
 	#g_fsm.add_theta(theta=normalize_angle(pi+atan2(state.ballPos.y,state.ballPos.x+3000)))
 	g_fsm.as_graphviz()
 	g_fsm.write_diagram_png()
diff --git a/test_role.py b/test_role.py
index d8000a08..49c01fe0 100644
--- a/test_role.py
+++ b/test_role.py
@@ -42,20 +42,18 @@ def function(id_,state):
 
 # rospy.Subscriber('/belief_state', BeliefState, BS_callback, queue_size=1000)
 
-while True:
-	state = None
-	rospy.wait_for_service('bsServer',)
-	getState = rospy.ServiceProxy('bsServer',bsServer)
-	try:
-		state = getState(state)
-	except rospy.ServiceException, e:
-		print("Error ",e)	
-	if state:
-		print('lasknfcjscnajnstate',state.stateB.homePos)
-		function(1,state.stateB)
-		print('chal ja')
-		# break
-rospy.spin()	
+state = None
+rospy.wait_for_service('bsServer',)
+getState = rospy.ServiceProxy('bsServer',bsServer)
+try:
+	state = getState(state)
+except rospy.ServiceException, e:
+	print("Error ",e)	
+if state:
+	print('lasknfcjscnajnstate',state.stateB.homePos)
+	function(1,state.stateB)
+	print('chal ja')
+	# break
 
 
 
diff --git a/utils/config.py b/utils/config.py
index 6aff8ec6..b37c502a 100644
--- a/utils/config.py
+++ b/utils/config.py
@@ -13,17 +13,17 @@ def SELECT(sim_param, ssl_param):
 GOAL_DEPTH                   = SELECT(300, 300)
 CENTER_X                     = SELECT(0, 0)
 CENTER_Y                     = SELECT(0, 0)
-HALF_FIELD_MAXX              = SELECT(4500, 4500)
-HALF_FIELD_MAXY              = SELECT(3000, 3000)
-OUR_GOAL_MAXY                = SELECT(350,	500)
-OUR_GOAL_MINY                = SELECT(-350, -500)
+HALF_FIELD_MAXX              = SELECT(6000, 4500)
+HALF_FIELD_MAXY              = SELECT(4500, 3000)
+OUR_GOAL_MAXY                = SELECT(600,	500)
+OUR_GOAL_MINY                = SELECT(-600, -500)
 OUR_GOAL_WIDTH               = OUR_GOAL_MAXY - OUR_GOAL_MINY
 CENTER_CIRCLE_DIAMETER       = SELECT(1000,1000)
 CENTER_CIRCLE_RADIUS		 = SELECT(500,500)
-DBOX_WIDTH                   = SELECT(1400,2000)	#Along y       
-DBOX_HEIGHT					 = SELECT(700,1000)	    #Along x      
-OUR_DBOX_MAXY				 = SELECT(700,1000)
-OUR_DBOX_MINY				 = SELECT(-700,-1000)
+DBOX_WIDTH                   = SELECT(2400,2000)	#Along y       
+DBOX_HEIGHT					 = SELECT(1200,1000)	    #Along x      
+OUR_DBOX_MAXY				 = SELECT(1200,1000)
+OUR_DBOX_MINY				 = SELECT(-1200,-1000)
 OUR_DBOX_X 					 =-HALF_FIELD_MAXX + DBOX_HEIGHT
 # Old DBOX Constants
 DBOX_WIDTH_OLD                   = SELECT(1000,1000)       # Along X direction
diff --git a/utils/math_functions.py b/utils/math_functions.py
index 2a20bc01..d4b0a30f 100644
--- a/utils/math_functions.py
+++ b/utils/math_functions.py
@@ -94,7 +94,7 @@ def intersection_with_circle(self, circle):
 
 	# CHECK ---->  SLOPE
 	def intersection_with_line(self, line2):
-		if not isinstance(line, line2):
+		if not isinstance(line2, Line):
 			raise ValueError("Expected Line instance, got %s" %type(line2).__name__)
 		c1 = self.point.y - self.slope * self.point.x
 		c2 = line2.point.y - line2.slope * line2.point.x
@@ -108,6 +108,7 @@ def intersection_with_line(self, line2):
 			return P
 		except:
 			return None
+		return P
 	##
 	# @brief      Find Prependicular distance of point from line
 	# https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line
diff --git a/utils/state_functions.py b/utils/state_functions.py
index 7a0afe16..8fed2638 100644
--- a/utils/state_functions.py
+++ b/utils/state_functions.py
@@ -31,9 +31,9 @@ def kub_has_ball(state, kub_id, is_opponent=False):
 ##
 def ball_moving_towards_our_goal(state):
     ballPos = Vector2D(state.ballPos.x, state.ballPos.y)
-    ballvel = Vector2D(state.ballvel.x, state.ballvel.y)
+    ballvel = Vector2D(state.ballVel.x, state.ballVel.y)
     if ballvel.absSq(ballvel) > 0.1:
-        ball_movement = Line(ballPos, ballPos + ballvel)
+        ball_movement = Line(ballPos, math.atan2(ballvel.y,ballvel.x))
         ptA = Vector2D(-HALF_FIELD_MAXX, DBOX_HEIGHT)
         ptB = Vector2D(-HALF_FIELD_MAXX, -DBOX_HEIGHT)
         defend_line = Line(point1=ptA,point2=ptB)
diff --git a/velocity/run.py b/velocity/run.py
index 20abb29a..09391357 100644
--- a/velocity/run.py
+++ b/velocity/run.py
@@ -31,7 +31,7 @@ def distance_(a, b):
     return sqrt(dx*dx+dy*dy)
 
 def Get_Vel(start, t, kub_id, target, homePos_, awayPos_,avoid_ball=False):
-    print("Get_Vel:     t - start = ",t-start)       
+    # print("Get_Vel:     t - start = ",t-start)       
     global expectedTraverseTime, REPLAN, v, errorInfo, pso, FIRST_CALL, homePos, awayPos, kubid, prev_target
     REPLAN = 0
     homePos = homePos_
diff --git a/velocity/run_w.py b/velocity/run_w.py
index 7bf378b5..13754ce0 100644
--- a/velocity/run_w.py
+++ b/velocity/run_w.py
@@ -11,7 +11,7 @@ def Get_Omega(kub_id, totalAngle, homePos):
     MAX_w = MAX_BOT_OMEGA
     # theta_left = float(homePos[kub_id].theta-totalAngle)
     theta_lft = normalize_angle(normalize_angle(homePos[kub_id].theta)-totalAngle)*-1.0
-    vw = 3.0*(theta_lft*abs(theta_lft)/(math.pi*math.pi))*MAX_w
+    vw = 10*(theta_lft*abs(theta_lft)/(math.pi*math.pi))*MAX_w
 
     if abs(vw) > MAX_BOT_OMEGA:
         vw = (vw/abs(vw))*MAX_BOT_OMEGA
diff --git a/vision_comm/src/kalman_filter.py b/vision_comm/src/kalman_filter.py
index 99958031..3ecccd65 100644
--- a/vision_comm/src/kalman_filter.py
+++ b/vision_comm/src/kalman_filter.py
@@ -118,29 +118,32 @@ def listener():
     arr1 = np.array(arr1)
     # plotting the results
     plt.figure()
-    plt.plot(np.arange(0, len(vel_rect)),vel_rect[:,0], color = 'g', label = 'filter')
+    plt.plot(np.arange(0, len(vel_rect)),vel_rect[:,0], color = 'r', label = 'filter')
     plt.xlabel('number of measurments')
     plt.ylabel('x-velocity')
     plt.title('vel_x')
     plt.legend()
+    plt.savefig('x_vel.png', dpi = 500)
     plt.figure()
-    plt.plot(np.arange(0, len(vel_rect)),vel_rect[:,1], color = 'g', label = 'filter')
+    plt.plot(np.arange(0, len(vel_rect)),vel_rect[:,1], color = 'r', label = 'filter')
     plt.xlabel('number of measurments')
     plt.ylabel('y-velocity')
     plt.title('vel_y')
     plt.legend()
+    plt.savefig('vel_y.png', dpi = 500)
     plt.figure()
-    plt.plot(np.arange(0, len(arr_rect)),arr_rect[:,0], ls='--', color = 'k', label = 'filter')
-    plt.plot(np.arange(0, len(arr1)),arr1[:,0], color = 'r', label = 'vision')
+    plt.plot(np.arange(0, len(arr_rect)),arr_rect[:,0], color = 'r', label = 'filter')
+    plt.plot(np.arange(0, len(arr1)),arr1[:,0], ls='--', color = 'k', label = 'vision')
     plt.xlabel('number of measurments')
     plt.ylabel('x')
     plt.title('pos_x')
     plt.legend()
+    plt.savefig('pos_x.png', dpi = 500)
     plt.figure()
-    plt.plot(np.arange(0, len(arr_rect)),arr_rect[:,1], ls='--', color = 'k', label = 'filter')
-    plt.plot(np.arange(0, len(arr1)),arr1[:,1], color= 'r', label = 'vision')
+    plt.plot(np.arange(0, len(arr_rect)),arr_rect[:,1], color= 'r', label = 'filter')
+    plt.plot(np.arange(0, len(arr1)),arr1[:,1], ls='--', color = 'k', label = 'vision')
     plt.xlabel('number of measurments')
     plt.ylabel('y')
     plt.title('pos_y')
     plt.legend()
-    plt.show()
+    plt.savefig('pos_y.png', dpi = 500)