Using Bluno beetle for trilateration

Hello Everybody!

I'm doing a project for my university in which I have to use 4 bluno beetles.
3 of them are set as iBeacons and they are fixed on a position, the fourth is attached to a bracelet (so it is mobile) and it gets the RSSI from the other three, in order to perform the trilateration ( this operation is
obviously performed by a server to which the mobile bluno sends the values).
In this way I will get the position of the mobile beetle in the space at each instant.
The problem is to continue to make the switch and constantly get the three RSSI.
Here's the code I've tried (the sketch of IBeacons is empty), but it does not work (it is inspired by others sketch of previous topics of this forum):



typedef enum {START, WAIT_AT, WAIT_BIND, WAIT_RSSI, WAIT_EXIT, NORM_MODE,CLEAN_STATE} rssi_state_type;

rssi_state_type state = START;
char chr;
String buffer="", rssi="", data="";
String MACS[3] = {"0xC4BE84E397E6","0x9059AF16A3C2","0x9059AF169CF3"};
String current_mac ;
int index =0;
bool reading = false;
int count=0;


void setup() {
Serial.begin(115200); //Initiate the Serial comm
while(!Serial){

}
state = START;
}

void loop() {

if (state == START) {
start();
} else if (state == WAIT_AT) {
wait_at();
} else if (state == WAIT_BIND){
wait_bind();
} else if (state == WAIT_RSSI) {
wait_rssi();
} else if (state == WAIT_EXIT) {
wait_exit();
} else if (state == NORM_MODE) {
norm_mode();
}
}

void start() {

count=0;

delay(1000);
Serial.flush();
clean();

//AT COMMANDS
Serial.print("+");
Serial.print("+");
Serial.print("+");
data = "";
state = WAIT_AT;
}

void wait_at() {
while (Serial.available() > 0) {
chr = Serial.read();
//buffer+=chr;
if (chr == 'E') {
reading = true;
buffer = "";
}
if (reading) {
buffer += chr;
} else {
data += chr;
}

}

if ( buffer == "Enter AT Mode\r\n") {
data="";
current_mac = MACS[index];
Serial.println("AT+BIND="+current_mac);
buffer="";
reading = false;
state = WAIT_BIND;
}
}

void wait_bind(){
while (Serial.available() > 0) {
chr = Serial.read();
buffer+=chr;

}

if(buffer == "OK\r\n")
{
buffer="";
Serial.println("AT+RSSI=?");
state= WAIT_RSSI;
}
}

void wait_rssi() {
while (Serial.available() > 0) {
chr = Serial.read();
if (chr == '-') {
reading = true;
buffer = "";
}
if (reading) {
buffer += chr;
}
}

if (buffer.length() == 4) {
rssi = buffer;
Serial.println("AT+RESTART");
reading = false;
state = WAIT_EXIT;
if (rssi == "-000") {
//TO BE PREPARED
}
}
}

void wait_exit() {

buffer = "";
delay(3000);
state = NORM_MODE;
}




void norm_mode() {
Serial.println("\r\n<<<");
Serial.println("RSSI letto da " + current_mac + "E' : " + rssi);
Serial.println(">>>\r\n");
Serial.flush();


if(index < 2)
{
index++;
}
else
{
index=0;
}


state=START;
}

//cleans the transmission buffer
void clean ()
{
while (count < 64)
{
while(Serial.available() > 0)
{
Serial.read();
}

count++;
delay(10);
}

state=START;
}



In particular, i cant't see the RSSI values on the Serial monitor, so I can't do Debugging operations.

I would be grateful if someone could help me.


while I wait for answers, many thanks to all of you.