Diff: main.cpp

Revision:

0:c2b6f8b48076

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