Benson Sin
CityU Dream Development
Dependencies: mbed ros_lib_melodic
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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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