![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |