Diff: XBee_Robot.cpp

Revision:

6:fb0316cafaa6

Parent:

4:af08c7749f9d

Child:

7:c3acafdb70c0

--- a/XBee_Robot.cpp	Tue Jan 05 14:48:39 2016 +0000
+++ b/XBee_Robot.cpp	Wed Jan 06 17:37:09 2016 +0000
@@ -25,8 +25,12 @@
     addr = addrIn;
 }  
 
+/**************************************************************************************************/
+
 //XBEE ROBOT CLASS METHODS
-XBee_Robot::XBee_Robot(PinName _txIn, PinName _rxIn): dataLink(_txIn,_rxIn){}
+XBee_Robot::XBee_Robot(PinName _txIn, PinName _rxIn): dataLink(_txIn,_rxIn){
+    ATQuery(0x4D,0x59); //create AT query with AT command MY to query own 16 bit network address
+}
 
 void XBee_Robot::setRxInterrupt()
 {
@@ -38,7 +42,7 @@
     std::vector<uint8_t> Rx_buffer;
     while(dataLink.readable()){
         Rx_buffer.push_back(dataLink.getc());//add each incoming byte to buffer
-        wait(0.0011); //wait for long enough so the next digit is recognised in the same stream
+        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
@@ -75,6 +79,30 @@
     }
 }
 
+void XBee_Robot::ATQuery(uint8_t ATu, uint8_t ATl)
+{
+    //calculate checksum
+    uint8_t lengthu = 0; //upper 8 bits of length
+    uint8_t lengthl = 0x04; //lower 8 bits of length
+    
+    
+    std::vector<uint8_t> ATRequestPacket; //create new vector packet
+    //populate packet 
+    ATRequestPacket.push_back(0x7E); //start delimeter
+    ATRequestPacket.push_back(lengthu); //upper byte of length
+    ATRequestPacket.push_back(lengthl); //lower byte of length
+    ATRequestPacket.push_back(0x08); //API ID (AT request)
+    ATRequestPacket.push_back(0x52); //channel ID
+    ATRequestPacket.push_back(ATu); //AT command (upper byte)
+    ATRequestPacket.push_back(ATl); //AT command (lower byte)
+    uint8_t checksum = calculateChecksum(ATRequestPacket);
+    ATRequestPacket.push_back(checksum); //calculate and add checksum  
+          
+    for (int i = 0; i < ATRequestPacket.size(); i++){
+        dataLink.printf("%c",ATRequestPacket[i]); //send packet
+    }
+}
+
 uint8_t XBee_Robot::calculateChecksum(std::vector<uint8_t> & packet)
 {
     uint8_t checksum = 0xFF; //start with FF as last byte of sum is subtracted from FF
@@ -85,14 +113,14 @@
 }
 
 void XBee_Robot::RxPacketControl(std::vector<uint8_t> & packet)
-{
+{        
     uint8_t command = packet[3]; //take API address
     switch (command) { //index for different commands
         case 0x90:{ //Receive packet command
         
-            std::vector<uint8_t> source_addr64; //create new vector to store source address
-            source_addr64.insert(source_addr64.end(), packet.begin() + 4, packet.begin() + 12); //insert source address part of packet into new vector
-            checkSourceAddr(source_addr64);
+            std::vector<uint8_t> source_addr16; //create new vector to store source address
+            source_addr16.insert(source_addr16.end(), packet.begin() + 13, packet.begin() + 15); //insert source address part of packet into new vector
+            checkSourceAddr(source_addr16);
             
             std::vector<uint8_t> data; //create new vector to store data
             data.insert(data.end(), packet.begin() + 15, packet.end() -1); //insert data part of packet into new vector
@@ -102,6 +130,16 @@
             
             break;
         }
+        case 0x88:{ //AT response packet command
+            if(packet[7] == 0x00){ //if packet command status is ok                    
+                std::vector<uint8_t> data; //create new vector to store data
+                data.insert(data.end(), packet.begin() + 8, packet.end() -1); //insert data part of packet into new vector
+                if((packet[5] == 0x4D) & (packet[6] == 0x59)){ //if AT command is MY
+                    checkSourceAddr(data);
+                }
+            }
+            break;
+        }
         default:
             printf("Received API address not recognised: %c",command);
     }
@@ -119,7 +157,7 @@
     if (exists == false){ //add new address to list if no match found
         NetworkNode newNode(addr,node_list.size());//create new node
         node_list.push_back(newNode); //add new node to list
-        printf("New address added\n");
+        printf("New address added: Node %d\n",(int)node_list.size()-1); 
     }
         
 }