moved the lca code to the right file

buzz_scripts/include/act/CA.bzz

@@ -1,10 +1,10 @@
-function LCA(vel_vec) {
+# Lightweight collision avoidance
+function LCA( vel_vec ) {
 	var safety_radius = 3.0
-	var default_velocity = 2.0
 	collide = 0
 
-	var k_v = 5 # velocity gain
+	var k_v = 5   # x axis gain
-	var k_w = 10   # angular velocity gain
+	var k_w = 5   # y axis gain
 
 	cart = neighbors.map(
 		function(rid, data) {
@@ -28,18 +28,24 @@ function LCA(vel_vec) {
 				return accum
 			}, {.distance= safety_radius * 2.0, .angle= 0.0})
 
+
 		d_i = result.distance
 		data_alpha_i = result.angle
 
-		penalty = math.exp(d_i - safety_radius)
+		delta = math.vec2.new( d_i * math.cos(data_alpha_i), d_i * math.sin(data_alpha_i) )
-		if( math.cos(math.vec2.angle(vel_vec)) < 0.0){
+
-			penalty = math.exp(-(d_i - safety_radius))
+		p = math.exp(-(d_i - safety_radius))
+		if ( math.cos( math.vec2.angle(vel_vec) - data_alpha_i ) < 0.0 ) {
+			p = math.exp(d_i - safety_radius)
 		}
 
-		V = math.vec2.length(vel_vec) - math.cos(math.vec2.angle(vel_vec)) * penalty * k_v
+		V = -1 * (p / d_i) * k_v * delta.x
-		W = -k_w * penalty * math.sin( math.vec2.angle(vel_vec) )
+		W = -1 * (p / d_i) * k_w * delta.y
+
+		Uavd = math.vec2.new( V, W )
+
+		return math.vec2.add( vel_vec, Uavd )
 
-		return math.vec2.newp(V, W)
 	}	else
 		return vel_vec
 }

buzz_scripts/include/act/states.bzz

@@ -210,58 +210,6 @@ function formation() {
   goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0)
 }
 
-# Lightweight collision avoidance
-function LCA_rafael( vel_vec ) {
-	var safety_radius = 3.0
-	collide = 0
-
-	var k_v = 5   # x axis gain
-	var k_w = 5   # y axis gain
-
-	cart = neighbors.map(
-		function(rid, data) {
-			var c = {}
-			c.distance = data.distance 
-			c.azimuth = data.azimuth
-			if (c.distance < (safety_radius * 2.0) )
-				collide = 1
-			return c
-	})
-	if (collide) {
-		log("")
-		log("------>  AVOIDING NEIGHBOR!  <------")
-		log("")
-		result = cart.reduce(function(rid, data, accum) {
-			if(data.distance < accum.distance and data.distance > 0.0){
-				accum.distance = data.distance
-				accum.angle = data.azimuth
-				return accum
-			}
-				return accum
-			}, {.distance= safety_radius * 2.0, .angle= 0.0})
-
-
-		d_i = result.distance
-		data_alpha_i = result.angle
-
-		delta = math.vec2.new( d_i * math.cos(data_alpha_i), d_i * math.sin(data_alpha_i) )
-
-		p = math.exp(-(d_i - safety_radius))
-		if ( math.cos( math.vec2.angle(vel_vec) - data_alpha_i ) < 0.0 ) {
-			p = math.exp(d_i - safety_radius)
-		}
-
-		V = -1 * (p / d_i) * k_v * delta.x
-		W = -1 * (p / d_i) * k_w * delta.y
-
-		Uavd = math.vec2.new( V, W )
-
-		return math.vec2.add( vel_vec, Uavd )
-
-	}	else
-		return vel_vec
-}
-
 # Custom state function for the developer to play with
 function cusfun(){
 	BVMSTATE="CUSFUN"
@@ -313,7 +261,7 @@ function cusfun(){
 
 	# Run LCA and increment waypoint
 	if(distance_to_target > 0.2){
-		vector_to_target = LCA_rafael(vector_to_target)
+		vector_to_target = LCA(vector_to_target)
 		vector_to_target = LimitSpeed(vector_to_target, 0.5)
 		goto_abs(vector_to_target.x, vector_to_target.y, 0.0, 0.0)
 	} else {