diff --git a/script/Graph_fixed.graph b/script/Graph_fixed.graph
index a9d7a61..061b65a 100644
--- a/script/Graph_fixed.graph
+++ b/script/Graph_fixed.graph
@@ -1,5 +1,5 @@
 0	-1	-1	-1	-1
 1	0	10.0	-1	-1
-2	0	10.0	1	12.0
-3	0	10.0	2	12.0
-4	0	10.0	1	12.0
\ No newline at end of file
+2	0	10.0	1	14.0
+3	0	10.0	2	14.0
+4	0	10.0	1	14.0
\ No newline at end of file
diff --git a/script/graphform.bzz b/script/graphform.bzz
index eded5bc..aa8ebb2 100644
--- a/script/graphform.bzz
+++ b/script/graphform.bzz
@@ -12,9 +12,9 @@ include "uavstates.bzz"
 #parameters for set_wheels
 m_sWheelTurningParams={.MaxSpeed=1.0}
 
-ROBOT_RADIUS=1.0
+ROBOT_RADIUS=0.5
 ROBOT_DIAMETER=2.0*ROBOT_RADIUS
-ROBOT_SAFETYDIST=3.0*ROBOT_DIAMETER
+ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER
 
 #
 # Global variables
@@ -774,7 +774,7 @@ function init() {
 	#
 	m_unResponseTimeThreshold=10
 	m_unLabelSearchWaitTime=10
-	m_fTargetDistanceTolerance=15
+	m_fTargetDistanceTolerance=1.5
 	m_unJoiningLostPeriod=100
 
 	#
@@ -782,8 +782,9 @@ function init() {
 	#
 	s = swarm.create(1)
 	s.join()
+	#ROBOT_NUM=5
 	Reset();
-	statef=idle
+	statef=turnedoff
 }
 
 #
diff --git a/script/include/uavstates.bzz b/script/include/uavstates.bzz
index a899199..dfb210c 100644
--- a/script/include/uavstates.bzz
+++ b/script/include/uavstates.bzz
@@ -6,6 +6,11 @@
 TARGET_ALTITUDE = 5.0
 UAVSTATE = "TURNEDOFF"
 
+function turnedoff() {
+    statef=turnedoff
+    UAVSTATE = "TURNEDOFF"
+}
+
 function idle() {
     statef=idle
     UAVSTATE = "IDLE"
diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index f7ad08a..1792330 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -17,9 +17,9 @@ namespace buzz_utility{
 	static char*        BO_FNAME        = 0;
 	static uint8_t*     BO_BUF          = 0;
 	static buzzdebug_t  DBG_INFO        = 0;
-	static uint8_t      MSG_SIZE        = 250;   // Only 100 bytes of Buzz messages every step
-	static int          MAX_MSG_SIZE    = 10000; // Maximum Msg size for sending update packets 
-	static int 	    Robot_id        = 0;
+	static uint32_t     MSG_SIZE        = 600;//250;   // Only 100 bytes of Buzz messages every step
+	static uint32_t     MAX_MSG_SIZE    = 10000; // Maximum Msg size for sending update packets 
+	static uint8_t 	    Robot_id        = 0;
 	static std::vector<uint8_t*> IN_MSG;
 	std::map< int,  Pos_struct> users_map;
 	
@@ -214,10 +214,10 @@ namespace buzz_utility{
 		/* Process messages VM call*/
 		buzzvm_process_inmsgs(VM);
 	}
+
 	/***************************************************/
 	/*Obtains messages from buzz out message Queue*/
 	/***************************************************/
-
    	uint64_t* obt_out_msg(){
 		/* Process out messages */
 		buzzvm_process_outmsgs(VM); 
@@ -234,12 +234,11 @@ namespace buzz_utility{
 	      			if(buzzoutmsg_queue_isempty(VM)) break;
 	      			/* Get first message */
 	      			buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
-	      			/* Make sure the next message makes the data buffer with buzz messages to be less than 100 Bytes */
-	      			if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)
-					>
-			 		MSG_SIZE) {
-			 		buzzmsg_payload_destroy(&m);
-			 		break;
+	      			/* Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes */
+					//ROS_INFO("read size : %i", (int)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)));
+	      			if((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MSG_SIZE) {
+			 			buzzmsg_payload_destroy(&m);
+			 			break;
 	      			}
 
       			/* Add message length to data buffer */
@@ -248,7 +247,7 @@ namespace buzz_utility{
 
       			/* Add payload to data buffer */
       			memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m));
-			tot += buzzmsg_payload_size(m);
+				tot += buzzmsg_payload_size(m);
 
       			/* Get rid of message */
       			buzzoutmsg_queue_next(VM);