/home/fwph/code/wurde/modules/rFlexBase/RFlex.cpp

Go to the documentation of this file.

#include <RFlex.H>
#include <cassert>
#include <cstring>
#include <cmath>
#include <iostream>
#include <unistd.h>
#include <sys/select.h>
#include <netinet/in.h>

#include <wds-debug.h>


// Magic numbers to convert rFlex units to real units
static const double B21_MAGIC_DISTANCE = 101230.0;
static const double B21_MAGIC_ANGLE = 35343.888;
static const double B21_MAGIC_TORQUE = 35000.0;

static const double ATRVJR_MAGIC_DISTANCE = 90810.0;
static const double ATRVJR_MAGIC_ANGLE = 37000.0;
static const double ATRVJR_MAGIC_TORQUE = 35000.0;

// Protocol information
static const int PROTOCOL_SIZE = 9;
static const int PACKET_CRC_START = 2;
static const int PACKET_CRC_OFFSET = 4;

// Escape codes used in the data packets
static const unsigned char NUL = 0;
static const unsigned char SOH = 1;
static const unsigned char STX = 2;
static const unsigned char ETX = 3;
static const unsigned char ESC = 27;

// Packet data identifiers
static const unsigned int SYSTEM_PORT = 1;
static const unsigned int MOTOR_PORT = 2;
static const unsigned int JOYSTICK_PORT = 3;
static const unsigned int SONAR_PORT = 4;
static const unsigned int DIGITAL_IO_PORT = 5;
static const unsigned int IR_PORT = 6;

// Packet data slots
static const int PACKET_PORT_BYTE = 2;
static const int PACKET_ID_BYTE = 3;
static const int PACKET_OPCODE_BYTE = 4;
static const int PACKET_SIZE_BYTE = 5;
static const int PACKET_DATA_START_BYTE = 6;


// Packet data opcodes
static const int SYSTEM_LCD_DUMP = 0;
static const int SYSTEM_STATUS = 1;

static const int MOTOR_AXIS_GET_SYSTEM = 0;
static const int MOTOR_AXIS_GET_MODEL = 1;
static const int MOTOR_AXIS_GET_TARGET = 2;
static const int MOTOR_AXIS_SET_LIMITS = 3;
static const int MOTOR_AXIS_GET_LIMITS = 4;
static const int MOTOR_AXIS_SET_POS_LIMITS = 5;
static const int MOTOR_AXIS_GET_POS_LIMITS = 6;
static const int MOTOR_AXIS_SET_DIR = 7;
static const int MOTOR_AXIS_SET_POS = 8;
static const int MOTOR_AXIS_GET_MODE = 9;
static const int MOTOR_SET_DEFAULTS = 10;
static const int MOTOR_BRAKE_SET = 11;
static const int MOTOR_BRAKE_RELEASE = 12;
static const int MOTOR_SYSTEM_REPORT = 33;
static const int MOTOR_SYSTEM_REPORT_REQUEST = 34;
static const int MOTOR_GET_NAXES = 65;
static const int MOTOR_SET_GEARING = 66;
static const int MOTOR_GET_GEARING = 67;
static const int MOTOR_MOTOR_SET_MODE = 68;
static const int MOTOR_MOTOR_GET_MODE = 69;
static const int MOTOR_MOTOR_SET_PARMS = 70;
static const int MOTOR_MOTOR_GET_PARMS = 71;
static const int MOTOR_MOTOR_SET_LIMITS = 72;
static const int MOTOR_MOTOR_GET_LIMITS = 73;
static const int MOTOR_MOTOR_GET_DATA = 74;
static const int MOTOR_AXIS_SET_PARMS = 75;
static const int MOTOR_AXIS_GET_PARMS = 76;
static const int MOTOR_AXIS_SET_PWM_LIMIT = 77;
static const int MOTOR_AXIS_GET_PWM_LIMIT = 78;
static const int MOTOR_AXIS_SET_PWM = 79;
static const int MOTOR_AXIS_GET_PWM = 80;

static const int JOYSTICK_GET_STATE = 0;

static const int SONAR_RUN = 0;
static const int SONAR_GET_UPDATE = 1;
static const int SONAR_REPORT = 2;

static const unsigned char SONAR_OFF = 0;
static const unsigned char SONAR_REPORT_ON_REQUEST = 1;
static const unsigned char SONAR_CONTINUOUS_REPORT = 2;

static const int DIGITAL_IO_REPORT_REQUEST = 0;
static const int DIGITAL_IO_REPORT = 1;
static const int DIGITAL_IO_GET_UPDATE = 2;
static const int DIGITAL_IO_UPDATE = 3;
static const int DIGITAL_IO_SET = 4;

static const int IR_RUN = 0;
static const int IR_REPORT = 1;


using namespace std;

//======================================================================
// RFlex
//======================================================================

RFlex::RFlex(const std::string &filename, const std::string &robotType) :m_port(filename, 115200), m_found(false), m_offset(0), m_translate_command(0.0), m_rotate_command(0.0) {
        // Set the initial values of the holding variables
        m_x = 0.0;
        m_y = 0.0;
        m_theta = 0;
        m_last_distance = 0;
        m_last_angdistance = 0;
        m_last_distance_set =false;
        m_last_angdistance_set = false;
        m_trans_velocity = 0.0;
        m_rot_velocity = 0.0;
        
        bzero(m_sonars,MAX_SONAR*sizeof(double));
        if(robotType=="B21r"){
               m_type=RFLEX_B21R;
               m_magicDistance=B21_MAGIC_DISTANCE;
               m_magicAngle=B21_MAGIC_ANGLE;
               m_magicTorque=B21_MAGIC_TORQUE;
               m_numSonar=0; // not doing B21 sonar yet
        }else if(robotType=="ATRVJr"){
               m_type=RFLEX_ATRVJR;
               m_magicDistance=ATRVJR_MAGIC_DISTANCE;
               m_magicAngle=ATRVJR_MAGIC_ANGLE;
               m_magicTorque=ATRVJR_MAGIC_TORQUE;
               m_numSonar=17;
        }else{
               cerr << "Unknown robot type!" << endl;
               exit(1);
        }
        
        // Initialize the mutexes
        pthread_mutex_init(&m_write_mutex, NULL);
        pthread_mutex_init(&m_motor_mutex, NULL);
        pthread_mutex_init(&m_command_mutex, NULL);

        // Start the read and periodic threads
        pthread_create(&m_read_thread, NULL, RFlex::rFlexReadThread, this);
        pthread_create(&m_periodic_thread, NULL, RFlex::rFlexPeriodicThread, this);

        // Get some initial values
        systemUpdate();
        odometryOn();
}



RFlex::~RFlex() {
        // Stop the systems
        motorSetTranslateVelocity(0.0);
        motorSetRotateVelocity(0.0);
        odometryOff();
        digitalIOOff();
        sonarOff();

        // Get rid of the mutexes
        pthread_mutex_destroy(&m_write_mutex);
        pthread_mutex_destroy(&m_motor_mutex);

        // Need to clean up the threads here
}


bool RFlex::systemUpdate() {
        pthread_mutex_lock(&m_write_mutex);
        const bool retval = sendCommand(SYSTEM_PORT, 0, SYSTEM_STATUS, 0);
        pthread_mutex_unlock(&m_write_mutex);

        return retval;
}


#warning Need to check this
bool RFlex::systemLCDDump() {
        pthread_mutex_lock(&m_write_mutex);
        const bool retval = sendCommand(SYSTEM_PORT, 0, SYSTEM_LCD_DUMP, 5);
        pthread_mutex_unlock(&m_write_mutex);

        return retval;
}


bool RFlex::motionSetDefaults() {
        pthread_mutex_lock(&m_write_mutex);
        pthread_mutex_unlock(&m_write_mutex);

#warning Need to implement this
        return false;
}


#warning No effect
bool RFlex::setBrake() {
        pthread_mutex_lock(&m_write_mutex);
        const bool retval = sendCommand(MOTOR_PORT, 0, MOTOR_BRAKE_SET, 0);
        pthread_mutex_unlock(&m_write_mutex);

        return retval;
}


#warning No effect
bool RFlex::releaseBrake() {
        pthread_mutex_lock(&m_write_mutex);
        const bool retval = sendCommand(MOTOR_PORT, 0, MOTOR_BRAKE_RELEASE, 0);
        pthread_mutex_unlock(&m_write_mutex);

        return retval;
}


void RFlex::motorSetTranslateVelocity(const double velocity) {
        pthread_mutex_lock(&m_command_mutex);
        m_translate_command = velocity;
        pthread_mutex_unlock(&m_command_mutex);
}


void RFlex::motorSetRotateVelocity(const double velocity) {
        pthread_mutex_lock(&m_command_mutex);
        m_rotate_command = velocity;
        pthread_mutex_unlock(&m_command_mutex);
}


void RFlex::motorSetVelocity(const double translate, const double rotate) {
        pthread_mutex_lock(&m_command_mutex);
        m_translate_command = translate;
        m_rotate_command = rotate;
        pthread_mutex_unlock(&m_command_mutex);
}


bool RFlex::odometryOn(const int period) {
        pthread_mutex_lock(&m_write_mutex);
        insertLong(period, 0);  // Period in ms
        insertLong(3, 4);       // Mask

        const bool retval = sendCommand(MOTOR_PORT, 0, MOTOR_SYSTEM_REPORT_REQUEST, 8);
        pthread_mutex_unlock(&m_write_mutex);

        return retval;
}


bool RFlex::odometryOff() {
        pthread_mutex_lock(&m_write_mutex);
        insertLong(0, 0);    // Period in ms
        insertLong(3, 4);    // Mask

        const bool retval = sendCommand(MOTOR_PORT, 0, MOTOR_SYSTEM_REPORT_REQUEST, 8);
        pthread_mutex_unlock(&m_write_mutex);

        return retval;
}


bool RFlex::digitalIOOn(const int period) {
        pthread_mutex_lock(&m_write_mutex);
        insertLong(period, 0);    // period in ms

        const bool retval = sendCommand(DIGITAL_IO_PORT, 0, DIGITAL_IO_REPORT_REQUEST, 4);
        pthread_mutex_unlock(&m_write_mutex);

        return retval;
}


bool RFlex::digitalIOOff() {
        pthread_mutex_lock(&m_write_mutex);
        insertLong(0, 0);    // period in ms

        const bool retval = sendCommand(DIGITAL_IO_PORT, 0, DIGITAL_IO_REPORT_REQUEST, 4);
        pthread_mutex_unlock(&m_write_mutex);

        return retval;
}


bool RFlex::joystickReport() {
        pthread_mutex_lock(&m_write_mutex);
        const bool retval = sendCommand(JOYSTICK_PORT, 0, JOYSTICK_GET_STATE, 0);
        pthread_mutex_unlock(&m_write_mutex);

        return retval;
}


bool RFlex::sonarOn() {
        pthread_mutex_lock(&m_write_mutex);
        insertLong(30000, 0);    // Echo delay
        insertLong(0, 4);        // Ping delay
        insertLong(0, 8);        // Set delay
        insertChar(SONAR_CONTINUOUS_REPORT, 12);   // Reporting mode

        const bool retval = sendCommand(SONAR_PORT, 4, SONAR_RUN, 13);
        pthread_mutex_unlock(&m_write_mutex);

        return retval;
}


bool RFlex::sonarOff() {
        pthread_mutex_lock(&m_write_mutex);

        memset(m_write_buffer + PACKET_DATA_START_BYTE, 0, 13);

        const bool retval = sendCommand(SONAR_PORT, 4, SONAR_RUN, 13);
        pthread_mutex_unlock(&m_write_mutex);

        return retval;
}


bool RFlex::motorSendTranslateVelocity(const double velocity) {
       const unsigned long vel =static_cast<unsigned long>(fabs(velocity) * m_magicDistance);
       const unsigned long acceleration = static_cast<unsigned long>(1.0 * m_magicDistance);
       const unsigned long torque = static_cast<unsigned long>(1.0 * m_magicTorque);
       
       pthread_mutex_lock(&m_write_mutex);
       insertChar(0, 0);             // Translation
       insertLong(vel, 1);           // Velocity
       insertLong(acceleration, 5);  // Acceleration
       insertLong(torque, 9);        // Torque
       insertChar((velocity < 0)?(0):(1), 13);  // Direction
       
       const bool retval = sendCommand(MOTOR_PORT, 0, MOTOR_AXIS_SET_DIR, 14);
       pthread_mutex_unlock(&m_write_mutex);
       
       return retval;
}


bool RFlex::motorSendRotateVelocity(const double velocity) {
       const unsigned long vel =static_cast<unsigned long>(fabs(velocity) * m_magicAngle);
       const unsigned long acceleration = static_cast<unsigned long>(1.0 * m_magicDistance);
       const unsigned long torque = static_cast<unsigned long>(1.0 * m_magicTorque);

        pthread_mutex_lock(&m_write_mutex);
        insertChar(1, 0);             // Rotation
        insertLong(vel, 1);           // Velocity
        insertLong(acceleration, 5);  // Acceleration
        insertLong(torque, 9);        // Torque
        insertChar((velocity < 0)?(0):(1), 13);  // Direction

        const bool retval = sendCommand(MOTOR_PORT, 0, MOTOR_AXIS_SET_DIR, 14);
        pthread_mutex_unlock(&m_write_mutex);

        return retval;
}


bool RFlex::sendCommand(const unsigned char port, const unsigned char id, const unsigned char opcode, const int length) {
        assert(length + PROTOCOL_SIZE <= s_buffer_size);

        // Header
        m_write_buffer[0] = ESC;
        m_write_buffer[1] = STX;
        m_write_buffer[PACKET_PORT_BYTE] = port;
        m_write_buffer[PACKET_ID_BYTE] = id;
        m_write_buffer[PACKET_OPCODE_BYTE] = opcode;
        m_write_buffer[PACKET_SIZE_BYTE] = static_cast<unsigned char>(length);

        // Footer
        m_write_buffer[length + PACKET_DATA_START_BYTE] = computeCRC(m_write_buffer + PACKET_CRC_START, length + PACKET_CRC_OFFSET);
        m_write_buffer[length + PACKET_DATA_START_BYTE + 1] = ESC;
        m_write_buffer[length + PACKET_DATA_START_BYTE + 2] = ETX;


        return writePacket(length + 9);
}


bool RFlex::writePacket(const int length) const {
        if (m_port.fail())
                return false;

        int bytes_written = 0;

        while (bytes_written < length) {
                int n = write(m_port, m_write_buffer + bytes_written, length - bytes_written);
                if (n < 0)
                        return false;
                else
                        bytes_written += n;

                // Put in a short wait to let the rFlex controller catch up
                usleep(1000);
        }

        return true;
}


void RFlex::readPacket() {
        // If there's no packet ready, just return
        const int read_size = readData();
        if (read_size == 0)
                return;

        // Check to make sure that the packet is the correct size
        const int data_size = read_size - PROTOCOL_SIZE;
        if (m_read_buffer[PACKET_SIZE_BYTE] != data_size) {
               //debug("Error in packet size.  Expected %i, got %i\n", data_size, static_cast<int>(m_read_buffer[PACKET_SIZE_BYTE]));
                for(int i = 0; i < s_buffer_size; ++i) {
                       //                       debug("%2x ", static_cast<int>(m_read_buffer[i]));
                       //                       if (((i + 1) % 10) == 0)
                               //                               debug("\n");
                }
        }

        // Calculate the packet CRC and verify that it matches
        if (computeCRC(m_read_buffer + PACKET_CRC_START, data_size + PACKET_CRC_OFFSET) != m_read_buffer[data_size + PACKET_DATA_START_BYTE]) {
               //               debug("CRC error: Expected %i, got %i\n", static_cast<int>(m_read_buffer[read_size - PROTOCOL_SIZE + PACKET_DATA_START_BYTE]), static_cast<int>(computeCRC(m_read_buffer + PACKET_CRC_START, data_size + PACKET_CRC_OFFSET)));

                for(int i = 0; i < s_buffer_size; ++i) {
                       //                       debug("%2x ", static_cast<int>(m_read_buffer[i]));
                       //       if (((i + 1) % 10) == 0)
                       //       debug("\n");
                }

                // Eat everything up to the end of the packet
                unsigned char tdata = 0;
                while (tdata != ETX)
                        while (read(m_port, &tdata, 1) != 1);

                return;
        }

        // If we get ti here, we have a good packet.  Parse the packet, and process the contents
        switch (m_read_buffer[PACKET_PORT_BYTE]) {
        case SYSTEM_PORT:
               //               debug("System report\n");
                parseSystemReport();
                break;
        case MOTOR_PORT:
               //debug("Motor report\n");
                parseMotorReport();
                break;
        case JOYSTICK_PORT:
               //debug("Joystick report\n");
                parseJoystickReport();
                break;
        case SONAR_PORT:
               //       debug("Sonar report\n");
                parseSonarReport();
                break;
        case DIGITAL_IO_PORT:
               //debug("Digital IO report\n");
                parseDigitalIOReport();
                break;
        case IR_PORT:
               //debug("IR report\n");
                parseIRReport();
                break;
        default:
               //debug("Unknown packet type: %02x\n", static_cast<int>(m_read_buffer[PACKET_PORT_BYTE]));
                break;
        }
}


int RFlex::readData() {
        // Read one byte of of the packet.  No need to check for errors, since this will be called repeatedly.
        if (read(m_port, m_read_buffer + m_offset, 1) != 1)
                return 0;

        // Have we started a packet yet?
        if (!m_found) {
                // If the first character isn't an ESC, the packet is invalid.  Reset the offset and return.  This
                // will eat badly-formed packets.
                if (m_read_buffer[0] != ESC) {
                        m_offset = 0;
                        return 0;
                }
                if (m_offset == 0) {
                        m_offset = 1;
                        return 0;
                }

                // We have to wait for a STX to show up before it's a valid packet.  If we see an ESC, then we just
                // keep looking for an STX.  If we see something else, give up and start looking for a new packet.
                if (m_read_buffer[1] == STX) {
                        m_found = true;
                        m_offset = 2;
                        return 0;
                } else if (m_read_buffer[1] == ESC) {
                        m_offset = 1;
                        return 0;
                } else {
                        m_offset = 0;
                        return 0;
                }
        } else {
                // If the previous character was an ESC,
                if (m_read_buffer[m_offset - 1] == ESC) {
                        switch (m_read_buffer[m_offset]) {
                        case NUL:  // Skip over NULs
                                read(m_port, m_read_buffer + m_offset, 1);  // Should we be checking the return code here?
                                ++m_offset;
                                return 0;
                        case SOH:  // Ignore SOHs by deleting them
                                --m_offset;
                                return 0;
                        case ETX: // ETX ends the packet, so return the length
                                const int retval = m_offset + 1;
                                m_found = false;
                                m_offset = 0;
                                return retval;
                        };
                } else {
                        // Just increment the counter
                        ++m_offset;
                        assert(m_offset < s_buffer_size);

                        return 0;
                }
        }

        // Should never get here
        return 0;
}


void RFlex::parseSystemReport() {
        assert(m_read_buffer[PACKET_PORT_BYTE] == SYSTEM_PORT);

        const int TIMESTAMP_OFFSET = 6;
        const int BATTERY_OFFSET = 10;
        const int BRAKE_OFFSET = 14;

        const int ROW_OFFSET = 10;
        const int MESSAGE_LENGTH_OFFSET = 11;
        const int MESSAGE_BODY_OFFSET = 12;

        const long timestamp = convertLong(TIMESTAMP_OFFSET);

        switch (m_read_buffer[PACKET_OPCODE_BYTE]) {
        case SYSTEM_LCD_DUMP: {
                const int row = static_cast<int>(convertChar(ROW_OFFSET));
                const int length = static_cast<int>(convertChar(MESSAGE_LENGTH_OFFSET));

#ifndef NDEBUG
                //debug("  row: %i\n  message (%i): ", row, length);
                for(int i = 0; i < length; ++i)
                       //               debug("%c", m_read_buffer[MESSAGE_BODY_OFFSET + i]);
                       //               debug("\n");
#endif
                break;
        }
        case SYSTEM_STATUS:
                m_battery_voltage = static_cast<float>(convertLong(BATTERY_OFFSET)) / 100.0;
                //              debug("  battery voltage: %f\n  brake: %s\n", m_battery_voltage, ((m_brake)?("on"):("off")));
                break;
        default:
               //               debug("Unknown system opcode: %02x\n", m_read_buffer[PACKET_OPCODE_BYTE]);
                break;
        }
}


#warning Not complete yet
void RFlex::parseMotorReport() {
        assert(m_read_buffer[PACKET_PORT_BYTE] == MOTOR_PORT);

        // We're not looking at offset 6, which is rv in the old code.  What is it?
        const int TIMESTAMP_OFFSET = 10;
        const int AXIS_OFFSET = 14;
        const int POSITION_OFFSET = 15;
        const int VELOCITY_OFFSET = 19;
        const int ACCELERATION_OFFSET = 23;
        const int TORQUE_OFFSET = 27;

        const int TRANSLATION_AXIS = 0;
        const int ROTATION_AXIS = 1;

        switch (m_read_buffer[PACKET_OPCODE_BYTE]) {
        case MOTOR_SYSTEM_REPORT: {
                const long timestamp = convertLong(TIMESTAMP_OFFSET);
                const long distance = static_cast<long>(convertLong(POSITION_OFFSET));

                switch (m_read_buffer[AXIS_OFFSET]) {
                case TRANSLATION_AXIS:
                        pthread_mutex_lock(&m_motor_mutex);
                        if(!m_last_distance_set){
                               m_last_distance=distance;
                               m_last_distance_set=true;
                        }
                        m_x += static_cast<double>(distance - m_last_distance) / m_magicDistance * cos(m_theta);
                        m_y += static_cast<double>(distance - m_last_distance) / m_magicDistance * sin(m_theta);

                        m_trans_velocity = static_cast<double>(convertLong(VELOCITY_OFFSET)) / m_magicDistance;
                        m_trans_acceleration = static_cast<double>(convertLong(ACCELERATION_OFFSET)) / m_magicDistance;
#warning Should the torque be using MAGIC_TORQUE?
                        m_trans_torque = static_cast<double>(convertLong(TORQUE_OFFSET)) / m_magicDistance;
                        m_last_distance = distance;
                        pthread_mutex_unlock(&m_motor_mutex);

                        break;
                case ROTATION_AXIS:
                        pthread_mutex_lock(&m_motor_mutex);
                        if(!m_last_angdistance_set){
                               m_last_angdistance=distance;
                               m_last_angdistance_set=true;
                        }
                        //                      m_theta = fmod(static_cast<double>(distance) / m_magicAngle, 2.0 * M_PI) - M_PI;
                        m_theta += static_cast<double>(distance-m_last_angdistance) / m_magicAngle;

                        m_rot_velocity = static_cast<double>(convertLong(VELOCITY_OFFSET)) / m_magicAngle;
                        m_rot_acceleration = static_cast<double>(convertLong(ACCELERATION_OFFSET)) / m_magicAngle;
#warning Should the torque be using MAGIC_TORQUE?
                        m_rot_torque = static_cast<double>(convertLong(TORQUE_OFFSET)) / m_magicAngle;
                        m_last_angdistance=distance;
                        pthread_mutex_unlock(&m_motor_mutex);
                        break;
                default:
                       //debug("Unknown motor axis: %02x\n", m_read_buffer[AXIS_OFFSET]);
                        break;
                };
                break;
        }
        case MOTOR_AXIS_SET_DIR:
                // Return value from a write
#warning Should we keep a total of requests and acks, to make sure that theyre more-or-less balanced?
                break;
        default:
               //       debug("Unknown motor opcode: %02x\n", m_read_buffer[PACKET_OPCODE_BYTE]);
                break;
        }
}


#warning Not complete yet
void RFlex::parseJoystickReport() {
        assert(m_read_buffer[PACKET_PORT_BYTE] == JOYSTICK_PORT);

        const int TIMESTAMP_OFFSET = 6;
        const int X_OFFSET = 10;
        const int Y_OFFSET = 14;
        const int BUTTON_OFFSET = 18;

        switch (m_read_buffer[PACKET_OPCODE_BYTE]) {
        case JOYSTICK_GET_STATE: {
                const long timestamp = convertLong(TIMESTAMP_OFFSET);
                const long x = convertLong(X_OFFSET);
                const long y = convertLong(Y_OFFSET);
                const unsigned char buttons = convertChar(BUTTON_OFFSET);

                //debug("  position: (%li, %li)\n  buttons: %02x\n", x, y, buttons);

#warning Need to update stored values
                break;
                }
        default:
               //               debug("Unknown joystick opcode: %02x\n", m_read_buffer[PACKET_OPCODE_BYTE]);
                break;
        }
}


#warning Not complete yet
void RFlex::parseSonarReport() {
#warning Need to implement this
        assert(m_read_buffer[PACKET_PORT_BYTE] == SONAR_PORT);

        const int DATA_LENGTH_OFFSET = 5;
        const int VALUE_OFFSET = 6;
        const int TIMESTAMP_OFFSET = 10;
        const int DATA_OFFSET = 14;

        switch (m_read_buffer[PACKET_OPCODE_BYTE]) {
        case SONAR_REPORT: 
               const unsigned char data_length = convertChar(DATA_LENGTH_OFFSET);
               const long value = convertLong(VALUE_OFFSET);
               const long timestamp = convertLong(TIMESTAMP_OFFSET);
               
               for(int i = 0; i < (data_length - 8) / 3; ++i) {
                      const int sonar_id = convertChar(DATA_OFFSET + (3 * i));
                      const int sonar_value = convertShort(DATA_OFFSET + (3 * i) + 1);
                      if (sonar_value < 500){
                             //not the prettiest way to do this
                             if(m_type==RFLEX_ATRVJR){
                                    switch(sonar_id){
                                    case 0x22:
                                           m_sonars[0]=sonar_value;
                                           break;
                                    case 0x23:
                                           m_sonars[1]=sonar_value;
                                           break;
                                    case 0x24:
                                           m_sonars[2]=sonar_value;
                                           break;
                                    case 0x25:
                                           m_sonars[3]=sonar_value;
                                           break;
                                    case 0x14:
                                           m_sonars[4]=sonar_value;
                                           break;
                                    case 0x13:
                                           m_sonars[5]=sonar_value;
                                           break;
                                    case 0x12:
                                           m_sonars[6]=sonar_value;
                                           break;
                                    case 0x11:
                                           m_sonars[7]=sonar_value;
                                           break;
                                    case 0x10:
                                           m_sonars[8]=sonar_value;
                                           break;
                                    case 0x05:
                                           m_sonars[9]=sonar_value;
                                           break;
                                    case 0x04:
                                           m_sonars[10]=sonar_value;
                                           break;
                                    case 0x03:
                                           m_sonars[11]=sonar_value;
                                           break;
                                    case 0x02:
                                           m_sonars[12]=sonar_value;
                                           break;
                                    case 0x01:
                                           m_sonars[13]=sonar_value;
                                           break;
                                    case 0x00:
                                           m_sonars[14]=sonar_value;
                                           break;
                                    case 0x20:
                                           m_sonars[15]=sonar_value;
                                           break;
                                    case 0x21:
                                           m_sonars[16]=sonar_value;
                                           break;
                                    }
                             }
                      }
                             cerr << "Sonar " << sonar_id << " value: " << sonar_value << endl;
                      //                               debug("Sonar %02x: %i\n", sonar_id, sonar_value);
               }
               
               break;
               
               //       default:
               //break;
               //               debug("Unknown sonar report opcode\n");
        }
}


#warning Not complete yet
void RFlex::parseDigitalIOReport() {
#warning Need to implement this
        assert(m_read_buffer[PACKET_PORT_BYTE] == DIGITAL_IO_PORT);
}


#warning Not complete yet
void RFlex::parseIRReport() {
#warning Need to implement this
        assert(m_read_buffer[PACKET_PORT_BYTE] == IR_PORT);
}


unsigned char RFlex::convertChar(const int offset) const {
        assert((offset >= 0) && (offset < s_buffer_size - 1));

        unsigned char n;
        memcpy(&n, m_read_buffer + offset, sizeof(unsigned char));
        return n;
}


unsigned short RFlex::convertShort(const int offset) const {
        assert((offset >= 0) && (offset < s_buffer_size - 2));

  unsigned short n;
  memcpy(&n, m_read_buffer + offset, sizeof(unsigned short));
  return htons(n);
}


unsigned long RFlex::convertLong(const int offset) const {
        assert((offset >= 0) && (offset < s_buffer_size - 4));

  unsigned long n;
  memcpy(&n, m_read_buffer + offset, sizeof(unsigned long));
  return htonl(n);
}


void RFlex::insertChar(const unsigned char value, const int offset) {
        assert(sizeof(unsigned char) == 1);
        assert((offset >= 0) && (offset < s_buffer_size - PACKET_DATA_START_BYTE - 1));

        memcpy(m_write_buffer + PACKET_DATA_START_BYTE + offset, &value, sizeof(unsigned char));
}


void RFlex::insertLong(const long value, const int offset) {
        assert(sizeof(unsigned long) == 4);
        assert((offset >= 0) && (offset < s_buffer_size - PACKET_DATA_START_BYTE - 4));

        unsigned long v = htonl(value);
        memcpy(m_write_buffer + PACKET_DATA_START_BYTE + offset, &v, sizeof(unsigned long));
}


unsigned char RFlex::computeCRC(const unsigned char *buffer, const int n) {
        assert(buffer != NULL);
  assert(n > 0);

        int crc =buffer[0];
        for(int i = 1; i < n; ++i)
                crc ^= buffer[i];

        return crc;
}


void *RFlex::rFlexReadThread(void *ptr) {
        RFlex *rflex = static_cast<RFlex *>(ptr);

        while (true) {
                // Set up the read set to include the serial port
                fd_set read_set;
                FD_ZERO(&read_set);
                FD_SET(rflex->m_port, &read_set);

                // Is there any new data to be read from the rFlex?
                if (select(rflex->m_port + 1, &read_set, NULL, NULL, NULL) < 0) {
                       //debug("Error in select\n");
                } else if (FD_ISSET(rflex->m_port, &read_set)) {
                        rflex->readPacket();
                }
        }
}


void *RFlex::rFlexPeriodicThread(void *ptr) {
        RFlex *rflex = static_cast<RFlex *>(ptr);

        int counter = 0;
        while (true) {
                // 100 Hz

                // 20 Hz
                if ((counter % 5) == 0) {
                }

                // 10 Hz
                if ((counter % 10) == 0) {
                        // This limits our control frequency to 10Hz.  If we want immediate control, then we need to
                        // add an active send to the velocity-setting commands.
                        pthread_mutex_lock(&(rflex->m_command_mutex));
                        rflex->motorSendTranslateVelocity(rflex->m_translate_command);
                        rflex->motorSendRotateVelocity(rflex->m_rotate_command);
                        pthread_mutex_unlock(&(rflex->m_command_mutex));

                        rflex->systemUpdate();
                        //rflex->joystickReport();
                }

                // 5 Hz
                if ((counter % 20) == 0) {
                }

                // 1 Hz
                if (counter == 0) {
                }

                // Maximum control frequency is 100 Hz
                usleep(10000);

                counter = (counter + 1) % 100;
        }
}

---