Que::Que(const Que& aQue) {
if(aQue.empty())
front = back = NULL;
else {
QueNodePtr temp_ptr_old = aQue.front; //temp_ptr_old moves front to back of aQue
QueNodePtr temp_ptr_new; //temp_ptr_new is used to create new nodes
back = new QueNode;
back->name = temp_ptr_old->name;
back->address = temp_ptr_old->address;
back->link = NULL;
front = back; //First node created and filled with data
//New nodes are entered AFTER this one
temp_ptr_old = temp_ptr_old->link; //tempptrold now points to second node
while(temp_ptr_old != NULL) {
temp_ptr_new = new QueNode;
temp_ptr_new->name = temp_ptr_old->name;
temp_ptr_new->address = temp_ptr_old->address;
temp_ptr_new->link = NULL;
back->link = temp_ptr_new;
back = temp_ptr_new;
temp_ptr_old = temp_ptr_old->link;
}
}
}
// Task to receive data from RPi
void xbeeTask(void* p) {
Serial.println("IN XBEE TASK");
while (1) {
int queueLen = 0;
int delPos = 0;
while (Serial1.available() > 0) {
unsigned char in = (unsigned char)Serial1.read();
Serial.println(in, HEX);
if (!RxQ.Enqueue(in)) {
break;
}
}
queueLen = RxQ.Size();
for (int i=0;i<queueLen;i++) {
if (RxQ.Peek(i) == 0x7E) {
unsigned char checkBuff[Q_SIZE];
unsigned char msgBuff[Q_SIZE];
int checkLen = 0;
int msgLen = 0;
checkLen = RxQ.Copy(checkBuff, i);
msgLen = xbee.Receive(checkBuff, checkLen, msgBuff);
if (msgLen > 0) {
unsigned char outMsg[MAX_RPI_MSG_SIZE];
unsigned char outFrame[Q_SIZE];
int frameLen = 0;
int packageID = (char)msgBuff[PKG_INDEX] - '0';
int ack_len = 0;
switch(packageID) {
case DEVICE_READY:
memcpy(outMsg, "ACK", 3);
deviceRdy = ack;
ack_len = 3;
break;
case NAVI_READY:
memcpy(outMsg, "ACK", 3);
naviRdy = true;
ack_len = 3;
break;
case NAVI_END:
memcpy(outMsg, "ACK", 3);
naviRdy = false;
deviceRdy = not_ack;
ack_len = 3;
break;
case OBSTACLE_DETECTED:
memcpy(outMsg, "ACK", 3);
// activate servos
if(msgBuff[9] == 'L')
Serial.println("left");
else
Serial.println("right");
// str = msgBuff[10];
//writeToActuator(left,right);
ack_len = 3;
break;
default:
// raise error ?
break;
}
frameLen = xbee.Send(outMsg, ack_len, outFrame, RPI_ADDR);
Serial1.write(outFrame, frameLen);
i += msgLen;
delPos = i;
}
else {
if (i > 0) {
delPos = i-1;
}
}
}
}
RxQ.Clear(delPos);
vTaskDelay(200);
}
}
int main()
{
Que copyQue;
Que myq;
Que testQ;
ItemType it;
bool ok;
PrintLines(3);
PrintMessage ("copy constructor", "empty");
Que bq(testQ);
PrintMessage("GetSize", "size 0");
cout << "size = " << testQ.GetSize() << endl;
PrintMessage ("IsEmpty", "empty");
PrintBool (testQ.IsEmpty());
ToContinue();
PrintMessage ("Enqueue", "empty");
cout << "Enter stuff -> ";
cin >> it;
testQ.Enqueue(it,ok);
PrintMessage("Enqueue", "after Enqueue of 1");
cout << testQ << endl;
PrintBool (ok);
PrintMessage ("IsEmpty", "non-empty");
PrintBool (testQ.IsEmpty());
ToContinue();
PrintMessage ("Enqueue", "size 1");
cout << testQ << endl;
cout << "Enter stuff -> ";
cin >> it;
testQ.Enqueue(it,ok);
PrintMessage("Enqueue", "after Enqueue of 2");
cout << testQ << endl;
PrintBool (ok);
PrintMessage(" GetFront", "size 2");
cout << testQ << endl;
if (!testQ.IsEmpty())
{
testQ.GetFront(it);
cout << "front = " << it << endl;
cout << testQ << endl;
PrintBool (ok);
}
ToContinue();
PrintMessage ("Enqueue", "size 2");
cout << testQ << endl;
cout << "Enter stuff -> ";
cin >> it;
testQ.Enqueue(it,ok);
PrintMessage("Enqueue", "after Enqueue of 3");
cout << testQ << endl;
PrintBool (ok);
ToContinue();
PrintMessage ("operator=", "size 3");
copyQue = testQ;
cout << "Original queue: " << testQ << endl;
cout << " Copied queue: " << copyQue << endl;
PrintMessage ("copy constructor", "size 3");
Que otherq(testQ);
cout << otherq << endl;
ToContinue();
PrintMessage ("Dequeue", "size 3");
cout << testQ << endl;
testQ.Dequeue(ok);
PrintBool (ok);
cout << testQ << endl;
PrintMessage ("Dequeue", "size 2");
testQ.Dequeue(ok);
cout << testQ << endl;
PrintBool (ok);
PrintMessage ("Dequeue", "size 1");
testQ.Dequeue(ok);
cout << testQ << endl;
PrintBool (ok);
ToContinue();
PrintMessage ("Dequeue", "empty");
testQ.Dequeue(ok);
PrintBool (ok);
ToContinue();
PrintMessage(" GetFront", "size 3");
cout << otherq << endl;
if (!otherq.IsEmpty())
{
otherq.GetFront(it);
cout << "front = " << it << endl;
cout << otherq << endl;
PrintBool (ok);
}
ToContinue();
PrintMessage("GetSize", "size 3");
cout << otherq << endl;
cout << "size = " << otherq.GetSize() << endl;
PrintMessage("GetSize", "size 0");
cout << "size = " << testQ.GetSize() << endl;
PrintLines(3);
return 0;
}