SBD Digital Accelerator Robotics / Mbed 2 deprecated Test

VNH ROSSERIAL PUB+SUB

Dependencies:   Motordriver QEI mbed ros_lib_kinetic

Fork of Test by SBD Digital Accelerator Robotics

Diff: main.cpp

Revision:
0:99538487421b
Child:
1:5f2cb6bc0a8e
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jul 13 19:58:04 2018 +0000
@@ -0,0 +1,34 @@
+#include "mbed.h"
+#include "motordriver.h"
+#include "ros.h"
+#include "geometry_msgs/Vector3.h"
+
+//pwm, fwd, rev, can brake
+Motor motorLeft(D9,D5,D4, 1);
+Motor motorRight(D10, D7, D8, 1); 
+
+ros:: NodeHandle nh; 
+
+void messageCb( const geometry_msgs::Vector3& msg)
+{
+      while (true) {
+      for (float s=-msg.x; s < msg.x ; s += 0.01f) {
+       motorLeft.speed(s);
+       motorRight.speed(s);
+       wait(0.005);}
+    }
+}
+
+ros::Subscriber<geometry_msgs::Vector3> sub("/motor/command", &messageCb );
+
+int main() {
+    
+    nh.initNode();
+    nh.subscribe(sub);
+    
+    while (true) {
+        nh.spinOnce();
+        wait_ms(1);
+    } 
+}
+