From db33ffd13c85b2d10c9af4eda918b9220e15dc60 Mon Sep 17 00:00:00 2001
From: Saurabh Agarwal <shourabh.agarwal@gmail.com>
Date: Sat, 12 May 2018 03:32:40 +0530
Subject: [PATCH] Changes in velocity package and sending velocity

Interchange sending velocity command in gr_Command
---
 skills/skill_node.py | 4 ++--
 velocity/profiler.py | 4 ++--
 velocity/run.py      | 8 +++++---
 3 files changed, 9 insertions(+), 7 deletions(-)

diff --git a/skills/skill_node.py b/skills/skill_node.py
index 2cf86e2..9605790 100644
--- a/skills/skill_node.py
+++ b/skills/skill_node.py
@@ -15,8 +15,8 @@ def send_command(pub, team, bot_id, v_x, v_y, v_w, kick_power, dribble, chip_pow
 	# Set the command to each bot
 	gr_command.id          = bot_id
 	gr_command.wheelsspeed = 0
-	gr_command.veltangent  = -v_y/1000.0
-	gr_command.velnormal   = +v_x/1000.0
+	gr_command.veltangent  = v_x/1000.0
+	gr_command.velnormal   = v_y/1000.0
 	gr_command.velangular  = v_w
 	gr_command.kickspeedx  = kick_power
 	gr_command.kickspeedz  = chip_power
diff --git a/velocity/profiler.py b/velocity/profiler.py
index d230233..bae39d0 100644
--- a/velocity/profiler.py
+++ b/velocity/profiler.py
@@ -102,9 +102,9 @@ def GetExpectedPositionIndex(self):
 	##
 	## @brief      Total length of path
 	##
-	def GetPathLength(self):
+	def GetPathLength(self, startIndex=1):
 		length = 0
-		for i in xrange(1,len(self.path)):
+		for i in xrange(startIndex,len(self.path)):
 			length += self.path[i].dist(self.path[i-1])
 		return length
 
diff --git a/velocity/run.py b/velocity/run.py
index 0939135..6096e8c 100644
--- a/velocity/run.py
+++ b/velocity/run.py
@@ -54,9 +54,10 @@ def Get_Vel(start, t, kub_id, target, homePos_, awayPos_,avoid_ball=False):
         FIRST_CALL = 0
 
     if distance < 1.5*BOT_BALL_THRESH:
-        return [0,0,0,0]
+        return [0,0,0,0,0]
     # print("ex = ",expectedTraverseTime) 
     # print("t = ",t," start = ",start)
+    remainingDistance = 0
     if (t - start< expectedTraverseTime):
         if v.trapezoid(t - start,curPos):
             index = v.GetExpectedPositionIndex()
@@ -65,6 +66,7 @@ def Get_Vel(start, t, kub_id, target, homePos_, awayPos_,avoid_ball=False):
                 vX,vY = 0,0
 
             else:
+                remainingDistance = v.GetPathLength(startIndex=index)
                 vX,vY,eX,eY = v.sendVelocity(v.getVelocity(),v.motionAngle[index],index)
 
         else:
@@ -109,7 +111,7 @@ def Get_Vel(start, t, kub_id, target, homePos_, awayPos_,avoid_ball=False):
             startPt.x = homePos[kubid].x
             startPt.y = homePos[kubid].y
             findPath(startPt,target, avoid_ball)
-            return [0,0,0, REPLAN]  
+            return [0,0,0, REPLAN,0]  
     else:
         errorInfo.errorX = eX
         errorInfo.errorY = eY
@@ -117,7 +119,7 @@ def Get_Vel(start, t, kub_id, target, homePos_, awayPos_,avoid_ball=False):
         botAngle = homePos[kubid].theta
         vXBot = vX*cos(botAngle) + vY*sin(botAngle)
         vYBot = -vX*sin(botAngle) + vY*cos(botAngle)
-        return [vXBot, vYBot, 0, REPLAN]            
+        return [vXBot, vYBot, 0, REPLAN,remainingDistance]            
     # print("end getVelocity")    
         
 def shouldReplan():