Diff: main.cpp

Revision:

8:e542adce95e9

Parent:

7:fe1f52d848fd

Child:

9:bfd88f814eec

--- a/main.cpp	Thu Feb 18 09:58:58 2021 +0000
+++ b/main.cpp	Thu Feb 18 10:15:52 2021 +0000
@@ -4,6 +4,7 @@
  * Blinks an LED on callback
  */
 #include "mbed.h"
+#include "rtos.h"
 #include <ros.h>
 #include <std_msgs/Float32MultiArray.h>
 #include <std_msgs/Int32MultiArray.h>
@@ -19,7 +20,9 @@
 DigitalOut motor_enable = PC_7;
 DigitalOut motor_stop[] = {PB_6, PC_5, PC_0};
 
-geometry_msgs::Twist wheel_data;
+// thread handling wheel
+geometry_msgs::Twist wheel_data; //global
+Thread wheel_calc;
 
 // -> Motor PWN Pin
 PwmOut motor_pwm[] = {PA_9, PC_8, PA_11};
@@ -103,18 +106,26 @@
             motorStop(i);
 }
 
-void Callback(const geometry_msgs::Twist& _msg) {
+void wheel_calc_thread() {
+    while (true) {
+        // calc
+        delay_for(5);
+    }
     
+}
 
+void saveCallback(const geometry_msgs::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", &twCallback);
+ros::Subscriber<geometry_msgs::Twist> subtw("base_twist", &saveCallback);
 int main() {
     //ros::Rate rate(10);
     nh.getHardware()->setBaud(BAUD);
     nh.initNode();
+    wheel_calc.start(wheel_calc_thread);
 //    nh.subscribe(subax);
     nh.subscribe(subbt);
     nh.subscribe(subtw);