Revision 0:c2b6f8b48076, committed 2021-02-25

Comitter:

bensonsinsin998

Date:

Thu Feb 25 07:41:29 2021 +0000

Commit message:

hi

Changed in this revision

diff -r 000000000000 -r c2b6f8b48076 MotorDrive.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorDrive.cpp	Thu Feb 25 07:41:29 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;
+}
\ No newline at end of file

diff -r 000000000000 -r c2b6f8b48076 MotorDrive.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorDrive.h	Thu Feb 25 07:41:29 2021 +0000
@@ -0,0 +1,39 @@
+#ifndef MOTOR_DRIVE
+#define MOTOR_DRIVE
+
+// Mbed Library
+#include <cmath>
+#include <vector>
+#include "OmniWheel.h"
+
+#define M_PI 3.14159265358979323846f
+
+using namespace std;
+
+class MotorDrive {
+    private:
+        const static double max_rpm = 5000;
+        const static double rads_to_rpm = 30.0 / M_PI;
+        const static double robot_radius = 0.19;
+        const static double wheel_radius = 0.05;
+        const static int wheel_num = 3;
+        
+        double offset_angle;
+        double offset_x;
+        double offset_y;
+        double pwm_coeff;
+        double wheel_radius_inv;
+        
+        OmniWheel omniWheel;
+        
+        vector<vector<double> > jacobian_matrix;
+        
+    public:
+        // Initialize MotorDrive
+        MotorDrive();
+        virtual ~MotorDrive();
+        // Function
+        OmniWheel getOmniWheelData(double linear_x, double linear_y, double angular_z);
+};
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r c2b6f8b48076 OmniWheel.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/OmniWheel.cpp	Thu Feb 25 07:41:29 2021 +0000
@@ -0,0 +1,55 @@
+// Mbed Library
+#include <vector>
+// Header File
+#include "OmniWheel.h"
+
+using namespace std;
+
+// Constructor
+OmniWheel::OmniWheel() {
+}
+
+// Destructor
+OmniWheel::~OmniWheel() {
+}
+
+// Function
+void OmniWheel::clear() {
+    this->vel.clear();
+    this->rpm.clear();
+    this->pwm.clear();
+}
+
+// -> Getter
+int OmniWheel::getWheelNum() {
+    return this->wheel_num;
+}
+
+vector<double> OmniWheel::getVel() {
+    return this->vel;
+}
+
+vector<double> OmniWheel::getRpm() {
+    return this->rpm;
+}
+
+vector<double> OmniWheel::getPwm() {
+    return this->pwm;
+}
+
+// -> Setter
+void OmniWheel::setWheelNum(int _wheel_num) {
+    this->wheel_num = _wheel_num;
+}
+
+void OmniWheel::setVel(double _vel) {
+    this->vel.push_back(_vel);
+}
+
+void OmniWheel::setRpm(double _rpm) {
+    this->rpm.push_back(_rpm);
+}
+
+void OmniWheel::setPwm(double _pwm) {
+    this->pwm.push_back(_pwm);
+}
\ No newline at end of file

diff -r 000000000000 -r c2b6f8b48076 OmniWheel.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/OmniWheel.h	Thu Feb 25 07:41:29 2021 +0000
@@ -0,0 +1,35 @@
+#ifndef OMNI_WHEEL
+#define OMNI_WHEEL
+
+// Mbed Library
+#include <vector>
+
+using namespace std;
+
+class OmniWheel {
+    private:
+        int wheel_num;
+        
+        vector<double> vel;
+        vector<double> rpm;
+        vector<double> pwm;
+    
+    public:
+        // Initialize Omniwheel
+        OmniWheel();
+        virtual ~OmniWheel();
+        // Function
+        void clear();
+        // -> Getter
+        int getWheelNum();
+        vector<double> getVel();
+        vector<double> getRpm();
+        vector<double> getPwm();
+        // -> Setter
+        void setWheelNum(int _wheel_num);
+        void setVel(double _vel);
+        void setRpm(double _rpm);
+        void setPwm(double _pwm);
+};
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r c2b6f8b48076 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Feb 25 07:41:29 2021 +0000
@@ -0,0 +1,121 @@
+/*
+    CityU Robocon Dream Development - 2021 TR Carbase
+*/
+
+// Mbed Library
+#include "mbed.h"
+#include "math.h"
+// ROS Library
+#include <ros.h>
+#include "geometry_msgs/Twist.h"
+#include "std_msgs/Bool.h"
+#include "std_msgs/Float32.h"
+#include "std_msgs/Float64MultiArray.h"
+#include "std_msgs/Int32.h"
+#include "std_msgs/MultiArrayLayout.h"
+#include "std_msgs/MultiArrayDimension.h"
+// Header File
+#include "MotorDrive.h"
+#include "OmniWheel.h"
+
+// ROS Connect Baud Rate 
+#define BAUD_RATE 115200
+
+using namespace std;
+using namespace ros;
+using namespace std_msgs;
+using namespace geometry_msgs;
+
+// Mbed Pin
+DigitalOut led = LED1;
+// -> 3.3V Reference Pin
+DigitalOut reference_voltage_3_3 = PB_1;
+// -> Motor Pin
+DigitalOut motor_enable = PC_7;
+DigitalOut motor_stop[] = {PB_6, PC_5, PC_0};
+// -> Motor PWN Pin
+PwmOut motor_pwm[] = {PA_9, PC_8, PA_11};
+
+// Constant Variable
+// -> Motor
+const int motor_num = sizeof(motor_pwm) / sizeof(PwmOut);
+// -> Motor -> PWM Wait Period
+const int pwm_period_ms = 20;
+// -> Motor -> Stop PWM
+const double stop_pwm = 0.5;
+
+// ROS Variable
+// -> Node Handler
+NodeHandle nh;
+// -> ROS Msessage
+Float64MultiArray motor_pwm_msg;
+Twist motor_command_msg;
+
+MotorDrive motor_drive;
+
+// Function
+// -> Motor
+// -> Motor -> Intialize
+void motorInit() {
+    // Initialize motor
+    motor_enable = 0;
+    for(int i = 0; i < motor_num; i++) {
+        motor_pwm[i].period_ms(pwm_period_ms);
+        motor_pwm[i] = stop_pwm;
+    }
+    
+    // Set Reference Voltage
+    reference_voltage_3_3 = 1;
+    wait(1);    // Wait 1s For Voltage
+    
+    // Enable Motor
+    motor_enable = 1;
+}
+
+// -> Motor -> Send PWM
+void motorMoveCallback(int _motor_index, double _pwm) {
+    motor_stop[_motor_index] = 0;
+    motor_pwm[_motor_index] = _pwm;
+}
+
+void motorStopCallback(int _motor_index) {
+    motor_stop[_motor_index] = 1;
+    motor_pwm[_motor_index] = 0.5;
+}
+
+// -> ROS Subscurber 
+void velCallback(const Twist& _msg) {
+    OmniWheel omni_wheel_data = motor_drive.getOmniWheelData(_msg.linear.x, _msg.linear.y, _msg.angular.z);
+    
+    vector<double> omni_wheel_pwm = omni_wheel_data.getPwm();   // Get PWM data
+    
+    for(int i = 0; i < motor_num; i++)      // Send PWM to each motor
+        if(omni_wheel_pwm[i] != stop_pwm)
+            motorMoveCallback(i, omni_wheel_pwm[i]);
+        else
+            motorStopCallback(i);
+}
+Subscriber<Twist> sub_motor_pwm("base_twist", &velCallback);
+
+// Main
+int main() {
+    // ROS Setup
+    // -> Set Baud Rate
+    nh.getHardware()->setBaud(BAUD_RATE);
+    // -> Initialize ROS Node
+    nh.initNode();
+    nh.subscribe(sub_motor_pwm);    // Subscribe the callback function
+    
+    // Initialize motor
+    motorInit();
+    
+    // Main programming
+    while(true) {
+        nh.spinOnce();
+
+        if(nh.connected())
+            led = 1;    // Turn on the LED if the ROS successfully connect with the Jetson
+        else 
+            led = 0;    // Turn off the LED if the ROS cannot connect with the Jetson
+    }
+}
\ No newline at end of file

diff -r 000000000000 -r c2b6f8b48076 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Feb 25 07:41:29 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file

diff -r 000000000000 -r c2b6f8b48076 ros_lib_melodic.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ros_lib_melodic.lib	Thu Feb 25 07:41:29 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/garyservin/code/ros_lib_melodic/#da82487f547e