Jonathan Wheadon / Mbed OS SOFT564Z_Group3

For coursework of group 3 in SOFT564Z

Dependencies:   Motordriver ros_lib_kinetic

ROS_Handler.cpp@7:2796f0b5228d, 2019-12-05 (annotated)

Committer:
Jonathan738
Date:
Thu Dec 05 13:02:10 2019 +0000
Revision:
7:2796f0b5228d
Parent:
4:8afc50a3e4ac
Child:
11:0b9098ec11c7
Merged code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jonathan738 4:8afc50a3e4ac 1 #include "mbed.h"
Jonathan738 0:3dfee562823a 2 #include "General.hpp"
Jonathan738 4:8afc50a3e4ac 3 #include "ros.h"
Jonathan738 4:8afc50a3e4ac 4 #include "rtos.h"
Jonathan738 4:8afc50a3e4ac 5 #include "ROS_Handler.hpp"
Jonathan738 4:8afc50a3e4ac 6 #include <motordriver.h>
Jonathan738 4:8afc50a3e4ac 7 #include "math.h"
Jonathan738 4:8afc50a3e4ac 8 #include "Motors.hpp"
Jonathan738 4:8afc50a3e4ac 9 #include "Pins.h"
Jonathan738 0:3dfee562823a 10
Jonathan738 3:7da9888ac8dc 11 extern Motor A; // pwm, fwd, rev, can brake
Jonathan738 3:7da9888ac8dc 12 extern Motor B; // pwm, fwd, rev, can brake
Jonathan738 3:7da9888ac8dc 13
Jonathan738 0:3dfee562823a 14 /******************************************************************************/
Jonathan738 0:3dfee562823a 15 /* ROS serial hardware setup */
Jonathan738 0:3dfee562823a 16 /******************************************************************************/
Jonathan738 0:3dfee562823a 17 // Create the ROS node handle
Jonathan738 0:3dfee562823a 18 class HaseHardware : public MbedHardware
Jonathan738 0:3dfee562823a 19 {
Jonathan738 0:3dfee562823a 20 public:
Jonathan738 0:3dfee562823a 21 HaseHardware(): MbedHardware(ROS_Tx, ROS_Rx, ROS_BaudRate) {};
Jonathan738 0:3dfee562823a 22 };
Jonathan738 0:3dfee562823a 23 /******************************************************************************/
Jonathan738 0:3dfee562823a 24
Jonathan738 0:3dfee562823a 25 ros::NodeHandle_<HaseHardware> Node_;
Jonathan738 0:3dfee562823a 26 void CallBack(const std_msgs::Int32MultiArray& msg);
Jonathan738 0:3dfee562823a 27 ros::Subscriber<std_msgs::Int32MultiArray> Sub_("/cmd_vel", &CallBack);
Jonathan738 0:3dfee562823a 28 std_msgs::Int32 PUB_MSG;
Jonathan738 0:3dfee562823a 29 ros::Publisher Pub_("/TOF_Data", &PUB_MSG);
Jonathan738 0:3dfee562823a 30
Jonathan738 0:3dfee562823a 31 // Main function for thread to handle ROS comms
Jonathan738 0:3dfee562823a 32 void ROS_Handler(void){
Jonathan738 0:3dfee562823a 33 Node_.initNode();
Jonathan738 0:3dfee562823a 34 Node_.advertise(Pub_);
Jonathan738 0:3dfee562823a 35 Node_.subscribe(Sub_);
Jonathan738 0:3dfee562823a 36
Jonathan738 0:3dfee562823a 37 PUB_MSG.data = 10;
Jonathan738 0:3dfee562823a 38
Jonathan738 0:3dfee562823a 39 while (1) {
Jonathan738 0:3dfee562823a 40 Pub_.publish(&PUB_MSG);
Jonathan738 0:3dfee562823a 41 Node_.spinOnce();
Jonathan738 0:3dfee562823a 42 wait_ms(20);
Jonathan738 0:3dfee562823a 43 }
Jonathan738 0:3dfee562823a 44 }
Jonathan738 0:3dfee562823a 45
Jonathan738 0:3dfee562823a 46 void CallBack(const std_msgs::Int32MultiArray& msg)
Jonathan738 0:3dfee562823a 47 {
firnenenrif 1:6a10e58b3d43 48 /**************************************************************************/
firnenenrif 1:6a10e58b3d43 49 //Extracting the only commands that will be used in the multiarray, and assuming that the int value attached to each point is 100 times
firnenenrif 1:6a10e58b3d43 50 //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):
firnenenrif 1:6a10e58b3d43 51 float xlin = 0.01 * msg.data[1];
firnenenrif 1:6a10e58b3d43 52 float zrot = 0.01 * msg.data[6];
firnenenrif 1:6a10e58b3d43 53 //assume rotation needs to be taken care of first, generally, then linear movement for this basic controller
firnenenrif 1:6a10e58b3d43 54 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)
firnenenrif 1:6a10e58b3d43 55 A.speed(-zrot);
firnenenrif 1:6a10e58b3d43 56 B.speed(zrot);
firnenenrif 1:6a10e58b3d43 57 } else if(xlin != 0) {
firnenenrif 1:6a10e58b3d43 58 A.speed(xlin);
firnenenrif 1:6a10e58b3d43 59 B.speed(xlin);
firnenenrif 1:6a10e58b3d43 60 } else {
firnenenrif 1:6a10e58b3d43 61 A.stop(1);
firnenenrif 1:6a10e58b3d43 62 B.stop(1);
firnenenrif 1:6a10e58b3d43 63 }
firnenenrif 1:6a10e58b3d43 64 /**************************************************************************/
Jonathan738 0:3dfee562823a 65 }
Jonathan738 0:3dfee562823a 66