MotorDrive.cpp

00001 // Mbed Library
00002 #include <cmath>
00003 #include <vector>
00004 #include "math.h"
00005 // ROS Library
00006 #include <ros.h>
00007 #include "std_msgs/MultiArrayLayout.h"
00008 #include "std_msgs/MultiArrayDimension.h"
00009 // Header file
00010 #include "MotorDrive.h"
00011 
00012 #define M_PI 3.14159265358979323846f
00013 
00014 using namespace std;
00015 using namespace ros;
00016 
00017 // Constructor
00018 MotorDrive::MotorDrive() {
00019     this->offset_x = 0.0;
00020     this->offset_y = 0.125;
00021     this->pwm_coeff = 0.4 / max_rpm;
00022     this->wheel_radius_inv = 1 / this->wheel_radius;
00023     
00024     // Jacobian matrix
00025     // -> Initialize
00026     this->jacobian_matrix.resize(this->wheel_num);
00027     for(int i = 0; i < this->wheel_num; i++)
00028         this->jacobian_matrix[i].resize(3);
00029         
00030     // -> Calculate carbase information
00031     for(int i = 0; i < this->wheel_num; i++) {
00032         double wheel_angle = ((360.0f / this->wheel_num) * i + offset_angle) * M_PI / 180;
00033         
00034         this->jacobian_matrix[i][0] = -sin(wheel_angle);
00035         this->jacobian_matrix[i][1] = cos(wheel_angle);
00036         this->jacobian_matrix[i][2] = cos(atan((this->offset_y - (this->robot_radius * this->jacobian_matrix[i][1] / (this->robot_radius * sin(wheel_angle))))));
00037     }
00038     this->jacobian_matrix[0][2] = this->offset_y / this->robot_radius + 1;
00039     
00040     this->omniWheel.setWheelNum(this->wheel_num);
00041 }
00042 
00043 // Destructor
00044 MotorDrive::~MotorDrive() {
00045 }
00046 
00047 // Function
00048 OmniWheel MotorDrive::getOmniWheelData(double linear_x, double linear_y, double angular_z) {
00049     // Clear orginal data
00050     this->omniWheel.clear();
00051     
00052     double motor_vel = 0.0;
00053     double normalization_factor = 1.0;
00054     double rpm = 0.0;
00055     double rpm_normalize = 0.0;
00056     
00057     // Calculate Each Motor PWM
00058     for(int i = 0; i < this->wheel_num; i++) {
00059         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;
00060         rpm_normalize = motor_vel * this->wheel_radius_inv * this->rads_to_rpm;
00061         
00062         if(rpm_normalize > this->max_rpm)
00063             normalization_factor = this->max_rpm / rpm_normalize;
00064             
00065         rpm = 0.5f + rpm_normalize * normalization_factor * this->pwm_coeff;
00066         
00067         this->omniWheel.setPwm(rpm > this->max_rpm ? this->max_rpm : rpm);
00068     }
00069     
00070     return this->omniWheel;
00071 }