Diff: XBee_Robot.cpp

Revision:

12:a7ec1238373e

Parent:

11:6b699617fc7f

--- a/XBee_Robot.cpp	Tue Feb 02 11:02:48 2016 +0000
+++ b/XBee_Robot.cpp	Fri Mar 04 18:46:05 2016 +0000
@@ -57,13 +57,10 @@
 
 //XBEE ROBOT CLASS METHODS
 XBee_Robot::XBee_Robot(PinName _txIn, PinName _rxIn): dataLink(_txIn,_rxIn){
+    dataLink.attach(this,&XBee_Robot::Rx_interrupt, Serial::RxIrq); //set interrupt function on receive pin
     ATQuery(0x4D,0x59); //create AT query with AT command 'MY' to query own 16 bit network address
     commandFlag = 0; //set command flag to 0 on initialisation
-}
-
-void XBee_Robot::setRxInterrupt()
-{
-    dataLink.attach(this,&XBee_Robot::Rx_interrupt, Serial::RxIrq);
+    finishedFlag = 0; //set finished flag to 0 on intialisation
 }
 
 void XBee_Robot::Rx_interrupt()
@@ -73,10 +70,12 @@
         Rx_buffer.push_back(dataLink.getc());//add each incoming byte to buffer
         wait(0.00107); //wait for long enough so the next digit is recognised in the same stream (updated from 0.0011 to accomodate for 2 bytes of data)
     }
-
+    
     //Check valid packet delimeter and checksum
     if((Rx_buffer[0] == 0x7E) && (Rx_buffer[Rx_buffer.size()] == calculateChecksum(Rx_buffer)))
         RxPacketControl(Rx_buffer); //call packet control function
+    else
+        printf("Packet failed delimeter and checksum check");
 }
 
 void XBee_Robot::transmitRequest(uint8_t *BitAddress64, uint8_t *BitAddress16, uint8_t broadcastRadius, uint8_t options, uint8_t *data,size_t dataLength)
@@ -231,8 +230,8 @@
             break;
         }
         case 0x01:{ //Receive location command
-            node_list[currentIndex].setCoordinates((int)packet[1],(int)packet[2],(int)packet[3]); //update coordinates for corresponding node
-            printf("X = %d, Y = %d, Heading = %d\n",node_list[currentIndex].getX(),node_list[currentIndex].getY(),node_list[currentIndex].getHeading()); //display data from packet
+            node_list[currentIndex].setCoordinates(convSigned((int)packet[1]),convSigned((int)packet[2]),convSigned((int)packet[3])); //update coordinates for corresponding node
+            printf("Receive Location Command, X = %d, Y = %d, Heading = %d\n",node_list[currentIndex].getX(),node_list[currentIndex].getY(),node_list[currentIndex].getHeading()); //display data from packet
             break;
         }
         case 0x02: { //Receive destination coordinates from coordinator command
@@ -243,6 +242,19 @@
             commandFlag = 1; //set command flag to indicate received coordinates
             break;
         }
+        case 0xFA: { //Receive router robot finished command
+            printf("Finished flag set\n");
+            finishedFlag = 1; //set command flag to indicate received coordinates
+            break;
+        }
+        case 0xFC: { //Receive obstacle coordinate command
+            printf("Obstacle coordinate received at %d, %d\n",convSigned((int)packet[1]),convSigned((int)packet[2]));
+            std::vector <int> coordinate;
+            coordinate.push_back(convSigned((int)packet[1]));
+            coordinate.push_back(convSigned((int)packet[2]));
+            obstacles.push_back(coordinate);
+            break;
+        }
         default:
             printf("Received data command not recognised: %c",command);
     }
@@ -260,6 +272,30 @@
     return commandFlag; //return value of command flag
 }
 
+int XBee_Robot::getFinishedFlag(){
+    return finishedFlag; //return value of finished flag
+}
+
+void XBee_Robot::resetFinishedFlag(){
+    finishedFlag = 0; //reset finished flag
+}
+
 void XBee_Robot::resetCommandFlag(){
     commandFlag = 0; //reset command flag
+}
+
+int XBee_Robot::checkObstacle(int x_in, int y_in){
+  for (int i = 0; i<obstacles.size();i++){ //search each entry in node_list for matching node
+        if((obstacles[i][0] == x_in)&&(obstacles[i][1] == y_in)){
+            return 1; //return 1 if obstacle exists at given coordinates
+            printf("Obstacle found at %d,%d\n",obstacles[i][0],obstacles[i][1]); //print node number
+        }
+    }
+    return 0; //return 0 if no obstacle exists at given coordinates
+}
+
+int XBee_Robot::convSigned(int int_in){
+    if(int_in >= 128)
+        return int_in-256; //if value is greater or equal to 128 convert to negative number
+    return int_in;
 }
\ No newline at end of file