From 271e4e5004ebdd573cca57099ddac54505bac328 Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Wed, 22 Feb 2017 17:14:27 -0500
Subject: [PATCH] test

---
 src/testflockfev.bzz | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz
index 92d6f42..7454517 100644
--- a/src/testflockfev.bzz
+++ b/src/testflockfev.bzz
@@ -13,6 +13,7 @@ neighbors.broadcast(updated, update_no)
 }
 
 TARGET_ALTITUDE = 5.0
+CURSTATE = TURNEDOFF
  
 # Lennard-Jones parameters
 TARGET     = 10.0	#0.000001001
@@ -36,6 +37,7 @@ function lj_sum(rid, data, accum) {
 # Calculates and actuates the flocking interaction
 function hexagon() {
   statef=hexagon
+  CURSTATE=HEXAGON
   # Calculate accumulator
   var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
   if(neighbors.count() > 0)
@@ -89,6 +91,7 @@ function barrier_wait(threshold, transf) {
 
 function idle() {
 statef=idle
+CURSTATE=IDLE
 neighbors.listen("cmd",
    function(vid, value, rid) {
       print("Got (", vid, ",", value, ") from robot #", rid)
@@ -129,6 +132,7 @@ function land() {
 # Executed once at init time.
 function init() {
 	statef=idle
+	CURSTATE=IDLE
 }
 
 # Executed at each time step.
@@ -137,19 +141,22 @@ function step() {
 	if(flight.rc_cmd==22) {
 		log("cmd 22")
 		flight.rc_cmd=0
-		statef = takeoff		
+		statef = takeoff
+		CURSTATE = TAKEOFF		
 		neighbors.broadcast("cmd", 22)
 	} else if(flight.rc_cmd==21) {
 		log("cmd 21")
 		log("To land")
 		flight.rc_cmd=0
 		statef = land
+		CURSTATE = LAND
 		neighbors.broadcast("cmd", 21)
 	} else if(flight.rc_cmd==16) {
 		flight.rc_cmd=0
 		uav_goto()
 	} 
 	statef()
+	log("Current state: ", CURSTATE)
 }
  
 # Executed once when the robot (or the simulator) is reset.