Labels

Tuesday, July 8, 2014

ROS Hydro SLAM with Kinect and iRobot Create

I have recently been trying to get into learning mapping with robotics and as a consequence I've been trying to get a SLAM (Simultaneous Localization And Mapping) algorithm running. Before investigating the mechanics and mathematics that cause SLAM to work, I wanted to get a known SLAM algorithm running so I can see it myself. With a laptop, Xbox Kinect, iRobot Create, and ROS, I knew I had all the tools I needed to get SLAM running.

The first step was to install ROS. Since Hydro is the newest version and has many drastic changes, I decided to go with it. Hindsight, that may have not been the best choice, but it's too late now! Also I am using Ubuntu 12.04, since Ubuntu is the officially supported OS and at the time, 12.04 is the LTS version that ROS supports.

Once ROS is installed, I had to get drivers for the iRobot Create and the Kinect, which were little adventures on there own with ROS Hydro and Ubuntu 12.04. See my posts on the Create. For the Kinect, I used the freenect stack, since it appears that openni (the most commonly used ROS-Kinect interface) has some issues with the hydro/Ubuntu 12.04 combination that I just didn't want to deal with.

Now that the sensor data is accessible, a SLAM algorithm can be selected. I went with gmapping, since it seemed to be the most widely used SLAM package on ROS. Gmapping requires odometry data, a laser scanner, and the position of the laser scanner relative to the base (create).

To fake a laser scanner with the Kinect, see my post here.

The odometry data and the odometry frame is published by the create node. The odom_frame is a standard frame from the tf package. The tf package allows the user to relate each part's coordinate frame to other coordinate frames on the robot. The primary coordinate frame that most everything needs to operate in relation to is the base_frame, which is provided by our Create. Tf wasn't the easiest thing for me to understand, and at the time of writing this, I still don't totally understand it.

To see each of your robot's coordinate frames, use:
rosrun tf view_frames
evince frames.pdf
tf view_frames will generate a pdf which displays your robot's tf tree. Each node in the tree is a separate coordinate frame, and each node in the same tree can access any other node's data with respect to the requesting node's frame.

So, gmapping needs to access the laser scanner's data with respect to the base_link frame. If you view your tf tree at this point, you will notice that the kinect's frames are not connected to the create's. To link the two, use:

rosrun tf static_transform_publisher "base_to_kinect" -0.115 0 0.226 0 0 0 base_link camera_link 100

This command creates a static transform between the kinect and the base. The numbers passed are the x,y,z coordinates of the kinect to the center of the create. If you view your tf tree after running the previous command you should get something that looks like this:


Now that all of the frames are linked, gmapping can be ran:

rosrun gmapping slam_gmapping tf_static:=tf

The tf_static:=tf renames gmapping's subscription to the /tf_static topic to the /tf topic. In ROS hydro the tf package has been deprecated, and now uses /tf2, which instead of just publishing one /tf topic, publishes a /tf and /tf_static. So, for gmapping to find the base_frame to link the map frame to, we have to redirect it to the /tf topic.

Once gmapping is running, you should be able to open up Rviz and see the data.

rosrun rviz rivz

Just be sure to add the map data from the /map topic to the display.


Monday, July 7, 2014

Faking a Laser Scanner in ROS Hydro using Kinect

I've been searching for a solution to fake a laser scanner using a kinect for ROS Hydro. The traditional method seems to be using the pointcloud_to_laserscan package, but unfortunately it hasn't been confirmed to work with ROS hydro. I also am using the freeneck_stack instead of the openni packages for interfacing with the kinect since I've had some trouble with it and apparently there are known problems with it and Ubuntu 12.04, which I am using.

The solution I'm using right now is the depthimage_to_laserscan package. It seems to work well, and it also works with freenect_stack just fine. To install and run these packages, I used the following steps:

Install and run freenect_stack:
sudo apt-get intall ros-hydro-freenect-stack
roslaunch freenect_launch freenect-xyz.launch

Be sure to note, that on the freenect page it says:
If you are using Ubuntu 12.04, you need to blacklist the kernel module that gets loaded by default for the Kinect:
sudo modprobe -r gspca_kinect
echo 'blacklist gspca_kinect' | sudo tee -a /etc/modprobe.d/blacklist.conf

Install and run depthimage_to_laserscan:
sudo apt-get install ros-hydro-depthimage-to-laserscan
rosrun depthimage_to_laserscan depthimage_to_laserscan image:=/camera/depth/image_raw

This will create the /scan topic and publish laserscan messages to it. To confirm data is being published, just use "rostopic echo /scan".

Thursday, June 12, 2014

Creating a ROS Package and C++ Node to Drive the iRobot Create

Today I'm going to go through the process I went through to create a simple node to drive the iRobot Create. I discussed last post how to get the driver for iCreate working in ROS Hydro. I want to make a package which contains a program that will allow me to drive the create using the w, a, s, and d keys on my keyboard. To get started, I need to navigate to my ROS workspace. Don't forget to source your ROS and ROS workspace settings.
source /opt/ros/hydro/setup.bash
source ~/catkin_ws/devel/setup.bash     # my workspace location

Now, in the workspace directory:
cd ~/catkin_ws/src  # navigate to my workspace src directory
catkin_create_pkg robot_mover geometry_msgs std_msgs genmsg      roscpp rospy
cd .. # return to workspace directory to make
catkin_make

Now, to create the program:
roscd robot_mover/src
gedit keyboard_mover.cpp

Inside of keyboard_mover file I used the following code, modified from here:

#include <iostream>
#include <stdio.h>
#include <ncurses.h>

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

class RobotDriver
{
private:
  //! The node handle we'll be using
  ros::NodeHandle n_;
  //! We will be publishing to the "/base_controller/command" topic to issue commands
  ros::Publisher cmd_vel_pub_;

public:
  //! ROS node initialization
  RobotDriver(ros::NodeHandle &n)
  {
    n_ = n;
    //set up the publisher for the cmd_vel topic
    cmd_vel_pub_ = n_.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
  }

  //! Loop forever while sending drive commands based on keyboard input
  bool driveKeyboard()
  {
    std::cout << "Type a command and then press enter.  "
      "Use 'w','a','s', and 'd' to navigate and 'q' to exit.\n";

    //we will be sending commands of type "twist"
    geometry_msgs::Twist base_cmd;

    char cmd;
    initscr(); //get terminal environment variables
    cbreak();  //line buffering disabled; pass on everything
    timeout(1000);  //getch blocks for 1 second

    while(n_.ok()){

      cmd = getch();

      if(cmd !='w' && cmd !='a' && cmd !='s' && cmd !='d' && cmd !='q' && cmd != 'e' && cmd != -1)
      {
        std::cout << "unknown command...\n\r";
        continue;
      }

      base_cmd.linear.x = base_cmd.linear.y = base_cmd.angular.z = 0.0;  
      //move forward
      if(cmd =='w'){
        base_cmd.linear.x = 0.25;
      }
      //move backwards
      if(cmd =='s'){
        base_cmd.linear.x = -0.25;
      }
      //turn left (yaw) and drive forward at the same time
      else if(cmd =='a'){
        base_cmd.angular.z = 0.75;
        base_cmd.linear.x = 0.0;
      }
      //turn right (yaw) and drive forward at the same time
      else if(cmd =='d'){
        base_cmd.angular.z = -0.75;
        base_cmd.linear.x = 0.0;
      }
      else if(cmd =='d'){
        base_cmd.angular.z = 0.0;
        base_cmd.linear.x = 0.0;
      }
      else if(cmd == -1){
        base_cmd.angular.z = 0.0;
        base_cmd.linear.x = 0.0;
      }        
      //quit
      else if(cmd =='q'){
        base_cmd.angular.z = 0.0;
        base_cmd.linear.x = 0.0;
        break;
      }

      //publish the assembled command
      cmd_vel_pub_.publish(base_cmd);

      std::cout << "\n\r";
    }
    nocbreak(); //return terminal to "cooked" mode
    return true;
  }

};

int main(int argc, char** argv)
{
  //init the ROS node
  ros::init(argc, argv, "robot_driver");
  ros::NodeHandle n;

  RobotDriver driver(n);
  driver.driveKeyboard();
}

Of course, I wanted this code to work without having to press enter in the terminal after each command, and I also wanted the robot to stop after not receiving a new command. I used the ncurses++ library to get characters from the terminal immediately using the getch() function. To use an external library, the CMakeList file and the package file needed to be modified.

At the end of my CMakeList.txt file I added:
## Build keyboard_mover
find_package( PkgConfig REQUIRED )
pkg_check_modules( ncurses++ REQUIRED ncurses++ )
add_executable(keyboard_mover src/keyboard_mover.cpp)
target_link_libraries(keyboard_mover ${catkin_LIBRARIES})
target_link_libraries(keyboard_mover ${ncurses++_LIBRARIES})
add_dependencies(keyboard_mover ${catkin_EXPORTED_TARGETS})


And in my package.xml I added:
<build_depend>ncurses++</build_depend>

And now I am able to drive the Create around after compiling with catkin_make. Note: you may need to install the ncurses library if you don't already have it.
sudo apt-get install ncurses-*
To run this you must run the following commands in different terminal windows in the following order:
roscore
rosrun irobot_create_2_1 # you may need to chmod 666 your tty/USB0 for this to work
rosrun robot_mover keyboard_mover


Update: I upgraded to ROS Indigo, and to get catkin_make to work again, I had to modify the CMakeLists.txt as follows:
## Build keyboard_mover
find_package( PkgConfig REQUIRED )
pkg_check_modules( ncurses++ REQUIRED ncurses++ )
add_executable(keyboard_mover src/keyboard_mover.cpp)
target_link_libraries(keyboard_mover ${catkin_LIBRARIES} ncurses)
target_link_libraries(keyboard_mover ${ncurses++_LIBRARIES})
add_dependencies(keyboard_mover ${catkin_EXPORTED_TARGETS} keyboard_mover)






Wednesday, June 11, 2014

iRobot Create ROS Hydro Drivers

I recently started messing around with ROS. I have a few common robotics development platforms laying around my lab, including an iRobot Create, which is essentially the Roomba without the vacuum parts. A lot of ROS projects are developed using the Create, so there's plenty of resources to pull from online.



Brown University has provided drivers for the Create, but the most recent ROS distro supported by the driver is Groovy. In my attempt to stay on the cutting edge, I had installed ROS Hydro, the newest distro at the time of writing this post. Fortunately, Hydro and Groovy aren't very different from each other. The biggest change since Groovy was the introduction of the catkin system which replaced rosbuild. 

From a previous Groovy installation that I was no longer using, I installed the brown drivers using:
sudo apt-get install ros-groovy-brown-drivers

After installing, I copied the "irobot_create_2_1" directory from inside "/opt/ros/groovy/stacks/brown_drivers" folder to my workspace's "src" directory. For example:
cp -r /opt/ros/groovy/stacks/brown_drivers ~/catkin_ws/src

Then, in my workspace directory, I ran catkin_make, and the driver ran perfectly! To run the driver use:
rosrun irobot_create_2_1 driver.py

 Thanks to Brown driver maintainers for updating the driver to be compatible with the new catkin system, everything works just fine so far. To test the driver, you can download BumpGo.py and place it in the "irobot_create_2_1/bin" directory. To test, simply enter:
rosrun irobot_create_2_1 bumpGo.py

The robot should then drive until the bump sensor is triggered, turn and continue. My future goals are to create a simple node to drive the robot using keyboard commands, interface an Xbox Kinect, and run some kind of SLAM algorithm. I really just wanted to make sure I added some documentation as to what I've done to run the create on ROS Hydro, regardless of how simple it was. (In the very likely event that I get distracted, come back months from now to continue the project, and forget everything I learned since then)

UPDATE:
Here's a link to the create drivers package:
https://dl.dropboxusercontent.com/u/47687797/irobot_create_2_1.tar.gz

UPDATE 2:
After reinstalling ROS on a new computer without my previous groovy installation, I had some trouble getting the above package to run. When I ran "rosrun irobot_create_2_1 driver.py", I would get a message which said " the rosdep view is empty: call 'sudo rosdep init' and 'rosdep update ". At first, the roomba would not connect and displayed some other error message I am now unable to reproduce. After replacing the battery the next morning, the create is now connecting without any problems! Let me know in the comments if you have any issues regarding this.


Friday, May 30, 2014

Estimating Distance from RSSI Values using XBee

Another really fun thing to do in wireless networks is finding your distance from another transceiver using signal strength values. Because of noise, multipathing, and various things that impede wireless signals (such as fleshy, water-filled human bodies), it is impossible to get very fine-grain estimations from RSSI values alone. From my research, it seems that a resolution of ~1m is as good as it gets. Which means if you don't have a good equation modeling the environment in which your network is deployed, your results will be horrible.

Distance Equation:

After searching all over the internet and the IEEE Xplore library, I have finally found an equation that works relatively well for RSSI/distance estimation from this journal article.

RSSI = -(10*n*log10(d) + A)
Where
  • RSSI is the RSSI value received (dBm)
  • n is the path-loss exponent
  • d is the distance
  • A is the RSSI value at a reference distance
So what is the path-loss exponent? How do you select a value for A?

Path Loss Exponent:

The path loss exponent has to be determined experimentally. The path loss variable ranges from around 2 to 4, where 2 is the free-space value (no obstruction, line of sight) and 4 represents a very lossy environment. A simplified form of the model is represented below from wikipedia, however, a more detailed one can be found here.

 L = 10\ n\ \log_{10}(d)+C, where L is the path loss in dB, n is the path loss exponent, d is the distance between transceivers, and C is a constant which accounts for system losses.

This table from this Zigbee book gives sample path loss exponent values for common environments:













Selecting the Reference Value:

A can be found by measuring the RSSI value at whatever distance you want to reference. My measurements are taken in meters, so A is the RSSI value received when the receiver is 1 meter in front of the transmitter with no obstacles in between.

Implementation on XBee:

So far this has been all talk. Let's see how this works in a real environment.

I will be testing this formula out using XBee series 2 modules and these antennas (Digi P\N: A24-HASM-525). This antenna is important, as it is an omni-directional antenna; meaning it has a relatively uniform radiation patter. Most chip antennae have a very non-unimform radiation pattern, causing the RSSI value to fluctuate drastically due to the orientation of the nodes, rendering everything we are trying to do here useless.

My experimental setup involves two series 2 XBees, one coordinator and one end device. The end device is attached to a Sparkfun RedBoard (arduino clone). The coordinator is connected to my laptop using a Sparkfun XBee Xplorer board. The RedBoard is running code written using Atmel Studio and my XBee library which echos back to the sender the RSSI value and estimated distance.

I have set the following function to be called when a new packet arrives using "setNewPacketCB()". This function retrieves the RSSI value and applies the formula above to estimate distance and return it to the sender.

float A = -45.0; // Reference RSSI value at 1 meter
float n = 2.2; // Path-loss exponent


void newPacket()
{
RxPacket rx_data = getPacket();
// Echo rssi value back
int len = 1; // sprintf buffer length
float distance;
char buff[20];
char rssi = getRSSI();
if(rssi != 0xFF) // 0xFF is error code when value is not returned
{
// sign in front of rssi changed to positive because rssi value from XBee is negative
distance = pow(10.0,((A + rssi)/(10.0*n)));
len = sprintf(buff, "%d + %3f\n",(unsigned int)rssi, distance);

ZigBee_TX_Request(0,0,0xFFFF,rx_data.source_addr_16bit, 1, 0, buff, len);
}
}

For this code to work you will need to ensure that you are using the floating point math library and floating point version of printf. To compile properly in AVR/Atmel Studio:

  1. Go to Project->Properties (ALT+F7) and under AVR/GNU C++ Linker select Libraries. Under Libraries (-Wl,-l) use the Add button twice and insert libprintf_flt then libm
  2. Go to Miscellaneous and add -Wl,-u,vfprintf -lprintf_flt -lm

After downloading my code to the RedBoard and connecting the coordinator to the laptop, if I open up a terminal using X-CTU connected to the coordinator and hit enter, I get the following output:


Where 51 means the RSSI value received was -51 dBm and the estimated distance was 1.87 meters.

Before testing, I recorded the RSSI value at one meter away, which gave me a value of -45dBm. Plugging this into the A variable in my code and setting n to be 2.2 (Choosing the n value for a retail store from the chart since the building I'm working in is somewhat similar. Sadly, this was a terrible first guess) I downloaded the code and set up my laptop in the hallway of the EPIC building at UNCC. I placed sticky note markers at various lengths to move my end device to for measuring.

Results:

I have unfortunately learned that hallways behave as giant waveguides, so my results were not spectacular. In a hallway, the multipathing effect is very high, so after certain distances away from the transmitter, the receiver begins to hear the same signal reflected back into itself, amplifying the value.

This is very bad for trying to estimate distance because you can't get any information out of the data because it doesn't attenuate as you move farther from the source as it would in free space. The following chart shows my results from the hallway test:

As you can see, the data is all over the place. However, multipathing was not the only problem in the test environment. I needed to reverse calculate the path-loss exponent by using the distance formula to solve for n by plugging in measured RSSI values at known distances. After changing the path loss exponent to 2, the following values were obtained:
This new n value caused less fluctuation in distance estimation results, but as the multipathing effect caused the signal to attenuate very slowly, it became indistinguishable at larger distances. I also took fewer data points having realized the hallway was going to produce these results.

I plan to test this code again in a large, open space, so be sure to check back later for those results!

Thursday, April 10, 2014

Using the XBee Library with Atmel Studio 6

Now that the library is coming together, I thought it would be useful to write up a quick tutorial on how to use the library, and elaborate a little more on the structure.

The test platform I initially used when developing this code is shown below:



Although an arduino board was used, the arduino bootloader was not. However, since the arduino compiler uses avr-gcc, the library can still be included with arduino code. The ISP programming pins are available on the arduino, and any ISP programmer can be used to upload c code to the Atmega328p microcontroller. I used the AVRISP MKII, which can be purchased from mouser for around 40 bucks.


I prefer the AVRISP MKII, although it is more expensive than 3rd party programmers, it is almost always guaranteed to work with Atmel studio software.

Now, I will create a new project in Atmel Studio 6:


Remember to select the proper AVR if you are using something other than an arduino uno. Since I'm using the uno, I've selected the Atmega328p. After the new project is created, I will copy the XbeeS2.c, XbeeS2.h, and XbeeHAL.h files into the same directory as the .c file with main(). After the files have been added, be sure to add the files to the project:



Once the files are added, be sure to #include "XbeeS2.h". Before compiling, the only file the user needs to edit to use the library is XbeeHAL.h. XbeeHAL contains all the hardware specific abstractions. At the time of writing this tutorial, the Atmega328p and Atmega2560 is supported. If you are also using the Atmega328p, you just have to make sure that "ATmega328P" is #defined. If you are using a different micro, there are a few functions to define that are necessary for the library to function properly.

  • XBEE_UDR - Xbee USART Data Register; This is macro'd to the Data Register Buffer of the USART that the microcontroller is using for interfacing with the Xbee.
  • TX_BUFFER_IS_FULL() - Normally macro'd to ((UCSR0A & (1 << RXC0)) == 0) for the ATmega328P, this is a simple function that is used to check if the UDR is ready to be written. Most USART modules on microcontroller include a bit in a configuration register that indicates when the register is clear, so just change it such that it is appropriate for your micro.
  • RX_BUFFER_IS_FULL() - Macro'd to ((UCSR0A & (1 << UDRE0)) == 0) for the ATmega328P, this is the same thing as TX_BUFFER_IS_FULL(), but indicates when new data is received on UDR rather than when UDR is available to be written.
  • TOGGLE_LED()/LED_OUTPUT() - A test led. This can be changed to whatever is appropriate for your setup, and may be removed from the library later. Not actually needed for proper functionality.
This covers the macros needed, but there are also 2 functions that need to be set in XbeeHAL.h.
  • ISR(USART_RX_vect) { rxISR(); } - This is strictly for AVR's, but the HAL needs to have access to your micro's USART ISR. All the ISR needs to do is call the rxISR() function from the Xbee.c file.
  • void USART_INIT(void) - This is a function just to set up the USART at whatever baud rate you want to specify. Just make sure it matches your Xbee's baud rate! ;)
That covers the hairy details. If you are using the Arduino UNO or ATmega328P, then all of those points can be ignored.


To use the library, the user must call XbeeUSART_init() first. Then, the user may call any function from the library, such at Zigbee_Transmit_Request() for sending messages. Also, a callback feature is available for when the user wants to perform some task when a new packet arrives. To do this, the user calls setNewPacketCB(), passing it the name of the function to be called when the new packet arrives. 




I hope this helps anyone who is looking to try out the library! I will post new revisions here as they come out! Remember, you should only have to modify the HAL, and all the functions available can be found in Xbee.h.

Thanks for reading!

XBee S2 HAL
XBeeS2.c
XBeeS2.h

UPDATE:
I have added more features and restructured some of the library's files. For the time being I will post a link here to the newest version, and later provide a tutorial and github link.
https://dl.dropboxusercontent.com/u/47687797/XBee%20Library.zip

Monday, April 7, 2014

Links to XBee Series 2 Code

Last post I had copied the XBee Series 2 Library code into the text itself, but now I have convenient links! They are still a work in progress, but feel free to give it a shot!

XBee S2 HAL
XBeeS2.c
XBeeS2.h

Monday, March 31, 2014

XBee Library Evolved

It has been quite some time since the last entry, and since then much has happened with the XBee C library and the progress on the ATmega-based sensor network.

The ATmega portion has forked off into its own separate project, which is in its prototype stage right now. The design itself has already won awards at IEEE Southeastcon for best poster/short paper, and we are expecting great progress with the project!

Now that we are working with a specific board, it was time to make the library much more versatile. The hardware-dependent functions have been moved into a hardware abstraction layer (HAL), and only raw XBee functionality remains in the c file. This way, we have no  microcontroller preference, and only the HAL has to be modified for use in the code.

In previous versions of the code, we had a function within the library for responding to new packets. This was a huge problem, as it required the user to go digging through the library itself, modifying the function inside, and exporting variables to the application code. This was a terrible design, and a solution had to be implemented. Now, the user calls a function, "setNewPacketCB()", which is passed a pointer to a function that is written in the application code. So, if you have a function, called, say, weiners(), you can set weiners to be called when a new packet arrives by calling "setNewPacketCB(weiners);".

Since the addition of the HAL, the user now has to #define which microcontroller he/she (who am I kidding?) he wants to use, and the HAL defines the appropriate ISR for the micro's USART, which USART is used, and what USART settings are configured. So, if the default settings aren't what you want, you only need to modify the contents of the HAL.

Originally, the micro being used was the AVR ATmega328P. We have now moved onto the mega2560, and soon to the mega1280 for low power capabilities. Introducing multiple USARTs have caused us to move the streaming target for printf() into the libraries for the development board, which we will discuss another day. Now, the print statements are safely wrapped in a #ifdefine statement, which will give the user feedback if DEBUG is #define'd. This also makes it easier for user to use this library with other micros sine the standard streaming target code won't need to be modified.

SO! Overall, the user now only needs to modify the HAL to use the library with any micro! More functions are still to come! But for now, here's what we have:

Xbee.h:
#include <stdio.h>
#include <stdlib.h>

#define RSSI_PIN PIND
#define RSSI_DDR DDRD
#define RSSI_PIN_NO PIND3

typedef struct{
int len;
char api_identifier;
long DH;
long DL;
int source_addr_16bit;
char options;
char frame_id;
char AT_com[2];
char status;
char data[20]; // also stores the "value" for at command response
int data_len; // stores the length of the data
char checksum;
} RxPacket;

void rxISR();
void receive_Msg(RxPacket*);
void send_Msg(char*, int);
void USART_vSendByte(char);
void ZigBee_TX_Request(char, long, long, int, char, char, char*, int);
void AT_Command(char, char, char, char*, int);
void Init_Timer0(void);
float getRSSIPWM(void);
char getRSSI(void);
int get16bitAddress(void);
void killTimer(void);
float getTime_us(void);
void setNewPacketCB(void (*funptr)(void));
RxPacket getPacket(void);
void XbeeUSART_init(void);

XBeeHAL.h:
#include "Xbee.h"

//#define ATmega328P
#define ATmega2560

// Register Names for selected microcontroller
#ifdef ATmega328P
#include <avr/io.h>
#include <avr/interrupt.h>

// USART Definitions
#ifdef F_CPU
#pragma message ("F_CPU already defined.")
#else
#define F_CPU 16000000 //be sure to adjust for various clock settings
#endif
#define USART_BAUDRATE 9600
#define BAUD_PRESCALE (((F_CPU / (USART_BAUDRATE * 16UL))) - 1)

// Registers
#define XBEE_UDR UDR0

// Macros
#define RX_BUFFER_IS_FULL() ((UCSR0A & (1 << RXC0)) == 0)
#define TX_BUFFER_IS_FULL() ((UCSR0A & (1 << UDRE0)) == 0)
#define TOGGLE_LED() (PORTD ^= 0x80)
#define LED_OUTPUT() (DDRD |= 0x80)
// Functions
ISR(USART_RX_vect) { rxISR(); }

/* Code to initialize USART
*
* Eventually, this function should receive a USART number (e.g., 0-X, X being the number of USARTs available
* on the chip) and a baud rate. For now, we will use the #define variables to set this.
*/
void USART_INIT(void)
{
UBRR0L = BAUD_PRESCALE; // Load lower 8-bits of the baud rate value into the low byte of the UBRR register
UBRR0H = (BAUD_PRESCALE >> 8); // Load upper 8-bits of the baud rate value into the high byte of the UBRR register

UCSR0C |= (1 << UCSZ00) | (1 << UCSZ01); // Use 8-bit character sizes
UCSR0B |= (1 << RXCIE0) | (1 << RXEN0) | (1 << TXEN0);   // Turn on the transmission and reception circuitry
// Enable RX interrupt as well.
}

#endif

#ifdef ATmega2560
#include <avr/io.h>
#include <avr/interrupt.h>

// USART Definitions
#ifdef F_CPU
#pragma message ("F_CPU already defined.")
#else
#define F_CPU 8000000 //be sure to adjust for various clock settings
#endif
#define USART_BAUDRATE 9600
#define BAUD_PRESCALE (((F_CPU / (USART_BAUDRATE * 16UL))) - 1)

// Registers
#define USART_NO 3
#define XBEE_UDR UDR3

// Macros
#define RX_BUFFER_IS_FULL() ((UCSR3A & (1 << RXC3)) == 0)
#define TX_BUFFER_IS_FULL() ((UCSR3A & (1 << UDRE3)) == 0)
#define TOGGLE_LED() (PORTD ^= 0x80)
#define LED_OUTPUT() (DDRD |= 0x80)
// Functions
ISR(USART3_RX_vect) { rxISR(); }
/* Code to initialize USART
*
* Eventually, this function should receive a USART number (e.g., 0-X, X being the number of USARTs available
* on the chip) and a baud rate. For now, we will use the #define variables to set this.
*/
void USART_INIT(void)
{
UBRR3L = BAUD_PRESCALE; // Load lower 8-bits of the baud rate value into the low byte of the UBRR register
UBRR3H = (BAUD_PRESCALE >> 8); // Load upper 8-bits of the baud rate value into the high byte of the UBRR register

UCSR3C |= (1 << UCSZ30) | (1 << UCSZ31); // Use 8-bit character sizes
UCSR3B |= (1 << RXCIE3) | (1 << RXEN3) | (1 << TXEN3);   // Turn on the transmission and reception circuitry
// Enable RX interrupt as well.
}
#endif

Xbee.c
/*
 * Xbee.c
 *
 * Created: 6/20/2013 1:29:02 PM
 *  Author: Waron
 */ 
#include "Xbee.h"
#include "XbeeHAL.h"

RxPacket rx_pkt;

// Global Variables
char RSSI;
int MY;
int cb; // callback flag -- set if a callback function for new packets has been specified
void (*cbFunPtr)(void); // callback function pointer

/* Routine to call USART initialization routine from the HAL
 *
 * This routine configures the USART that the XBee is attached to.
 * Configues for settings 8N1 and 9600 baud.
 */
void XbeeUSART_init()
{
USART_INIT();
}

/* Routine to send a byte through USART
 *
 * This routine polls the USART data buffer register full flag until it indicates it is ready
 * for a new byte to be transmitted, then loads the new byte into the USART data buffer
 * register for transmission.
 */
void USART_vSendByte(char Data)
{
// Wait if a byte is being transmitted
while (TX_BUFFER_IS_FULL()) {}; // Do nothing until UDR is ready for more data to be written to it
// Transmit data
XBEE_UDR = Data;
}

/* USART Receive Interrupt
 * 
 * Hardware interrupt routine inside the HAL calls this function. When a new byte is received, it is
 * checked for the start delimeter of a new packet. If it is, then the receive message function is called.
 * Otherwise, nothing happens. After a packet is received, the callback function is called to handle the
 * packet, if one has been configured.
 */
void rxISR()
{
cli();
TOGGLE_LED();
if(XBEE_UDR == 0x7E)
{
RxPacket pkt;
receive_Msg(&pkt);
rx_pkt = pkt;
// Call function for responding to new packet
if(cb)
(*cbFunPtr)();
}

TOGGLE_LED();
sei();
}
/* Function to return the 16 bit Address of the Xbee
 *
 * This function calls the MY AT command and waits until the response frame is 
 * received.
 */
int get16bitAddress()
{
RxPacket pkt;
int temp;

do 
{
AT_Command(0x01, 'M', 'Y', 0, 0); // Send DB AT Command
while (RX_BUFFER_IS_FULL()){};
temp = XBEE_UDR;
receive_Msg(&pkt);
} while (pkt.api_identifier != 0x88);

#ifdef DEBUG
printf("MY is %d\n", (pkt.data[0]<<8)|pkt.data[1]);
#endif

return (pkt.data[0]<<8)|pkt.data[1]; // Return MY
}

/* Send Message Function
 *
 * This function is passed a character array of an XBee API Packet, along with the
 * length of the packet. The checksum is calculated, then the start delimeter is transmitted,
 * followed by the length of the packet, then the data, and finally the checksum.
 */
void send_Msg(char *data, int len)
{
//Generate checksum
char checksum;
int counter = 0, sum = 0;
for(counter = 0; counter <= len - 1; counter++)
sum += data[counter];
//Checksum is calculated by adding the data values together, and subtracting the 
//last 8 bits from 0xFF.
checksum = 0xFF - (sum & 0x00FF); 
//Transmit data
USART_vSendByte(0x7E);  //Start delimiter
USART_vSendByte(8 >> len);  //Length MSB
USART_vSendByte(len); //Length LSB
for(counter = 0; counter <= len - 1; counter++)  //Transmit data
USART_vSendByte(data[counter]);
USART_vSendByte(checksum);  //Transmit checksum
}

void receive_Msg(RxPacket *rx_data)
{
int count;
char temp;
rx_data->data_len = 0;

while (RX_BUFFER_IS_FULL()) {}; // Do nothing until data have been received and is ready to be read from UDR
temp = XBEE_UDR; //next incoming byte is the MSB of the data size
while (RX_BUFFER_IS_FULL()) {}; // Do nothing until data have been received and is ready to be read from UDR

rx_data->len = (temp << 8) | XBEE_UDR; //merge LSB and MSB to obtain data length
while (RX_BUFFER_IS_FULL()) {}; // Do nothing until data have been received and is ready to be read from UDR
rx_data->api_identifier = XBEE_UDR;

switch(rx_data->api_identifier) // Select proper sequence for receiving various packet types
{
case 0x90: // Zigbee Receive Packet
for(count = 1; count < rx_data->len; count++)
{
while (RX_BUFFER_IS_FULL()) {}; // Do nothing until data have been received and is ready to be read from UDR
if(count == 1)
rx_data->DH = ((long)XBEE_UDR << 24);
else if(count == 2)
rx_data->DH |= ((long)XBEE_UDR << 16);
else if(count == 3)
rx_data->DH |= ((long)XBEE_UDR << 8);
else if(count == 4)
rx_data->DH |= (long)XBEE_UDR;
else if(count == 5)
rx_data->DL = ((long)XBEE_UDR << 24);
else if(count == 6)
rx_data->DL |= ((long)XBEE_UDR << 16);
else if(count == 7)
rx_data->DL |= ((long)XBEE_UDR << 8);
else if(count == 8)
rx_data->DL |= (long)XBEE_UDR;
else if(count == 9)
rx_data->source_addr_16bit = (XBEE_UDR << 8);
else if(count == 10)
rx_data->source_addr_16bit = XBEE_UDR;
else if(count == 11)
rx_data->options = XBEE_UDR;
else
{
rx_data->data[count - 12] = XBEE_UDR;
rx_data->data_len++;
}
}
while (RX_BUFFER_IS_FULL()) {}; // Do nothing until data have been received and is ready to be read from UDR
rx_data->checksum = XBEE_UDR; //store checksum
break;
case 0x88: // AT Command
for(count = 1; count < rx_data->len; count++)
{
while (RX_BUFFER_IS_FULL()) {}; // Do nothing until data have been received and is ready to be read from UDR
if(count == 1)
rx_data->frame_id = XBEE_UDR;
else if(count == 2)
rx_data->AT_com[0] = XBEE_UDR;
else if(count == 3)
rx_data->AT_com[1] = XBEE_UDR;
else if(count == 4)
rx_data->status = XBEE_UDR;
else
{
rx_data->data[count - 5] = XBEE_UDR;
rx_data->data_len++;
}
}
while (RX_BUFFER_IS_FULL()) {}; // Do nothing until data have been received and is ready to be read from UDR
rx_data->checksum = XBEE_UDR; //store checksum
break;
default:
break;
}

}

void ZigBee_TX_Request(char Frame_ID, long DH, long DL, int _16bitAddr, char Hops, char Options, char *RF_Data, int len )
{
int i; // counting variable
char buff[14 + len]; //temporary buffer for transmitting
// ZigBee Transmit Request API Identifier
buff[0] = 0x10;
// Identifies the UART data frame for the host to correlate with a 
// subsequent ACK (acknowledgment). Setting Frame ID to ‘0' will disable response frame.
buff[1] = Frame_ID;
// MSB first, LSB last. Broadcast = 0x000000000000FFFF
buff[2] = (DH >> 24);
buff[3] = (DH >> 16);
buff[4] = (DH >> 8);
buff[5] = DH;
buff[6] = (DL >> 24);
buff[7] = (DL >> 16);
buff[8] = (DL >> 8);
buff[9] = DL;
// 16 bit address
buff[10] = (_16bitAddr >> 8);
buff[11] = _16bitAddr;
// Number of hops for message to take
buff[12] = Hops;
// Options
buff[13] = Options;
for(i = 0; i < len; i++)
buff[14+i] = RF_Data[i];
send_Msg(buff, 14+len);
}

void AT_Command(char frameid, char command1, char command2, char *options, int len)
{
char buff[5]; //temporary buffer for transmitting
int count;
buff[0] = 0x08; // API ID for AT Commands
buff[1] = frameid; // Frame ID; set to 0 for no response
buff[2] = command1;
buff[3] = command2;
for(count = 1; count <= len; count++)
buff[3 + count] = options;
send_Msg(buff, 4 + len);
}

/* Function to get RSSI from the PWM pin on the XBee
 *
 * This function returns the high time in microseconds from the PWM signal with corresponds to
 * the RSSI of the last received message. This is accomplished by starting and stopping an 8-bit
 * timer.
 */
float getRSSIPWM(void)
{
char RSSI;
float time_us;

Init_Timer0();
while(bit_is_set(PIND, PORTD3) && (TCNT0 != 50));
killTimer();
while(bit_is_clear(PIND, PORTD3));
Init_Timer0();
while(bit_is_set(PIND, PORTD3) && (TCNT0 != 50));
#ifdef DEBUG
printf("TCNT0 is %d\n", TCNT0);
#endif
killTimer();

return time_us/200.0;
}

/* This function is under construction, and is not guaranteed to work for all uC's */
void Init_Timer0(void)
{
TCNT0 = 0; // clear count value
TCCR0B = 0x03; // Select clk/64
}

/* This function is under construction, and is not guaranteed to work for all uC's */
void killTimer(void)
{
TCCR0B = 0x00; // disable clk
TCNT0 = 0; // clear count value
}

/* This function is under construction, and is not guaranteed to work for all uC's */
float getTime_us(void)
{
return (float)TCNT0*(1.0/((float)F_CPU/64.0));

}

/* Function to get the RSSI value of the last received message in dB
 *
 * This function calls the DB AT command. It then waits for the returning packet.
 * if the packet is not the returned DB value, then it is discarded.
 * The returned value is the attenuation of the signal in -dB.
 */
char getRSSI(void)
{
RxPacket pkt;
int temp;

AT_Command(0x01, 'D', 'B', 0, 0); // Send DB AT Command
while (RX_BUFFER_IS_FULL()){};
temp = XBEE_UDR; // Probably should add some code to check for start delimeter
receive_Msg(&pkt);

if(pkt.api_identifier != 0x88)
{
#ifdef DEBUG
printf("Failed to retrieve RSSI\n");
#endif
// otherwise, handle the packet as usual
if(cb)
(*cbFunPtr)();
return 0xFF;
}
else
{
#ifdef DEBUG
printf("RSSI is %d\n", pkt.data[0]);
#endif
return pkt.data[0]; // Return RSSI value
}

}

/* Callback function for new packets
 *
 * The user passes the name of their designated callback function. The function's address is
 * then set to the callback fuction pointer, cbFunPtr. The callback flag, cb, is set.
 */
void setNewPacketCB(void (*funptr)())
{
cb = 1;
cbFunPtr = funptr;
}

/* Get Packet
 *
 * Returns the last received packet.
 */
RxPacket getPacket()
{
return rx_pkt;
}