diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz
index 02d1fdc..fa6cffc 100644
--- a/buzz_scripts/include/act/states.bzz
+++ b/buzz_scripts/include/act/states.bzz
@@ -10,12 +10,12 @@ include "utils/conversions.bzz"
 TARGET_ALTITUDE = 15.0 # m.
 BVMSTATE = "TURNEDOFF"
 PICTURE_WAIT = 20 # steps
-GOTO_MAXVEL = 1.5 # m/steps
+GOTO_MAXVEL = 2.5 # m/steps
 GOTO_MAXDIST = 150 # m.
 GOTODIST_TOL = 0.5  # m.
 GOTOANG_TOL = 0.1 # rad.
 path_it = 0
-rc_State = 0
+graphid = 0
 pic_time = 0
 g_it = 0
 
@@ -163,8 +163,8 @@ function formation() {
   var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
   if(neighbors.count() > 0)
     math.vec2.scale(accum, 1.0 / neighbors.count())
-  if(math.vec2.length(accum)>GOTO_MAXVEL*15)
-  	accum = math.vec2.scale(accum, 15*GOTO_MAXVEL/math.vec2.length(accum))
+  if(math.vec2.length(accum)>GOTO_MAXVEL*10)
+  	accum = math.vec2.scale(accum, 10*GOTO_MAXVEL/math.vec2.length(accum))
   goto_abs(accum.x, accum.y, 0.0, 0.0)
 }
 
@@ -195,19 +195,22 @@ function rc_cmd_listen() {
 		stattab_send()
 	} else if (flight.rc_cmd==900){
 		flight.rc_cmd=0
-		rc_State = 0
+		BVMSTATE = "TASK_ALLOCATE"
 		neighbors.broadcast("cmd", 900)
 	} else if (flight.rc_cmd==901){
 		flight.rc_cmd=0
-		rc_State = 1
+		destroyGraph()
+		BVMSTATE = "PURSUIT"
 		neighbors.broadcast("cmd", 901)
 	} else if (flight.rc_cmd==902){
 		flight.rc_cmd=0
-		rc_State = 2
+		destroyGraph()
+		BVMSTATE = "AGGREGATE"
 		neighbors.broadcast("cmd", 902)
 	} else if (flight.rc_cmd==903){
 		flight.rc_cmd=0
-		rc_State = 3
+		destroyGraph()
+		BVMSTATE = "FORMATION"
 		neighbors.broadcast("cmd", 903)
 	}
 }
@@ -224,14 +227,17 @@ function nei_cmd_listen() {
 					uav_arm()
 				} else if(value==401 and BVMSTATE=="TURNEDOFF"){
 					uav_disarm()
-				} else if(value==900){
-					rc_State = 0
-				} else if(value==901){
-					rc_State = 1
-				} else if(value==902){
-					rc_State = 2
-				} else if(value==903){
-					rc_State = 3
+				} else if(value==900){	# Shapes
+					BVMSTATE = "TASK_ALLOCATE"
+				} else if(value==901){	# Pursuit
+					destroyGraph()
+					BVMSTATE = "PURSUIT"
+				} else if(value==902){	# Agreggate
+					destroyGraph()
+					BVMSTATE = "AGGREGATE"
+				} else if(value==903){	# Formation
+					destroyGraph()
+					BVMSTATE = "FORMATION"
 				} else if(value==16 and BVMSTATE=="IDLE"){
 				#  neighbors.listen("gt",function(vid, value, rid) {
 				#    print("Got (", vid, ",", value, ") from robot #", rid)
diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz
index 0252971..826ee01 100644
--- a/buzz_scripts/include/taskallocate/graphformGPS.bzz
+++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz
@@ -11,7 +11,6 @@ ROBOT_RADIUS = 50
 ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
 ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER
 ROOT_ID = 2
-old_state = -1
 
 #
 # Global variables
@@ -401,7 +400,7 @@ function TransitionToJoined(){
 
 	m_navigation.x=0.0
 	m_navigation.y=0.0
-	goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
+	goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
 }
 #
 #Transistion to state Lock, lock the current formation
@@ -422,10 +421,8 @@ function TransitionToLock(){
   }
   m_navigation.x=0.0
   m_navigation.y=0.0
-  goto_abs(m_navigation.x, m_navigation.y, 0.0)
+  goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
 
-  # prepare to restart a new shape
-  old_state = rc_State
 	#stop listening 
 	neighbors.ignore("m")
 }
@@ -536,7 +533,7 @@ function DoAsking(){
 
 	m_navigation.x=0.0
 	m_navigation.y=0.0
-	goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
+	goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
     BroadcastGraph()
 }
 #
@@ -666,20 +663,14 @@ function DoJoined(){
 	   }
 	}
 
-	#check if should to transists to lock
-	#write statues
-  #v_tag.get(m_nLabel)
-	#log(v_tag.size(), " of ", ROBOTS, " ready to lock")
-	#if(v_tag.size()==ROBOTS){
-	#	TransitionToLock()
-	#}
   barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED")
   BroadcastGraph()
 }
 #
 #Do Lock
 #
-function DoLock(){
+timeout_graph = 40
+function DoLock() {
   UpdateGraph()
   m_selfMessage.State=s2i(BVMSTATE)
   m_selfMessage.Label=m_nLabel
@@ -693,19 +684,23 @@ function DoLock(){
   if(m_nLabel!=0){
       m_navigation=motion_vector()
   }
-  #move
+#  #move
   goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
   BroadcastGraph()
-}
-#
-# Executed after takeoff
-#
-function startGraph(){
-  BVMSTATE="GRAPH_FREE"
 
-  # reset the graph
-  statef=resetGraph
+  if(loop) {
+	  if(timeout_graph==0) {
+		if(graphid < 3)
+			graphid = graphid + 1
+		else
+			graphid = 0
+		timeout_graph = 40
+		BVMSTATE="TASK_ALLOCATE"
+	  }
+  	timeout_graph = timeout_graph - 1
+  }
 }
+
 #
 # Executed at init
 #
@@ -750,6 +745,7 @@ function BroadcastGraph() {
 # Executed when reset
 #
 function resetGraph(){
+	BVMSTATE="GRAPH_FREE"
 	m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
 	m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
 	m_navigation={.x=0,.y=0}
@@ -766,16 +762,16 @@ function resetGraph(){
 	assign_id=-1
 	m_gotjoinedparent = 0
 
-	if(rc_State==0){
+	if(graphid==0){
 		log("Loading P graph")
 		Read_GraphP()
-	} else if(rc_State==1) {
+	} else if(graphid==1) {
 		log("Loading O graph")
 		Read_GraphO()
-	} else if(rc_State==2) {
+	} else if(graphid==2) {
 		log("Loading L graph")
 		Read_GraphL()
-	} else if(rc_State==3) {
+	} else if(graphid==3) {
 		log("Loading Y graph")
 		Read_GraphY()
 	}
@@ -802,7 +798,7 @@ function destroyGraph() {
 	#clear neighbour message
 	m_navigation.x=0.0
 	m_navigation.y=0.0
-	goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
+	goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
 	m_vecNodes={}
 	#stop listening 
 	neighbors.ignore("m")
diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz
index d898bb3..7191155 100644
--- a/buzz_scripts/main.bzz
+++ b/buzz_scripts/main.bzz
@@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz"
 include "vstigenv.bzz"
 
 #State launched after takeoff
-AUTO_LAUNCH_STATE = "PURSUIT"
+AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
 
 #####
 # Vehicule type:
@@ -27,6 +27,7 @@ function init() {
 	init_swarm()
 
 	TARGET_ALTITUDE = 10 + id*2.0 # m
+	loop = 1
 
 	# start the swarm command listener
 	nei_cmd_listen()
@@ -60,21 +61,19 @@ function step() {
 		statef=formation
 	else if(BVMSTATE=="PURSUIT")
 		statef=pursuit
-	else if(BVMSTATE=="TASK_ALLOCATE") {	# or bidding ?
-		startGraph()
-	} else if(BVMSTATE=="GRAPH_FREE") {
+	else if(BVMSTATE=="TASK_ALLOCATE")	# or bidding ?
+		statef=resetGraph
+	else if(BVMSTATE=="GRAPH_FREE")
 		statef=DoFree
-	} else if(BVMSTATE=="GRAPH_ASKING") {
+	else if(BVMSTATE=="GRAPH_ASKING")
 		statef=DoAsking
-	} else if(BVMSTATE=="GRAPH_JOINING") {
+	else if(BVMSTATE=="GRAPH_JOINING")
 		statef=DoJoining
-	} else if(BVMSTATE=="GRAPH_JOINED") {
+	else if(BVMSTATE=="GRAPH_JOINED")
 		statef=DoJoined
-	} else if(BVMSTATE=="GRAPH_TRANSTOLOCK")
+	else if(BVMSTATE=="GRAPH_TRANSTOLOCK")
 		statef=TransitionToLock
-	else if(BVMSTATE=="GRAPH_LOCK" and old_state!=rc_State)	#switch to a new graph
-		startGraph()
-	else if(BVMSTATE=="GRAPH_LOCK" and old_state==rc_State)	# move all together (TODO: not tested)
+	else if(BVMSTATE=="GRAPH_LOCK")	# move all together (TODO: not tested), but auto-loop in graph list
 		statef=DoLock
 	else if(BVMSTATE=="PATHPLAN")	# ends on navigate, defined in rrtstar
 		statef=rrtstar