Hi all
I am trying to run a bunch of autonomous robots together. Each robot takes in the information of the preceding robot and follows it. I have 3 arduinos mega 2560 and 3 SF xbee shield and series 1 xbee. So I put the arduino, motor controller and xbee stack on each robot and try to coordinate them. Here is where the problem is
My program runs something like this..... the robots are named 1,2,3 and this is how im running them.
The comm loop runs every 75ms and communication is initiated by the 1st robot. Robot1 --> Robot2 --> Robot3 --> Robot1 --> Robot2 --> Robot3 and so on. Robot 2 transmits only after it receives info from robot1.
The problem is for reasons unknown to me the communication just stops I mean, after robot 1 transmits, robot 2 does not receive and so it does not transmit stopping the comm loop. I dont know why this is happening. Sometimes when I run it, the loop works for a few seconds and then stops. I tried doing this with only 2 robots and it works for about 30-40 secs and it stops after that.
I have attached my program outline. Any help into figuring out this problem will be greatly appreciated.
#include "Arduino.h"
#include "QuadratureEncoder.h"
#include "TimerOne.h"
//Declare encoder interrupts and initialize pwm
//Define gains and other constants.
//Initialize Velocity and Time
//For Communication routine
float vData[4];
char inData[50];
byte index;
bool started=false;
bool ended=false;
void setup()
{
Serial2.begin(9600);
//Other startup stuff
// Initialize timer interrupt for control routine.
vData[0]=0;
vData[1]=0;
vData[2]=0;
senddata();
} //setup
void loop()
{
// motor Velocity controller
vel_ctrl();
//Run communication loop every 75ms
if ((millis()-t) > 75){
comm();
t=millis();
}
delay(5);
} //loop
The comm() function where the communication received is parsed and information is transmitted is as follows.
void comm()
{
char *Data=NULL;
int count=0;
//Read the Serial2 data from the buffer
while(Serial2.available()>0)
{
char inChar = Serial2.read();
if(inChar == '<')
{
index=0;
inData[index]='\0';
started=true;
ended=false;
}
else if(inChar == '>')
{
ended=true;
break;
}
else
{
if(index < 49)
{
inData[index]=inChar;
index++;
inData[index]='\0';
}
}
} //while(Serial2.available()>0)
//Process data received
if(started && ended)
{
Data =strtok(inData,",");
//Convert received data into floats and assign them to variables
if(atof(Data)==3){
while(Data != NULL){
vData[count]=atof(Data);
Data=strtok(NULL,",");
count++;
}
//Transmit data if information received is from the robot 3
senddata();
}
vData[0]=0;
//Reset booleans
started = false;
ended= false;
index=0;
inData[index]='\0';
} //if(started && ended)
} //comm
When senddata() is called the information is transmitted.
void senddata()
{
Serial2.print('<');
Serial2.print(1);Serial2.print(',');
Serial2.print(PosX);Serial2.print(',');
Serial2.print(PosY);Serial2.print(',');
Serial2.print(Veld);
Serial2.print('>');
Serial2.print(millis());Serial2.print("\n");
} //senddata