Jonathan Wheadon / Mbed OS SOFT564Z_Group3

For coursework of group 3 in SOFT564Z

Dependencies:   Motordriver ros_lib_kinetic

ROS_Handler.cpp@3:7da9888ac8dc, 2019-11-28 (annotated)

Committer:
Jonathan738
Date:
Thu Nov 28 13:36:44 2019 +0000
Revision:
3:7da9888ac8dc
Parent:
1:6a10e58b3d43
Child:
4:8afc50a3e4ac
Re-factored Sami's code for battery checking to work in threaded environment.;

Who changed what in which revision?

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