diff --git a/buzz_scripts/include/act/CA.bzz b/buzz_scripts/include/act/CA.bzz
index 6d923f0..ed0d055 100644
--- a/buzz_scripts/include/act/CA.bzz
+++ b/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_w = 10   # angular velocity gain
+	var k_v = 5   # x axis 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)
-		if( math.cos(math.vec2.angle(vel_vec)) < 0.0){
-			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) )
+
+		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
-		W = -k_w * penalty * math.sin( math.vec2.angle(vel_vec) )
+		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 )
 
-		return math.vec2.newp(V, W)
 	}	else
 		return vel_vec
 }
\ No newline at end of file
diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz
index 7245ce0..81f7a73 100644
--- a/buzz_scripts/include/act/states.bzz
+++ b/buzz_scripts/include/act/states.bzz
@@ -234,14 +234,62 @@ function formation() {
 # Custom state function for the developer to play with
 function cusfun(){
 	BVMSTATE="CUSFUN"
-	log("cusfun: yay!!!")
-	LOCAL_TARGET = math.vec2.new(5 ,0 )
-	local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location
-    if(math.vec2.length(local_set_point) > GOTODIST_TOL){
-		local_set_point = LimitSpeed(local_set_point, 1.0)
-		goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0)
-    }
-    else{
-    	log("TARGET REACHED")
-    }
+	#log("cusfun: yay!!!")
+	#LOCAL_TARGET = math.vec2.new(5 ,0 )
+	#local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location
+    #if(math.vec2.length(local_set_point) > GOTODIST_TOL){
+	#	local_set_point = LimitSpeed(local_set_point, 1.0)
+	#	goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0)
+    #}
+    #else{
+    #	log("TARGET REACHED")
+    #}
+
+	initialPoint = 0
+
+	# Get waypoint list
+	if (id == 1) {
+		waypoints = {
+			.0 = math.vec2.new(-5.0, -5.0),
+			.1 = math.vec2.new(5.0, 5.0)
+		}
+	} else if (id == 2 ) {
+		waypoints = {
+			.0 = math.vec2.new(5.0, 5.0),
+			.1 = math.vec2.new(-5.0, -5.0)
+		}
+	}
+
+	# Current waypoint
+	target_point = waypoints[point]
+
+	# Get drone position and transform to global frame - depends on lauch file
+	offset_x = 0.0
+	offset_y = 0.0
+	if (id == 1) {
+		offset_x = 5.0
+		offset_y = 5.0
+	} else if (id == 2 ) {
+		offset_x = -5.0
+		offset_y = -5.0
+	}
+	pos_x = pose.position.x + offset_x;
+	pos_y = pose.position.y + offset_y;
+
+	# Get a vector pointing to target and target distance
+	vector_to_target = math.vec2.sub(target_point, math.vec2.new(pos_x, pos_y))
+	distance_to_target = math.vec2.length(vector_to_target)
+
+	# Run LCA and increment waypoint
+	if(distance_to_target > 0.2){
+		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 {
+		if(point < size(waypoints) - 1){
+			point = point + 1
+		} else {
+			point = 0
+		}
+	}
 }
\ No newline at end of file