base_motor_driver.h

Go to the documentation of this file.

#ifndef __BASE_MOTOR_DRIVER_H__
#define __BASE_MOTOR_DRIVER_H__

#include <fcntl.h>      // open modes devices
#include <cmath>
#include <cstring>
#include <sys/sem.h>    // IPC semaphores
#include <termios.h>    // struct termios
#include <ros/ros.h>
#include "maggie_data.h"

// device names
//TODO: convert this to rosparam
#define DEVICE_NAME1    "/dev/motor_base2"
#define DEVICE_NAME2    "/dev/motor_base1"

// semaphore name
#define SEM_FILENAME    "/tmp/sem"
key_t SEMFDID;

// baud rate
#define BAUDRATE        B19200

// period between samples in seconds
#define PERIOD          0.060

// length for GST command response
#define SIZE_REPORT_GST 8

typedef struct {
        double v;       // linear velocity (mm / sec)
        double w;       // angular velocity (degrees / sec)
        double x;       // x position (mm)
        double y;       // y position (mm)
        double theta;   // angle (degrees)
} cinematic_data;

class BaseMotor {
    public:
        BaseMotor();

        ~BaseMotor();

        int init_communication();

        int end_communication();

        int enable_motors();

        int disable_motors();

        int enable_disable_motor(int *fd, const char *order);

        int read_data(cinematic_data *data);

        int read_data_variable_time_diff(cinematic_data *data, double time_since_last_call);

        int set_velocity(double v, double w);

        int set_displacement_velocity(double d, double v, double w);

        int move_ahead(double distance);

        int relative_turn(double theta);

        int reset_odometry();

        int update_odometry(double x, double y, double theta);

        cinematic_data * get_estimate_position();

        int set_estimate_position(double x, double y, double theta);

        std::string motor_order;

    private:
        // file descriptors
        int _fd_motor1, _fd_motor2;

        // motors odometry
        double _distance1, _distance2, _before_distance1, _before_distance2;
        cinematic_data _current_cinematic, _last_cinematic;

        // aggregated variable for estimated position
        int _corrected_data;
        cinematic_data _current_estimated, _before_estimated;
};

#endif