Benson Sin / Mbed 2 deprecated Robocon_2021_carbase

CityU Dream Development

Dependencies:   mbed ros_lib_melodic

MotorDrive.cpp@0:c2b6f8b48076, 2021-02-25 (annotated)

Committer:
bensonsinsin998
Date:
Thu Feb 25 07:41:29 2021 +0000
Revision:
0:c2b6f8b48076
hi

Who changed what in which revision?

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