File content as of revision 4:aa8ef06b9469:

// Mbed Library
#include <cmath>
#include <vector>
#include "math.h"
// ROS Library
#include <ros.h>
#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"
// Header file
#include "MotorDrive.h"

#define M_PI 3.14159265358979323846f

using namespace std;
using namespace ros;

// Constructor
MotorDrive::MotorDrive() {
    this->offset_x = 0.0;
    this->offset_y = 0.125;
    this->pwm_coeff = 0.4 / max_rpm;
    this->wheel_radius_inv = 1 / this->wheel_radius;
    
    // Jacobian matrix
    // -> Initialize
    this->jacobian_matrix.resize(this->wheel_num);
    for(int i = 0; i < this->wheel_num; i++)
        this->jacobian_matrix[i].resize(3);
        
    // -> Calculate carbase information
    for(int i = 0; i < this->wheel_num; i++) {
        double wheel_angle = ((360.0f / this->wheel_num) * i + offset_angle) * M_PI / 180;
        
        this->jacobian_matrix[i][0] = -sin(wheel_angle);
        this->jacobian_matrix[i][1] = cos(wheel_angle);
        this->jacobian_matrix[i][2] = cos(atan((this->offset_y - (this->robot_radius * this->jacobian_matrix[i][1] / (this->robot_radius * sin(wheel_angle))))));
    }
    this->jacobian_matrix[0][2] = this->offset_y / this->robot_radius + 1;
    
    this->omniWheel.setWheelNum(this->wheel_num);
}

// Destructor
MotorDrive::~MotorDrive() {
}

// Function
OmniWheel MotorDrive::getOmniWheelData(double linear_x, double linear_y, double angular_z) {
    // Clear orginal data
    this->omniWheel.clear();
    
    double motor_vel = 0.0;
    double normalization_factor = 1.0;
    double rpm = 0.0;
    double rpm_normalize = 0.0;
    
    // Calculate Each Motor PWM
    for(int i = 0; i < this->wheel_num; i++) {
        motor_vel = linear_x * this->jacobian_matrix[i][0] + linear_y * this->jacobian_matrix[i][1] + angular_z * this->jacobian_matrix[i][2] * 0.3;
        rpm_normalize = motor_vel * this->wheel_radius_inv * this->rads_to_rpm;
        
        if(rpm_normalize > this->max_rpm)
            normalization_factor = this->max_rpm / rpm_normalize;
            
        rpm = 0.5f + rpm_normalize * normalization_factor * this->pwm_coeff;
        
        this->omniWheel.setPwm(rpm > this->max_rpm ? this->max_rpm : rpm);
    }
    
    return this->omniWheel;
}