Diff: ROS_Handler.cpp

Revision:

1:6a10e58b3d43

Parent:

0:3dfee562823a

Child:

3:7da9888ac8dc

--- a/ROS_Handler.cpp	Wed Nov 27 18:28:06 2019 +0000
+++ b/ROS_Handler.cpp	Thu Nov 28 11:56:51 2019 +0000
@@ -37,6 +37,22 @@
 
 void CallBack(const std_msgs::Int32MultiArray& msg)
 {
-    myLED != myLED;
+    /**************************************************************************/
+    //Extracting the only commands that will be used in the multiarray, and assuming that the int value attached to each point is 100 times
+    //the intended PWM constant for that movement command (e.g. a value of 50 in msg.data[6] would give a PWM duty cycle of 0.5 for rotation):
+    float xlin = 0.01 * msg.data[1];
+    float zrot = 0.01 * msg.data[6];
+    //assume rotation needs to be taken care of first, generally, then linear movement for this basic controller
+    if(zrot != 0) { //assume positive z is clockwise, negative is anticlockwise, A is left motor, B is right motor (viewed from bottom layer battery switch direction)
+        A.speed(-zrot);
+        B.speed(zrot);
+    } else if(xlin != 0) {
+        A.speed(xlin);
+        B.speed(xlin);
+    } else {
+        A.stop(1);
+        B.stop(1);
+    }
+    /**************************************************************************/
 }