From 5f64151e7e9ae6c27f47b1e1ff048faf1450546d Mon Sep 17 00:00:00 2001
From: hurricaneJoef <33767776+hurricaneJoef@users.noreply.github.com>
Date: Fri, 19 Jul 2019 09:39:24 -0400
Subject: [PATCH] added pot field

---
 scripts/driveNode.py | 23 ++++++++++++++++++++++-
 1 file changed, 22 insertions(+), 1 deletion(-)

diff --git a/scripts/driveNode.py b/scripts/driveNode.py
index 7fe4842..bc91265 100755
--- a/scripts/driveNode.py
+++ b/scripts/driveNode.py
@@ -129,7 +129,28 @@ def drive_to_cone(self):
             self.cmd.drive.speed = 1
         else:
             self.cmd.drive.speed = 0
-
+	def fieldpot(self):
+        points=[]
+       	for item in self.data.ranges:
+       		#inverse squer law
+       	    x,y=self.pol2cart((float(item)),(self.data.angle_min) + (self.data.ranges.index(item) * self.data.angle_increment))
+            #type(points)-
+       	    points.append([x,y])
+            
+        	#print("points array is {0}".format(isdr))
+		points.append([0,5])
+       	total=np.sum(points,axis=0)
+       	#print(total)
+       	#total[1]+=self.behindforce*(len(self.data.ranges)/3)*1/self.DESIRED_DISTANCE**2
+       	tvel, trad =self.cart2pol(total[0],total[1])
+       	print("total array is {0}".format(total))#. total sum is (r,theta){1},{2}".format(total,tvel,trad))
+        self.VELOCITY=tvel*self.kv
+       	angle=self.ktheta*trad
+       	#print(tvel)
+       	#self.drive(1,angle)
+		self.cmd.drive.speed = 1
+        self.cmd.drive.steering_angle = angle
+		
     def find_sign(self):
         color_box, sign_box = sift_det("images/oneway.jpg",self.camera_data.cv_image)
         rospy.loginfo("cb: {} sb: {}".format(color_box,sign_box))