For coursework of group 3 in SOFT564Z
Dependencies: Motordriver ros_lib_kinetic
Diff: ROS_Handler.cpp
- Revision:
- 1:6a10e58b3d43
- Parent:
- 0:3dfee562823a
- Child:
- 3:7da9888ac8dc
diff -r 3dfee562823a -r 6a10e58b3d43 ROS_Handler.cpp --- 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); + } + /**************************************************************************/ }