Diff: MotorDrive.cpp

Revision:

4:aa8ef06b9469

diff -r a1a69ae50c18 -r aa8ef06b9469 MotorDrive.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorDrive.cpp	Sat Jan 30 12:05:58 2021 +0000
@@ -0,0 +1,71 @@
+// 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;
+}