Jonathan Wheadon / Mbed OS SOFT564Z_Group3

For coursework of group 3 in SOFT564Z

Dependencies:   Motordriver ros_lib_kinetic

ROS_Handler.cpp@4:8afc50a3e4ac, 2019-11-30 (annotated)

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