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

Go to the documentation of this file.

#ifndef RFlex_H
#define RFlex_H


#include <string>
#include <pthread.h>
#include <SerialPort.H>

#include <Angle.H>
//this value chosen since we're only mucking with atrv sonar for now(which really only has 17)
#define MAX_SONAR 18 
#define RFLEX_B21R 0
#define RFLEX_ATRVJR 1

class RFlex {
public:
        RFlex(const std::string &filename, const std::string &robotType);
        ~RFlex();

        // System
        bool systemUpdate();
        bool systemLCDDump();

        // Motion
        bool motionSetDefaults();

        // Brake
        bool setBrake();
        bool releaseBrake();

        // Motors
        void motorSetTranslateVelocity(const double velocity);
        void motorSetRotateVelocity(const double velocity);
        void motorSetVelocity(const double translate, const double rotate);

        // Odometry
        bool odometryOn(const int period = 100);
        bool odometryOff();

        // Digital IO
        bool digitalIOOn(const int period = 100);
        bool digitalIOOff();

        // Joystick
        bool joystickReport();

        // Sonar
        bool sonarOn();
        bool sonarOff();
       int getNumSonars(){ return m_numSonar;}
       double getSonar(int i) { if(i<m_numSonar){ return m_sonars[i]; } else { return 0; }}

        // Battery
        float batteryVoltage() {return m_battery_voltage;}

        // Brake
        bool brakeOn() {return m_brake;}
        bool brakeOff() {return !brakeOn();}

        // Odometry
        double x() {return lockedValue(m_x, m_motor_mutex);}
        double y() {return lockedValue(m_y, m_motor_mutex);}
        double heading() {return lockedValue(m_theta, m_motor_mutex);}

        // Movement
        double translateVelocity() {return lockedValue(m_trans_velocity, m_motor_mutex);}
        double translateAcceleration() {return lockedValue(m_trans_acceleration, m_motor_mutex);}
        double translateTorque() {return lockedValue(m_trans_torque, m_motor_mutex);}

        double rotateVelocity() {return lockedValue(m_rot_velocity, m_motor_mutex);}
        double rotateAcceleration() {return lockedValue(m_rot_acceleration, m_motor_mutex);}
        double rotateTorque() {return lockedValue(m_rot_torque, m_motor_mutex);}

       void zero(){m_x=0; m_y=0; m_theta = 0; m_last_distance_set=false; m_last_angdistance_set=false;}

private:
       SerialPort m_port;

       unsigned char m_type;

       // constants for translating real values into rflex values and back
       double m_magicDistance;
       double m_magicAngle;
       double m_magicTorque;

       double m_sonars[MAX_SONAR];
       int m_numSonar;
       
        // Housekeeping
        static const int s_buffer_size = 1024;
        bool m_found;
        int m_offset;
        unsigned char m_read_buffer[s_buffer_size];
        unsigned char m_write_buffer[s_buffer_size];

        // Some synchronization
        pthread_mutex_t m_write_mutex;
        pthread_mutex_t m_motor_mutex;
        pthread_mutex_t m_command_mutex;

        // Threads for reading and periodic requests
        pthread_t m_read_thread;
        pthread_t m_periodic_thread;

        // Current motion parameters
        double m_translate_command;
        double m_rotate_command;

        // System reports
        float m_battery_voltage;
        bool m_brake;

        // Motor reports
        double m_x;
        double m_y;
        Angle m_theta;

        double m_trans_velocity;
        double m_trans_acceleration;
        double m_trans_torque;

        double m_rot_velocity;
        double m_rot_acceleration;
        double m_rot_torque;

       long m_last_distance;
       long m_last_angdistance;
       bool m_last_distance_set;
       bool m_last_angdistance_set;

        bool motorSendTranslateVelocity(const double velocity);
        bool motorSendRotateVelocity(const double velocity);

        bool sendCommand(const unsigned char port, const unsigned char id, const unsigned char opcode, const int length);
        bool writePacket(const int size) const;

        void readPacket();
        int readData();

        void parseSystemReport();
        void parseMotorReport();
        void parseJoystickReport();
        void parseSonarReport();
        void parseDigitalIOReport();
        void parseIRReport();

        unsigned char convertChar(const int offset) const;
        unsigned short convertShort(const int offset) const;
        unsigned long convertLong(const int offset) const;
        void insertChar(const unsigned char value, const int offset);
        void insertLong(const long value, const int offset);

        static unsigned char computeCRC(const unsigned char *buffer, const int n);

        static void *rFlexReadThread(void *ptr);
        static void *rFlexPeriodicThread(void *ptr);

        template <typename T> T lockedValue(T &value, pthread_mutex_t &mutex) {
                pthread_mutex_lock(&mutex);
                T retval = value;
                pthread_mutex_unlock(&mutex);
                return retval;
        }

        // Not allowed to use these
        RFlex(const RFlex &rflex);
        RFlex &operator=(const RFlex &rflex);
};


#endif

---