Revision 9:bfd88f814eec, committed 2021-02-25

Comitter:

howanglam3

Date:

Thu Feb 25 05:54:25 2021 +0000

Parent:

8:e542adce95e9

Commit message:

finish thread

Changed in this revision

--- a/main.cpp	Thu Feb 18 10:15:52 2021 +0000
+++ b/main.cpp	Thu Feb 25 05:54:25 2021 +0000
@@ -13,16 +13,23 @@
 #include "MotorDrive.h"
 #include "OmniWheel.h"
 
-ros::NodeHandle nh;
+using namespace ros;
+using namespace std_msgs;
+using namespace geometry_msgs;
+
+
+NodeHandle nh;
 
 DigitalOut myled(LED1);
 DigitalOut Refer_Volt_3_3 = PB_1;
 DigitalOut motor_enable = PC_7;
 DigitalOut motor_stop[] = {PB_6, PC_5, PC_0};
 
+
 // thread handling wheel
-geometry_msgs::Twist wheel_data; //global
+Twist wheel_data; //global
 Thread wheel_calc;
+Twist save;
 
 // -> Motor PWN Pin
 PwmOut motor_pwm[] = {PA_9, PC_8, PA_11};
@@ -61,7 +68,7 @@
     float value = (int)(x*100+0.5);
     return (float)value/100;
     }
-void baseCallback(const std_msgs::Float32MultiArray& base_msg){
+void baseCallback(const Float32MultiArray& base_msg){
     double x = (double)round(base_msg.data[1]);
     double y = (double)round(base_msg.data[0]);
     double z = (double)round(base_msg.data[2]);
@@ -90,11 +97,11 @@
         motorMove(2, omni_wheel_pwm[2]);
 }
 
-void messageCb(const std_msgs::Float32MultiArray& toggle_msg){
+void messageCb(const Float32MultiArray& toggle_msg){
     myled = !myled;   // blink the led
 }
 
-void twCallback(const geometry_msgs::Twist& _msg) {
+void twCallback(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
@@ -108,21 +115,31 @@
 
 void wheel_calc_thread() {
     while (true) {
-        // calc
-        delay_for(5);
+        
+bool x = wheel_data.linear.x==save.linear.x;
+bool y = wheel_data.linear.y==save.linear.y;
+bool z = wheel_data.angular.z==save.angular.z;
+
+// calc
+        if(!(x && y && z))
+        {
+            twCallback(wheel_data);
+            wait(5);
+        }
+        save = wheel_data;
     }
     
 }
 
-void saveCallback(const geometry_msgs::Twist& _msg) {
+void saveCallback(const Twist& _msg) {
     wheel_data = _msg;
 }
 
-ros::Subscriber<std_msgs::Float32MultiArray> subax("moving_base", &baseCallback);
-ros::Subscriber<std_msgs::Float32MultiArray> subbt("fcn_button", &messageCb);
-ros::Subscriber<geometry_msgs::Twist> subtw("base_twist", &saveCallback);
+Subscriber<Float32MultiArray> subax("moving_base", &baseCallback);
+Subscriber<Float32MultiArray> subbt("fcn_button", &messageCb);
+Subscriber<Twist> subtw("base_twist", &saveCallback);
 int main() {
-    //ros::Rate rate(10);
+    //Rate rate(10);
     nh.getHardware()->setBaud(BAUD);
     nh.initNode();
     wheel_calc.start(wheel_calc_thread);

--- a/mbed-os.lib	Thu Feb 18 10:15:52 2021 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/users/bLandais/code/mbed-os/#4c0e0edd4545

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Thu Feb 25 05:54:25 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed-rtos/#5713cbbdb706