John Lam / Mbed 2 deprecated rosserial_mbed_carbase

carbase

Dependencies:   mbed mbed-rtos ros_lib_melodic

MotorDrive.cpp@4:aa8ef06b9469, 2021-01-30 (annotated)

Committer:
howanglam3
Date:
Sat Jan 30 12:05:58 2021 +0000
Revision:
4:aa8ef06b9469
updated

Who changed what in which revision?

UserRevisionLine numberNew contents of line
howanglam3 4:aa8ef06b9469 1 // Mbed Library
howanglam3 4:aa8ef06b9469 2 #include <cmath>
howanglam3 4:aa8ef06b9469 3 #include <vector>
howanglam3 4:aa8ef06b9469 4 #include "math.h"
howanglam3 4:aa8ef06b9469 5 // ROS Library
howanglam3 4:aa8ef06b9469 6 #include <ros.h>
howanglam3 4:aa8ef06b9469 7 #include "std_msgs/MultiArrayLayout.h"
howanglam3 4:aa8ef06b9469 8 #include "std_msgs/MultiArrayDimension.h"
howanglam3 4:aa8ef06b9469 9 // Header file
howanglam3 4:aa8ef06b9469 10 #include "MotorDrive.h"
howanglam3 4:aa8ef06b9469 11
howanglam3 4:aa8ef06b9469 12 #define M_PI 3.14159265358979323846f
howanglam3 4:aa8ef06b9469 13
howanglam3 4:aa8ef06b9469 14 using namespace std;
howanglam3 4:aa8ef06b9469 15 using namespace ros;
howanglam3 4:aa8ef06b9469 16
howanglam3 4:aa8ef06b9469 17 // Constructor
howanglam3 4:aa8ef06b9469 18 MotorDrive::MotorDrive() {
howanglam3 4:aa8ef06b9469 19 this->offset_x = 0.0;
howanglam3 4:aa8ef06b9469 20 this->offset_y = 0.125;
howanglam3 4:aa8ef06b9469 21 this->pwm_coeff = 0.4 / max_rpm;
howanglam3 4:aa8ef06b9469 22 this->wheel_radius_inv = 1 / this->wheel_radius;
howanglam3 4:aa8ef06b9469 23
howanglam3 4:aa8ef06b9469 24 // Jacobian matrix
howanglam3 4:aa8ef06b9469 25 // -> Initialize
howanglam3 4:aa8ef06b9469 26 this->jacobian_matrix.resize(this->wheel_num);
howanglam3 4:aa8ef06b9469 27 for(int i = 0; i < this->wheel_num; i++)
howanglam3 4:aa8ef06b9469 28 this->jacobian_matrix[i].resize(3);
howanglam3 4:aa8ef06b9469 29
howanglam3 4:aa8ef06b9469 30 // -> Calculate carbase information
howanglam3 4:aa8ef06b9469 31 for(int i = 0; i < this->wheel_num; i++) {
howanglam3 4:aa8ef06b9469 32 double wheel_angle = ((360.0f / this->wheel_num) * i + offset_angle) * M_PI / 180;
howanglam3 4:aa8ef06b9469 33
howanglam3 4:aa8ef06b9469 34 this->jacobian_matrix[i][0] = -sin(wheel_angle);
howanglam3 4:aa8ef06b9469 35 this->jacobian_matrix[i][1] = cos(wheel_angle);
howanglam3 4:aa8ef06b9469 36 this->jacobian_matrix[i][2] = cos(atan((this->offset_y - (this->robot_radius * this->jacobian_matrix[i][1] / (this->robot_radius * sin(wheel_angle))))));
howanglam3 4:aa8ef06b9469 37 }
howanglam3 4:aa8ef06b9469 38 this->jacobian_matrix[0][2] = this->offset_y / this->robot_radius + 1;
howanglam3 4:aa8ef06b9469 39
howanglam3 4:aa8ef06b9469 40 this->omniWheel.setWheelNum(this->wheel_num);
howanglam3 4:aa8ef06b9469 41 }
howanglam3 4:aa8ef06b9469 42
howanglam3 4:aa8ef06b9469 43 // Destructor
howanglam3 4:aa8ef06b9469 44 MotorDrive::~MotorDrive() {
howanglam3 4:aa8ef06b9469 45 }
howanglam3 4:aa8ef06b9469 46
howanglam3 4:aa8ef06b9469 47 // Function
howanglam3 4:aa8ef06b9469 48 OmniWheel MotorDrive::getOmniWheelData(double linear_x, double linear_y, double angular_z) {
howanglam3 4:aa8ef06b9469 49 // Clear orginal data
howanglam3 4:aa8ef06b9469 50 this->omniWheel.clear();
howanglam3 4:aa8ef06b9469 51
howanglam3 4:aa8ef06b9469 52 double motor_vel = 0.0;
howanglam3 4:aa8ef06b9469 53 double normalization_factor = 1.0;
howanglam3 4:aa8ef06b9469 54 double rpm = 0.0;
howanglam3 4:aa8ef06b9469 55 double rpm_normalize = 0.0;
howanglam3 4:aa8ef06b9469 56
howanglam3 4:aa8ef06b9469 57 // Calculate Each Motor PWM
howanglam3 4:aa8ef06b9469 58 for(int i = 0; i < this->wheel_num; i++) {
howanglam3 4:aa8ef06b9469 59 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;
howanglam3 4:aa8ef06b9469 60 rpm_normalize = motor_vel * this->wheel_radius_inv * this->rads_to_rpm;
howanglam3 4:aa8ef06b9469 61
howanglam3 4:aa8ef06b9469 62 if(rpm_normalize > this->max_rpm)
howanglam3 4:aa8ef06b9469 63 normalization_factor = this->max_rpm / rpm_normalize;
howanglam3 4:aa8ef06b9469 64
howanglam3 4:aa8ef06b9469 65 rpm = 0.5f + rpm_normalize * normalization_factor * this->pwm_coeff;
howanglam3 4:aa8ef06b9469 66
howanglam3 4:aa8ef06b9469 67 this->omniWheel.setPwm(rpm > this->max_rpm ? this->max_rpm : rpm);
howanglam3 4:aa8ef06b9469 68 }
howanglam3 4:aa8ef06b9469 69
howanglam3 4:aa8ef06b9469 70 return this->omniWheel;
howanglam3 4:aa8ef06b9469 71 }