diff --git a/KickP.py b/KickP.py
new file mode 100644
index 00000000..25c94510
--- /dev/null
+++ b/KickP.py
@@ -0,0 +1,64 @@
+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, KickToPointP
+from multiprocessing import Process
+from kubs import kubs
+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)
+
+BOT_ID=int(sys.argv[1])
+x=float(sys.argv[2])
+y=float(sys.argv[3])
+
+xx=int(x)
+yy=int(y)
+target=Vector2D(x,y)
+
+def function(id_,state):
+    	kub = kubs.kubs(id_,state,pub)
+	kub.update_state(state)
+	print(kub.kubs_id) 
+	g_fsm = KickToPointP.KickToPoint(target)
+	# g_fsm = GoToPoint.GoToPoint()
+	g_fsm.add_kub(kub)
+	# 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.get_pos_as_vec2d(target)
+    #g_fsm.as_graphviz()
+	#g_fsm.write_diagram_png()
+	print('something before spin')
+	g_fsm.spin()
+	# 
+
+
+
+#print str(kub.kubs_id) + str('***********')
+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
+	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(BOT_ID,state.stateB)
+		print('chal ja')
+		# break
+rospy.spin()	
+
diff --git a/role/Dribble_Kick.py b/role/Dribble_Kick.py
new file mode 100644
index 00000000..249c9119
--- /dev/null
+++ b/role/Dribble_Kick.py
@@ -0,0 +1,372 @@
+from enum import Enum
+import behavior
+import rospy
+from role import GoToPoint
+import math
+import kubs
+from utils import math_functions
+from utils.functions import *
+from utils.state_functions import *
+from utils.config import *
+from velocity.run import *
+from krssg_ssl_msgs.srv import *
+import krssg_ssl_msgs.msg
+import _turnAround_, _GoToPoint_
+
+start_time = None
+DIST_THRESH = 75
+dboxp = Vector2D((abs(OUR_DBOX_X)-(DBOX_HEIGHT/2)),0)
+
+class DribbleKick(behavior.Behavior):
+	"""docstring for Dribble-and-Kick-into-Goal"""
+	class State(Enum):
+		
+		normal = 1
+		movetoball = 2
+		align = 3
+		dribble = 4
+		goal = 5
+
+	def __init__(self,target):
+
+		super(DribbleKick,self).__init__()
+
+		self.name = "Dribble_Kick"
+
+		self.goal_point = target
+
+		self.go_at = None
+
+		self.target_point = None
+
+		self.dbox = dboxp
+
+		self.power = 0        
+		
+		self.approachball = False
+
+		self.turn = False
+
+		self.goto_dribble = False
+
+		self.approachdbox = False
+
+		self.readytogoal = False
+
+		self.theta = 0
+
+		self.behavior_failed = False
+
+
+		#States:-
+		self.add_state(DribbleKick.State.normal, behavior.Behavior.State.running)
+
+		self.add_state(DribbleKick.State.movetoball, behavior.Behavior.State.running)
+
+		self.add_state(DribbleKick.State.align, behavior.Behavior.State.running)
+		
+		self.add_state(DribbleKick.State.dribble, behavior.Behavior.State.running)
+
+		self.add_state(DribbleKick.State.goal, behavior.Behavior.State.running)
+
+
+		#Transition Functions:-
+		self.add_transition(behavior.Behavior.State.start,
+			DribbleKick.State.normal,lambda: True,'immediately')
+
+		self.add_transition(DribbleKick.State.normal,
+			DribbleKick.State.movetoball, lambda: self.reachball(),'far_away_from_ball')
+		
+		self.add_transition(DribbleKick.State.movetoball,
+			DribbleKick.State.align, lambda: self.alignnow(),'Aligning_')
+		
+		self.add_transition(DribbleKick.State.align,
+			DribbleKick.State.dribble, lambda: self.goto_dribble,'Dribbling_')
+
+		self.add_transition(DribbleKick.State.dribble, 
+			DribbleKick.State.goal, lambda: self.readytogoal, 'Goaling!')
+
+		self.add_transition(DribbleKick.State.goal,
+			behavior.Behavior.State.completed, lambda: self.done(),'Ball inside Goal')
+		
+		#Behavior - Failed !!!
+		self.add_transition(DribbleKick.State.movetoball,
+			behavior.Behavior.State.failed, lambda: self.behavior_failed, 'failed')
+		
+		self.add_transition(DribbleKick.State.align,
+			behavior.Behavior.State.failed, lambda: self.behavior_failed, 'failed')
+			
+		self.add_transition(DribbleKick.State.dribble,
+			behavior.Behavior.State.failed, lambda: self.behavior_failed, 'failed')
+		
+
+	def add_kub(self,kub):
+		self.kub = kub
+	
+	def add_theta(self,theta):
+		self.theta = theta
+	
+	def get_pos_asvec2d(self, point2d):
+		return Vector2D(int(point2d.x),int(point2d.y))
+	
+	def add_target_theta(self,theta):
+		self.theta = theta
+
+	def point_on_D_Box(self):
+		return self.get_pos_asvec2d(self.dbox)
+	
+	def bot_moving(self):
+		if abs(self.kub.state.homeVel[self.kub.kubs_id].x) != 0 or abs(self.kub.state.homeVel[self.kub.kubs_id].y) != 0:
+			return True
+		return False
+	
+	def kicking_power(self):
+		return math.sqrt(dist(self.goal_point,self.kub.state.ballPos)/6400.0)*5.0 
+	
+#LAMBDA FUNCTIONS:-
+	def reachball(self):
+		ballpos = self.kub.state.ballPos
+		theta = angle_diff(ballpos, self.point_on_D_Box())
+		go_at = getPointBehindTheBall(ballpos, theta, -2)
+		return not(self.turn) and not(self.goto_dribble) and go_at.dist(self.get_pos_asvec2d(self.kub.get_pos())) >= DISTANCE_THRESH*0.25 and self.approachball
+
+	def alignnow(self):
+		ballpos = self.kub.state.ballPos
+		theta = angle_diff(ballpos, self.point_on_D_Box())
+		go_at = getPointBehindTheBall(ballpos, theta, -2)
+		return not (go_at.dist(self.get_pos_asvec2d(self.kub.get_pos())) >= DISTANCE_THRESH*0.25) and self.turn and not(self.goto_dribble)
+
+	def done(self):
+		ballpos = self.get_pos_asvec2d(self.kub.state.ballPos)
+		condition = (ballpos.x >= 4500) and (ballpos.y > -300) and (ballpos.y < 300)
+		return condition
+	
+# STATE FUNCTIONS:-
+	def on_enter_normal(self):
+		pass
+
+	def execute_normal(self):
+		self.approachball=True
+
+	def on_exit_normal(self):
+		pass
+
+	def on_enter_movetoball(self):
+		print("Entered movetoball")
+		self.theta = normalize_angle(angle_diff(self.kub.state.ballPos,self.point_on_D_Box()))
+		self.power = 0 
+		self.go_at = getPointBehindTheBall(self.kub.state.ballPos, self.theta, -2)
+		_GoToPoint_.init(self.kub, self.go_at, 0)
+
+	def execute_movetoball(self):
+		
+		print("Executing MOVETOBALL ... ")
+		start_time = rospy.Time.now()
+		start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)  
+		print "----------------------------------------------------------------"
+
+		generatingfunction = _GoToPoint_.execute(start_time, BOT_RADIUS, True)
+		
+		for gf in generatingfunction:
+			self.kub,target_point = gf
+			# self.target_point = getPointBehindTheBall(self.kub.state.ballPos,self.theta)
+			theta = angle_diff(self.kub.state.ballPos, self.point_on_D_Box())
+			print(self.theta)
+			#print theta,"------------------------"
+			self.go_at = getPointBehindTheBall(self.kub.state.ballPos, theta, -1.5)
+			# #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):
+				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
+		self.turn = True
+		print("Reached the ball. Start ALIGN ... ")
+		self.approachball = False
+
+	def on_exit_movetoball(self):
+		print("I have reached the ball")
+		self.turn = True
+		
+
+	def on_enter_align(self):
+		print("Entering align: ", self.turn)
+		ballpos = self.kub.state.ballPos
+		self.theta = angle_diff(self.kub.get_pos(),ballpos)
+		self.go_at = getPointBehindTheBall(ballpos, self.theta, -2)
+
+	def execute_align(self):
+		
+		READY_TO_DRIBBLE = self.goto_dribble
+		theta = self.theta
+
+		rospy.wait_for_service('bsServer',)
+		getState = rospy.ServiceProxy('bsServer',bsServer)
+
+		prev_state = None   #HERE decalred None
+		#What should it be?
+		try:
+				prev_state = getState(prev_state).stateB
+		except rospy.ServiceException, e:
+				print("Error :: ROS ", e)
+
+		vw=0.0
+		while not READY_TO_DRIBBLE: 
+
+			try:
+				prev_state = getState(prev_state).stateB
+			except rospy.ServiceException, e:
+				print("Error :: ROS ", e)
+			# print(kub.state)
+			self.kub.update_state(prev_state)
+			if not(prev_state == self.kub.state):
+				prev_state = self.kub.state
+		
+			print(self.kub.get_theta())
+			print("Target Alignment : ",theta)
+			totalAngle = theta
+			MAX_w = (MAX_BOT_OMEGA+MIN_BOT_OMEGA)/1.2
+			MIN_w = (MIN_BOT_OMEGA)
+
+				# theta2 = totalAngle :-- target angular alignment ; theta1 = current_bot_theta; New x-axis: Bot_theta_line
+				# rotating standard axes
+			deltheta = totalAngle-normalize_angle(self.kub.get_theta()) 
+			modtheta = min(abs(deltheta),abs((2*math.pi)-deltheta))  #shortest turn
+			sign = (normalize_angle(deltheta))/(abs(normalize_angle(deltheta))) 
+
+			print "Remaining angle: ",modtheta,"   ",sign
+				
+			theta_lft = modtheta * sign    
+			if abs(theta_lft)<ROTATION_FACTOR/2:
+				vw = 0.0
+				READY_TO_DRIBBLE=True
+			else:
+				READY_TO_DRIBBLE=False
+				vw = 3.0*(theta_lft)/(math.pi)*MAX_w
+					#vw = (theta_lft/2*math.pi)*MAX_w
+				
+			if abs(vw)<1*MIN_w and READY_TO_DRIBBLE==False:
+				vw = 1*MIN_w*(1 if vw>0 else -1)
+				
+			if abs(vw) > MAX_w and READY_TO_DRIBBLE==False:
+				vw = (vw/abs(vw))*MAX_w
+						
+			if vw!=0:
+				print("TURNING")
+						#print("vw Hoon Main =",vw)
+						#print("BOT THETA:",data.homePos[BOT_ID].theta)
+				print "\nAngle Remaining : ",theta_lft
+			else:
+				print("DONE !!")
+				break
+					
+			print "Omega Return: ",vw
+			print READY_TO_DRIBBLE
+			print "___________________________________________________________________"
+			self.kub.reset()
+			self.kub.turn(vw)
+			self.kub.execute()
+		
+		try:
+			prev_state = getState(prev_state).stateB
+		except rospy.ServiceException, e:
+			print("Error :: ROS ", e)
+			# print(kub.state)
+		self.kub.update_state(prev_state)
+		if not(prev_state == self.kub.state):
+			prev_state = self.kub.state
+
+		self.turn = False
+		self.goto_dribble = True
+		print("HO GYA ALIGN !! ---------------------------------------")
+
+	def on_exit_align(self):
+		print("EXITING ALIGN... goto_dribble:",self.goto_dribble)
+		self.goto_dribble = True #Now, Ready to dribble after aligning
+		self.approachball = False
+		self.turn = False
+		
+
+	def on_enter_dribble(self):
+		print("Enter DRIBBLE------------------------------")
+		self.go_at = self.point_on_D_Box()
+		ballpos = self.kub.state.ballPos
+		#self.theta = normalize_angle(angle_diff(self.point_on_D_Box(), self.get_pos_asvec2d(self.goal_point)))
+		self.theta = angle_diff(ballpos, self.get_pos_asvec2d(self.goal_point))
+		print "Required Rotation while dribble: ",self.theta
+		print "Goal Point: ",self.goal_point
+		self.kub.reset()
+		self.approachball = False
+		self.kub.dribbler = True
+		self.kub.execute()
+		_GoToPoint_.init(self.kub, self.go_at, 0)
+		
+
+	def execute_dribble(self):
+
+		print("Executing DRIBBLE ... ")
+		start_time = rospy.Time.now()
+		start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)  
+		print "----------------------------------------------------------------"
+		self.kub.dribbler = True
+		self.kub.execute()
+		
+		generatingfunction = _GoToPoint_.execute(start_time, BOT_RADIUS*2.0, False)
+		
+		for gf in generatingfunction:
+			self.kub,target_point = gf
+			# self.target_point = getPointBehindTheBall(self.kub.state.ballPos,self.theta)
+			print "--------------------------------------------------"
+			ballPos=self.kub.state.ballPos
+			print(ballPos)
+			print "--------------------------------------------------"
+			theta = angle_diff(self.kub.state.ballPos, self.goal_point)
+			print(self.theta)
+			self.kub.dribbler = True
+			#self.kub.execute()
+			#self.kub.kick(self.power)
+			#print theta,"------------------------"
+			#self.go_at = getPointBehindTheBall(self.kub.state.ballPos, theta, -2)
+			# #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("Reached the D_Box. Prepare to Goal ...")
+		self.kub.reset()
+		self.kub.execute()
+		self.goto_dribble = False
+		self.kub.dribbler = False
+		self.readytogoal = True
+		self.kub.execute()
+	
+	def on_exit_dribble(self):
+		print("Dribble executed successfully, now goal. ----------")
+		self.kub.dribbler = False
+		self.kub.execute()
+		self.kub.kick(4)
+		self.kub.execute()
+		self.goto_dribble = False
+		self.go_at = None
+		self.turn = False
+		self.kub.reset()
+		self.kub.execute()
+		print("GOALLL !!!")
+		print("EXITING ROLE and enter GOAL_STATE")
+
+	def on_enter_goal(self):
+		pass
+	def execute_goal(self):
+		self.readytogoal = True
+		pass
+	def on_exit_goal(self):
+		pass
+	
+
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/test_Dribble_Kick.py b/test_Dribble_Kick.py
new file mode 100644
index 00000000..a69ba86c
--- /dev/null
+++ b/test_Dribble_Kick.py
@@ -0,0 +1,55 @@
+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, Dribble_Kick
+from multiprocessing import Process
+from kubs import kubs
+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)
+
+'''Arguments : (BOT_ID) (x coordinate of goal point) (y coordinate of goal point)'''
+# Can try making it dynamic according to goalie's position, during gameplay...
+
+BOT_ID = int(sys.argv[1])
+x=int(sys.argv[2])
+y=int(sys.argv[3])
+
+target=Vector2D(x,y)
+
+def function(id_,state):
+	kub = kubs.kubs(id_,state,pub)
+	kub.update_state(state)
+	print("BOT ",kub.kubs_id ," IN ACTION ------")
+	#ub.state.homePos[kub.kubs_id].theta += 3.1412
+	g_fsm = Dribble_Kick.DribbleKick(target)
+	g_fsm.add_kub(kub)
+	#g_fsm.as_graphviz()
+	#g_fsm.write_diagram_png()
+	g_fsm.spin()
+
+
+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
+	rospy.wait_for_service('bsServer',)
+	getState = rospy.ServiceProxy('bsServer',bsServer)
+	try:
+		state = getState(state)
+	except rospy.ServiceException, e:
+		print e	
+	if state:
+		function(BOT_ID,state.stateB)
+		# break
+rospy.spin()
diff --git a/test_KickToP.py b/test_KickToP.py
index 3f3c3abc..eb47a1bc 100644
--- a/test_KickToP.py
+++ b/test_KickToP.py
@@ -1,8 +1,11 @@
-print "In test GOToPoint"
+print "In test GoToPoint"
 from kubs import kubs, cmd_node
 from velocity.run import *
-import rospy,sys
+import velocity
+import rospy,sys,os
 import math
+import time
+from role import GoToBall, GoToPoint, KickToPointP
 from krssg_ssl_msgs.msg import point_2d
 from krssg_ssl_msgs.msg import BeliefState
 from krssg_ssl_msgs.msg import gr_Commands
@@ -11,19 +14,27 @@
 from utils.math_functions import *
 from utils.config import *
 from krssg_ssl_msgs.srv import bsServer
-import sys
+import subprocess
 
 BOT_ID = int(sys.argv[1])
-print "bot_id received",BOT_ID
+x=int(sys.argv[2])
+y=int(sys.argv[3])
+
+target=Vector2D(x,y)
+
+print "BOT_ID Received",BOT_ID
 pub = rospy.Publisher('/grsim_data', gr_Commands, queue_size=1000)
 
 GOAL_POINT = point_2d()
 GOAL_POINT.x = 1000
 GOAL_POINT.y = 1200
+READY_TO_KICK = False
 REPLANNED = 0
 homePos = None
 awayPos = None
 state = None
+
+
 rospy.wait_for_service('bsServer',)
 getState = rospy.ServiceProxy('bsServer',bsServer)
 try:
@@ -32,7 +43,10 @@
 	print e
 if state:
 	BState = state.stateB
+
 kub = kubs.kubs(BOT_ID, BState, pub)
+
+
 st = None 
 tnow = None 
 planned = False
@@ -40,7 +54,11 @@
 ramp_dnt = 0.0
 ramp_rampt = 0.0
 case = -1
-mvw = 0.0
+
+mvw=1.0
+mvx=1.0
+mvy=1.0
+
 
 def reset():
 	global start_time
@@ -53,130 +71,158 @@ def GUI_Callback(data):
 	print BOT_ID, "_____________________________"
 	kub = kubs.kubs(BOT_ID, BState, pub)
 
+
+
+def function(id_,state):
+	kub = kubs.kubs(id_,state,pub)
+	kub.update_state(state)
+	print(kub.kubs_id) 
+	g_fsm = KickToPointP.KickToPoint(target)
+	# g_fsm = GoToPoint.GoToPoint()
+	g_fsm.add_kub(kub)
+	# 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.get_pos_as_vec2d(target)
+    #g_fsm.as_graphviz()
+	#g_fsm.write_diagram_png()
+	print('something before spin')
+	g_fsm.spin()
+	# 
+
 def kp_callback(data):
-	global st, planned, ramp_rampt, ramp_dnt, ramp_upt, case, mvw, tnow
-	target = Vector2D(0,0)
+	global st, planned, ramp_rampt, ramp_dnt, ramp_upt, case, tnow
+	
 	ballpos = data.ballPos
-	theta = angle_diff(target, ballpos)
-	go_at = getPointToGo(ballpos, theta)
+	theta = angle_diff(ballpos, target)    #Point1: ballpos; Point2: target 
+	go_at = getPointBehindTheBall(ballpos, theta, -2)      #Modify getPoint*Behind*TheBall late on**. Factor should be negative.
+
 	print(radian_2_deg(theta))
 	print(go_at.x)
 	print(go_at.y)
+	
 	t = rospy.Time.now()
 	t = t.secs + 1.0*t.nsecs/pow(10,9)
 	print(" t - start = ",t-start_time)
-	[vx, vy, vw, REPLANNED] = Get_Vel(start_time, t, BOT_ID, go_at, data.homePos, data.awayPos, True)
-		#vx, vy, vw, replanned
-	print("-------------------REPLANNED = ",REPLANNED)
+	[vx, vy, vw, REPLANNED] = Get_Vel(start_time, t, BOT_ID, go_at, data.homePos, data.awayPos, True, 1.2)
+	#vx, vy, vw, replanned
+	print("-------------------  REPLANNED = ",REPLANNED)
 	if(REPLANNED):
-		reset()
+    		reset()
+	
 	print("vx = ",vx)
 	print("vy = ",vy)
+
 	# print("kubs_id = ",kub.kubs_id)
+
 	curPos = Vector2D(int(data.homePos[BOT_ID].x),int(data.homePos[BOT_ID].y))
 	#if vicinity_points(go_at, curPos, 4) == False:
+	d=distance_(curPos,go_at)
 	try:	
 		kub.move(vx, vy)
 		print(vw)
-		print("homePos")
+		print("#Test Statement ---- homePos")
 		kub.turn(vw)
 		kub.execute()
 	except Exception as e:
 		print("In except",e)
 		pass
 	print(dist(go_at, curPos))
-	# if planned == False:
-	# 	theta_lft = abs(normalize_angle(normalize_angle(data.homePos[BOT_ID].theta)-theta))
-	# 	print(theta_lft)
-	# 	if theta_lft >= 5.0*MAX_BOT_OMEGA:
-	# 		case = 1
-	# 		ramp_upt = 5.0
-	# 		ramp_dnt = 5.0
-	# 		ramp_rampt = (theta_lft-5.0*MAX_BOT_OMEGA)*1.0/MAX_BOT_OMEGA
-	# 	elif theta_lft >= 2.5*MAX_BOT_OMEGA:
-	# 		case = 2
-	# 		ramp_upt = 0.0
-	# 		ramp_dnt = 5.0
-	# 		ramp_rampt = (theta_lft-2.5*MAX_BOT_OMEGA)*1.0/MAX_BOT_OMEGA
-	# 	else:
-	# 		case = 3
-	# 		ramp_rampt = 0.0
-	# 		ramp_dnt = sqrt(2.0*theta_lft/MAX_BOT_OMEGA_ACC)*1.0
-	# 		mvw = sqrt(2.0*theta_lft*MAX_BOT_OMEGA_ACC)*1.0
-	# 		ramp_upt = 0.0
-	# 	planned = True
-	# print(case)
-	# print(ramp_rampt)
-	# print(ramp_upt)
-	# print(ramp_dnt)
-	# print(mvw)
-	if vx == 0 and vy == 0:
-		print("TURNING")
+
+
+	#READY TO TAKE STANCE
+	if vx == 0 and vy == 0 and d < 1.2*BOT_BALL_THRESH:  
+    
 		print(data.homePos[BOT_ID].theta)
-		print(theta)
+		print("Target Alignment : ",theta)
 		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(data.homePos[BOT_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:
+		MIN_w = (MIN_BOT_OMEGA)
+
+		# theta2 = totalAngle :-- target angular alignment ; theta1 = current_bot_theta; New x-axis: Bot_theta_line
+		# rotating standard axes
+		deltheta = totalAngle-normalize_angle(data.homePos[BOT_ID].theta) 
+		modtheta = min(abs(deltheta),abs((2*math.pi)-deltheta))  #shortest turn
+		sign = (normalize_angle(deltheta))/(abs(normalize_angle(deltheta))) 
+
+		print "Remaining angle: ",modtheta,"   ",sign
+ 		
+		theta_lft = modtheta * sign  
+
+		if abs(theta_lft)<ROTATION_FACTOR/10:
 			vw = 0.0
+			READY_TO_KICK=True
+		else:
+			READY_TO_KICK=False
+			vw = 3.0*(theta_lft)/(math.pi)*MAX_w
+			#vw = (theta_lft/2*math.pi)*MAX_w
 
-		print "Omega return",vw
+		if abs(vw)<1*MIN_w and READY_TO_KICK==False:
+			vw = 1*MIN_w*(1 if vw>0 else -1)
+		
+		if abs(vw) > MAX_w and READY_TO_KICK==False:
+    			vw = (vw/abs(vw))*MAX_w
+
+		if vw!=0:
+			print("TURNING")
+			#print("vw Hoon Main =",vw)
+			print("BOT THETA: ",data.homePos[BOT_ID].theta)
+			print "\nAngle Remaining : ",theta_lft
+		else:
+			print("DONE !!")
+			
+		print "Omega Return: ",vw
+		print READY_TO_KICK
+		print "___________________________________________________________________"
 		kub.reset()
 		kub.turn(vw)
 		kub.execute()
-		# if st == None:
-		# 	st = rospy.Time.now()
-		# 	st = 1.0*st.secs + 1.0*st.nsecs/pow(10,9)
-		# tnow = rospy.Time.now()
-		# tnow = 1.0*tnow.secs + 1.0*tnow.nsecs/pow(10,9)
-		# diff = tnow-st
-		# if case == 1:
-		# 	if diff <= 5.0:
-		# 		vw = (tnow-st)*MAX_BOT_OMEGA_ACC
-		# 	elif diff > 5.0 and diff <= 5.0+ramp_rampt:
-		# 		vw = MAX_BOT_OMEGA
-		# 	else:
-		# 		tim = 10.0+ramp_rampt-diff
-		# 		vw = tim*MAX_BOT_OMEGA_ACC
-		# elif case == 2:
-		# 	if diff <= ramp_rampt:
-		# 		vw = MAX_BOT_OMEGA
-		# 	else :
-		# 		tim = 5.0+ramp_rampt-diff
-		# 		vw = tim*MAX_BOT_OMEGA_ACC
-		# else :
-		# 	vw = mvw - MAX_BOT_OMEGA_ACC*diff 
-		# if normalize_angle(homePos[BOT_ID].theta) < theta:
-		# 	vw = -1.0*vw
-		# print(vw)
-		# kub.reset()
-		# kub.turn(vw)
-		# kub.execute()
+
+		if READY_TO_KICK  :
+    			print "\nAngle Remaining : ",theta_lft
+    			print "      ____________REACHED BALL, READY TO KICK____________\n"
+    			rospy.signal_shutdown("=======ALIGNED======")
+
 
 
 if __name__ == "__main__":
-	global start_time, st, planned, case, mvw
+	global start_time, st, planned, case
 	case = -1
-	mvw = 0.0
 	st = None
 	planned = False
 	print("here")
+
+	#Begin the process: Driver, START MOVING
+
 	rospy.init_node('node_new',anonymous=False)
 	start_time = rospy.Time.now()
 	start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)
+
+
 	pub = rospy.Publisher('/grsim_data', gr_Commands, queue_size=1000)
 	print('testing...')
-	rospy.Subscriber('/belief_state', BeliefState, kp_callback, queue_size=1000)
-	rospy.Subscriber('/gui_params', point_SF, GUI_Callback, queue_size = 1000)
+	sub1 = rospy.Subscriber('/belief_state', BeliefState, kp_callback, queue_size=1000)
+	
+	sub2 = rospy.Subscriber('/gui_params', point_SF, GUI_Callback, queue_size = 1000)
+
+	print("Ho gya align!!")
 	rospy.spin()
+	
+	sub1.unregister()
+	sub2.unregister()
+	print "AB MAIN KARUNGA -------- KICKKK !!!"
+
+command="python KickP.py %s %s %s"%(BOT_ID,target.x,target.y)
+print("______________________________________________________________")
+print("/n",command,"/n")
+print("______________________________________________________________\n")
+os.system(command)
+	
+
+
+#cmd = "date"
 
+#returns output as byte string
+#returned_output = subprocess.check_output(cmd)
 
+#using decode() function to convert byte string to string
+#print('Current date is:', returned_output.decode("utf-8"))
diff --git a/velocity/run.py b/velocity/run.py
index 09391357..35f1f9ef 100644
--- a/velocity/run.py
+++ b/velocity/run.py
@@ -30,7 +30,7 @@ def distance_(a, b):
     dy = a.y-b.y
     return sqrt(dx*dx+dy*dy)
 
-def Get_Vel(start, t, kub_id, target, homePos_, awayPos_,avoid_ball=False):
+def Get_Vel(start, t, kub_id, target, homePos_, awayPos_,avoid_ball=False, f=1.5):
     # print("Get_Vel:     t - start = ",t-start)       
     global expectedTraverseTime, REPLAN, v, errorInfo, pso, FIRST_CALL, homePos, awayPos, kubid, prev_target
     REPLAN = 0
@@ -53,7 +53,7 @@ def Get_Vel(start, t, kub_id, target, homePos_, awayPos_,avoid_ball=False):
         findPath(startPt, target, avoid_ball)
         FIRST_CALL = 0
 
-    if distance < 1.5*BOT_BALL_THRESH:
+    if distance < f*BOT_BALL_THRESH:
         return [0,0,0,0]
     # print("ex = ",expectedTraverseTime) 
     # print("t = ",t," start = ",start)