Working sTurntoangle

skills/sTurnToAngle.py

@@ -1,28 +1,36 @@
 import skill_node
 import math
-import sys
+from utils.config import *
+from utils.geometry import *
+from navigation_py.wrapperpy import *
+from navigation_py.obstacle import Obstacle
 
-sys.path.append('../../../plays_py/scripts/utils/')
-sys.path.insert(0, '../../../navigation_py/scripts/navigation/src')
-sys.path.insert(0, '../../../navigation_py/scripts/navigation')
+def debug(param, state, bot_id):
+		botPos = Vector2D(state.homePos[bot_id].x, state.homePos[bot_id].y)
+		ballPos = Vector2D(state.ballPos.x, state.ballPos.y)
+		print '#'*50
+		print 'In sTurnToAngle'
+		print 'Current bot pos: {}, {}'.format(state.homePos[bot_id].x, state.homePos[bot_id].y)
+		print 'Target alignment: {}'.format(param.TurnToAngleP.finalSlope)
+		print 'Bot Orientation: {}'.format(state.homePos[bot_id].theta)
+		print '#'*50
 
-from utils.geometry import *
-from utils.config import *
 def execute(param, state, bot_id,pub, dribbler=False):
-    finalSlope = param.TurnToAngleP.finalslope # Yet to be defined
-    ballPos = Vector2D(int(state.ballPos.x),int(state.ballPos.y))
-    botPos = Vector2D(int(state.homePos[bot_id].x),int(state.homePos[bot_id].y))
-    obj=Vector2D()
-
-    turnAngleLeft = obj.normalizeAngle(finalSlope - state.homePos[bot_id].theta); # Angle left to turn
-
-    omega = 3*turnAngleLeft * MAX_BOT_OMEGA/(2*math.pi); # Speedup turn 
-    if(omega < 1.43*MIN_BOT_OMEGA and omega > -1.43*MIN_BOT_OMEGA): # This is a rare used skill so believe in Accuracy more than speed. Hence reducing minimum Omega
-      if(omega < 0): omega = -1.43*MIN_BOT_OMEGA
-      else: omega = 1.43*MIN_BOT_OMEGA
+		debug(param,state,bot_id)
+		finalSlope = param.TurnToAngleP.finalSlope # Yet to be defined
+		ballPos = Vector2D(int(state.ballPos.x),int(state.ballPos.y))
+		botPos = Vector2D(int(state.homePos[bot_id].x),int(state.homePos[bot_id].y))
+		obj=Vector2D()
+
+		turnAngleLeft = obj.normalizeAngle(finalSlope - state.homePos[bot_id].theta); # Angle left to turn
+
+		omega = 3*turnAngleLeft * MAX_BOT_OMEGA/(2*math.pi); # Speedup turn 
+		if(omega < 1.43*MIN_BOT_OMEGA and omega > -1.43*MIN_BOT_OMEGA): # This is a rare used skill so believe in Accuracy more than speed. Hence reducing minimum Omega
+			if(omega < 0): omega = -1.43*MIN_BOT_OMEGA
+			else: omega = 1.43*MIN_BOT_OMEGA
 
-    dist = ballPos.dist(botPos)
-    if(dist < DRIBBLER_BALL_THRESH):
-		  skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0,True)
-    else:
-      skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0,False)
+		dist = ballPos.dist(botPos)
+		if(dist < DRIBBLER_BALL_THRESH):
+			skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0,True)
+		else:
+			skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0,False)

skills/skills_union.py

@@ -43,7 +43,7 @@ class GoalKeepingP(Structure):
 class TurnToAngleP(Structure):
 	_fields_ = [("x", c_float), 
 	("y", c_float), 
-	("finalslope", c_float), 
+	("finalSlope", c_float), 
 	("radius", c_float)]
 
 class TurnToPointP(Structure):