Luka Danilovic / Mbed OS PROJ515_USS_GPS

Q

Dependencies:   SRF05 ros_lib_kinetic TinyGPSPlus

PROJ515.cpp@11:955c2ed70de2, 2019-05-08 (annotated)

Committer:
Luka_Danilovic
Date:
Wed May 08 14:50:32 2019 +0000
Revision:
11:955c2ed70de2
Parent:
10:7d954fba5e7a
Child:
12:1a3272d67500
BOARD CHANGE!; ROS - Working; USS - Working; GPS - Working; LED - Not Working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Luka_Danilovic 8:49306a01c52a 1 #include "PROJ515.hpp" // Contains Libraries, Definitions & Function Prototypes
Luka_Danilovic 0:215bdd87b602 2
Luka_Danilovic 2:7288dd12186e 3 float cfDist1 = 0; // Distance returned by cliff u_sensor1 (m)
Luka_Danilovic 2:7288dd12186e 4 float cfDist2 = 0; // Distance returned by cliff u_sensor2 (m)
Luka_Danilovic 2:7288dd12186e 5 float cfDist3 = 0; // Distance returned by cliff u_sensor3 (m)
Luka_Danilovic 2:7288dd12186e 6 float cfDist4 = 0; // Distance returned by cliff u_sensor4 (m)
Luka_Danilovic 2:7288dd12186e 7
Luka_Danilovic 6:40fc84f50432 8 float cbDist1 = 0; // Distance returned by curb u_sensor1 (m)
Luka_Danilovic 2:7288dd12186e 9 float cbDist2 = 0; // Distance returned by curb u_sensor2 (m)
Luka_Danilovic 2:7288dd12186e 10 float cbDist3 = 0; // Distance returned by curb u_sensor3 (m)
Luka_Danilovic 2:7288dd12186e 11 float cbDist4 = 0; // Distance returned by curb u_sensor4 (m)
Luka_Danilovic 2:7288dd12186e 12
Luka_Danilovic 7:6c26fdb1d226 13 float wPeriod = 10; // Wait period(ms) for ROS publisher to finish publishing
Luka_Danilovic 0:215bdd87b602 14
Luka_Danilovic 9:2d9a0c9e5456 15 char gps_c = 0; // GPS serial character
Luka_Danilovic 10:7d954fba5e7a 16 int sats_n = 0; // GPS Satelite count
Luka_Danilovic 9:2d9a0c9e5456 17
Luka_Danilovic 11:955c2ed70de2 18 void AudioStatusCB(const std_msgs::String &status) {
Luka_Danilovic 11:955c2ed70de2 19 audio_state = status.data;
Luka_Danilovic 11:955c2ed70de2 20 }
Luka_Danilovic 11:955c2ed70de2 21
Luka_Danilovic 11:955c2ed70de2 22 void VelocityCB(const nav_msgs::Odometry &odom) {
Luka_Danilovic 11:955c2ed70de2 23 vel = odom.twist.twist.linear.x;
Luka_Danilovic 11:955c2ed70de2 24 ang = odom.twist.twist.angular.z;
Luka_Danilovic 11:955c2ed70de2 25 }
Luka_Danilovic 11:955c2ed70de2 26
Luka_Danilovic 11:955c2ed70de2 27 // Instance for ROS subscribers
Luka_Danilovic 11:955c2ed70de2 28 ros::Subscriber<std_msgs::String> AudioStatus("audio_status", &AudioStatusCB);
Luka_Danilovic 11:955c2ed70de2 29 ros::Subscriber<nav_msgs::Odometry> Velocity("wheel_odom/exact_odom", &VelocityCB);
Luka_Danilovic 11:955c2ed70de2 30
Luka_Danilovic 9:2d9a0c9e5456 31 int main()
Luka_Danilovic 9:2d9a0c9e5456 32 {
Luka_Danilovic 9:2d9a0c9e5456 33 nh.getHardware()->setBaud(ROS_Baud); // Set Baud Rate for ROS Serial
Luka_Danilovic 9:2d9a0c9e5456 34 nh.initNode(); // Initialise ROS Node Handler
Luka_Danilovic 9:2d9a0c9e5456 35
Luka_Danilovic 9:2d9a0c9e5456 36 advRosPub(); // Adverstise Ultrasound Topic
Luka_Danilovic 9:2d9a0c9e5456 37 nh.advertise(gps_odom_pub); // Adverstise Odometry topic
Luka_Danilovic 9:2d9a0c9e5456 38
Luka_Danilovic 10:7d954fba5e7a 39 nh.subscribe(AudioStatus);
Luka_Danilovic 10:7d954fba5e7a 40 nh.subscribe(Velocity);
Luka_Danilovic 10:7d954fba5e7a 41
Luka_Danilovic 9:2d9a0c9e5456 42 setupRosMsg(); // Setup the message constant values
Luka_Danilovic 9:2d9a0c9e5456 43 gps_odom_msg.header.frame_id = frameID_g; // Pack ROS frame ID
Luka_Danilovic 9:2d9a0c9e5456 44 gps_odom_msg.status.status = UNAGUMENTED; // Location fix method
Luka_Danilovic 9:2d9a0c9e5456 45 gps_odom_msg.status.service = GPS_CONSTELL; // Satelite constellation used
Luka_Danilovic 9:2d9a0c9e5456 46 gps_odom_msg.altitude = 0; // Altitude (Always fixed)
Luka_Danilovic 9:2d9a0c9e5456 47 gps_odom_msg.position_covariance[ 0] = CVX; // X pos covariance
Luka_Danilovic 9:2d9a0c9e5456 48 gps_odom_msg.position_covariance[ 1] = CVO; // Zero
Luka_Danilovic 9:2d9a0c9e5456 49 gps_odom_msg.position_covariance[ 2] = CVO; // Zero
Luka_Danilovic 9:2d9a0c9e5456 50 gps_odom_msg.position_covariance[ 3] = CVO; // Zero
Luka_Danilovic 9:2d9a0c9e5456 51 gps_odom_msg.position_covariance[ 4] = CVY; // Y pos covariance
Luka_Danilovic 9:2d9a0c9e5456 52 gps_odom_msg.position_covariance[ 5] = CVO; // Zero
Luka_Danilovic 9:2d9a0c9e5456 53 gps_odom_msg.position_covariance[ 6] = CVO; // Zero
Luka_Danilovic 9:2d9a0c9e5456 54 gps_odom_msg.position_covariance[ 7] = CVO; // Zero
Luka_Danilovic 9:2d9a0c9e5456 55 gps_odom_msg.position_covariance[ 8] = CVZ; // Z pos covariance
Luka_Danilovic 9:2d9a0c9e5456 56 gps_odom_msg.position_covariance_type = UNNWN; // Covariance type
Luka_Danilovic 9:2d9a0c9e5456 57 gpsSer.baud(GPS_Baud); // Set Baud rate for GPS Serial
Luka_Danilovic 9:2d9a0c9e5456 58
Luka_Danilovic 10:7d954fba5e7a 59 int LEDBrightness = 128;
Luka_Danilovic 10:7d954fba5e7a 60 float led_tmp = LED_COUNT/4;
Luka_Danilovic 10:7d954fba5e7a 61 int led_num = floor(led_tmp);
Luka_Danilovic 10:7d954fba5e7a 62 int k = 0;
Luka_Danilovic 10:7d954fba5e7a 63 rgb_color led_colour = (rgb_color){0, 0, 0};
Luka_Danilovic 9:2d9a0c9e5456 64
Luka_Danilovic 9:2d9a0c9e5456 65 while(!nh.connected()) { // While node handler is !connected
Luka_Danilovic 9:2d9a0c9e5456 66 nh.spinOnce(); // Attempt to connect & synchronise
Luka_Danilovic 9:2d9a0c9e5456 67 }
Luka_Danilovic 9:2d9a0c9e5456 68
Luka_Danilovic 9:2d9a0c9e5456 69 while(true) {
Luka_Danilovic 9:2d9a0c9e5456 70 startSensors(); // Start the Sensors
Luka_Danilovic 9:2d9a0c9e5456 71 recordDist(); // Record the Distances (meters)
Luka_Danilovic 9:2d9a0c9e5456 72 checkCliff(); // Check the Cliff Sensors Readings
Luka_Danilovic 9:2d9a0c9e5456 73 setRosDist(); // Put distance in ROS messages
Luka_Danilovic 9:2d9a0c9e5456 74 setRosStamp(); // Put time into ROS messages
Luka_Danilovic 9:2d9a0c9e5456 75 publishRosMsg(); // Publish ROS messages
Luka_Danilovic 9:2d9a0c9e5456 76
Luka_Danilovic 9:2d9a0c9e5456 77 if(gpsSer.readable()) { // If serial buffer has character
Luka_Danilovic 9:2d9a0c9e5456 78 gps_c = gpsSer.getc(); // Read serial buffer and store character
Luka_Danilovic 9:2d9a0c9e5456 79 gpsModule.encode(gps_c); // Encode character from GPS
Luka_Danilovic 9:2d9a0c9e5456 80
Luka_Danilovic 9:2d9a0c9e5456 81 } else {
Luka_Danilovic 9:2d9a0c9e5456 82 leds = leds & 0b100; // Flash LED[0,1] bus OFF. Leave LED[2] on if its on to indicate that the signal was present
Luka_Danilovic 9:2d9a0c9e5456 83 }
Luka_Danilovic 9:2d9a0c9e5456 84 if (gpsModule.location.isValid()) { // If GPS location is Valid
Luka_Danilovic 9:2d9a0c9e5456 85 gps_odom_msg.latitude = gpsModule.location.lng(); // Get Longtitude
Luka_Danilovic 9:2d9a0c9e5456 86 gps_odom_msg.longitude = gpsModule.location.lat(); // Get Latitude
Luka_Danilovic 9:2d9a0c9e5456 87 nh.spinOnce(); // Reccuring connect and synchronise
Luka_Danilovic 9:2d9a0c9e5456 88 gps_odom_msg.header.stamp = nh.now(); // Get current time
Luka_Danilovic 9:2d9a0c9e5456 89 gps_odom_pub.publish(&gps_odom_msg); // Publish the Odometry message
Luka_Danilovic 9:2d9a0c9e5456 90 }
Luka_Danilovic 9:2d9a0c9e5456 91 sats_n = gpsModule.satellites.value(); // Aquire satelite number
Luka_Danilovic 9:2d9a0c9e5456 92 if (sats_n > 0){
Luka_Danilovic 9:2d9a0c9e5456 93 leds = LEDS_ON;
Luka_Danilovic 9:2d9a0c9e5456 94 }
Luka_Danilovic 10:7d954fba5e7a 95
Luka_Danilovic 10:7d954fba5e7a 96 nh.spinOnce();
Luka_Danilovic 10:7d954fba5e7a 97 if (((abs(vel) || abs(ang)) > 0.1f) && (audio_state == "Playing")){
Luka_Danilovic 10:7d954fba5e7a 98 if(abs(vel) >= abs(ang)) {
Luka_Danilovic 10:7d954fba5e7a 99 LEDBrightness = floor(Map(abs(vel), 0, MaxVelocity, 0, 255));
Luka_Danilovic 10:7d954fba5e7a 100 } else {
Luka_Danilovic 10:7d954fba5e7a 101 LEDBrightness = floor(Map(abs(ang), 0, MaxVelocity, 0, 255));
Luka_Danilovic 10:7d954fba5e7a 102 }
Luka_Danilovic 10:7d954fba5e7a 103 led_colour = (rgb_color) {0, 0, 255};
Luka_Danilovic 10:7d954fba5e7a 104 for(int i = 0; i < led_num; i++){
Luka_Danilovic 10:7d954fba5e7a 105 colors[i] = led_colour;
Luka_Danilovic 10:7d954fba5e7a 106 }
Luka_Danilovic 10:7d954fba5e7a 107 led_colour = (rgb_color) {LEDBrightness, 0, 0};
Luka_Danilovic 10:7d954fba5e7a 108 for(int i = led_num; i < ((3*led_num)+1); i++){
Luka_Danilovic 10:7d954fba5e7a 109 colors[i] = led_colour;
Luka_Danilovic 10:7d954fba5e7a 110 }
Luka_Danilovic 10:7d954fba5e7a 111 led_colour = (rgb_color) {0, 0, 255};
Luka_Danilovic 10:7d954fba5e7a 112 for(int i = ((3*led_num)+1); i < LED_COUNT; i++){
Luka_Danilovic 10:7d954fba5e7a 113 colors[i] = led_colour;
Luka_Danilovic 10:7d954fba5e7a 114 }
Luka_Danilovic 10:7d954fba5e7a 115 } else if((abs(vel) || abs(ang)) > 0.1f) { //If Not In DeadZone (Robot Is Moving)
Luka_Danilovic 10:7d954fba5e7a 116 if(abs(vel) >= abs(ang)) {
Luka_Danilovic 10:7d954fba5e7a 117 LEDBrightness = floor(Map(abs(vel), 0, MaxVelocity, 0, 255));
Luka_Danilovic 10:7d954fba5e7a 118 } else {
Luka_Danilovic 10:7d954fba5e7a 119 LEDBrightness = floor(Map(abs(ang), 0, MaxVelocity, 0, 255));
Luka_Danilovic 10:7d954fba5e7a 120 }
Luka_Danilovic 10:7d954fba5e7a 121 led_colour = (rgb_color) {0, LEDBrightness, 0};
Luka_Danilovic 10:7d954fba5e7a 122 for(int i = 0; i < LED_COUNT; i++){
Luka_Danilovic 10:7d954fba5e7a 123 colors[i] = led_colour;
Luka_Danilovic 10:7d954fba5e7a 124 }
Luka_Danilovic 10:7d954fba5e7a 125 } else if (audio_state == "Playing") { //Else If Audio Is Playing
Luka_Danilovic 10:7d954fba5e7a 126 led_colour = (rgb_color) {0, 0, 255};
Luka_Danilovic 10:7d954fba5e7a 127 for(int i = 0; i < LED_COUNT; i++){
Luka_Danilovic 10:7d954fba5e7a 128 colors[i] = led_colour;
Luka_Danilovic 10:7d954fba5e7a 129 }
Luka_Danilovic 10:7d954fba5e7a 130 } else { //Robot Is Neither Moving Nor Playing Audio
Luka_Danilovic 10:7d954fba5e7a 131 led_colour = (rgb_color) {255, 0, 0};
Luka_Danilovic 10:7d954fba5e7a 132 for(int i = 0; i < LED_COUNT; i++){
Luka_Danilovic 10:7d954fba5e7a 133 colors[i] = led_colour;
Luka_Danilovic 10:7d954fba5e7a 134 }
Luka_Danilovic 10:7d954fba5e7a 135 }
Luka_Danilovic 10:7d954fba5e7a 136
Luka_Danilovic 10:7d954fba5e7a 137 ledStripFront.write(colors, LED_COUNT);
Luka_Danilovic 10:7d954fba5e7a 138 ledStripBack.write(colors, LED_COUNT);
Luka_Danilovic 10:7d954fba5e7a 139 wait_ms(5);
Luka_Danilovic 9:2d9a0c9e5456 140 }
Luka_Danilovic 9:2d9a0c9e5456 141 }
Luka_Danilovic 9:2d9a0c9e5456 142
Luka_Danilovic 9:2d9a0c9e5456 143 /*============================================================================*/
Luka_Danilovic 9:2d9a0c9e5456 144 /* FUNCTIONS */
Luka_Danilovic 9:2d9a0c9e5456 145 /*============================================================================*/
Luka_Danilovic 9:2d9a0c9e5456 146
Luka_Danilovic 9:2d9a0c9e5456 147
Luka_Danilovic 8:49306a01c52a 148 void advRosPub()
Luka_Danilovic 8:49306a01c52a 149 {
Luka_Danilovic 8:49306a01c52a 150 // Adverstise Ultrasound topic
Luka_Danilovic 8:49306a01c52a 151 nh.advertise(ultrasound_pub_cf1);
Luka_Danilovic 8:49306a01c52a 152 nh.advertise(ultrasound_pub_cf2);
Luka_Danilovic 8:49306a01c52a 153 nh.advertise(ultrasound_pub_cf3);
Luka_Danilovic 8:49306a01c52a 154 nh.advertise(ultrasound_pub_cf4);
Luka_Danilovic 8:49306a01c52a 155 nh.advertise(ultrasound_pub_cb1);
Luka_Danilovic 8:49306a01c52a 156 nh.advertise(ultrasound_pub_cb2);
Luka_Danilovic 8:49306a01c52a 157 nh.advertise(ultrasound_pub_cb3);
Luka_Danilovic 8:49306a01c52a 158 nh.advertise(ultrasound_pub_cb4);
Luka_Danilovic 8:49306a01c52a 159 }
Luka_Danilovic 0:215bdd87b602 160
Luka_Danilovic 0:215bdd87b602 161 void setupRosMsg()
Luka_Danilovic 3:cd1f2bde7ac2 162 {
Luka_Danilovic 8:49306a01c52a 163 // Setup the message constant values
Luka_Danilovic 8:49306a01c52a 164
Luka_Danilovic 3:cd1f2bde7ac2 165 // Frame ID
Luka_Danilovic 3:cd1f2bde7ac2 166 ultrasound_msg_cf1.header.frame_id = "uss_cf1";
Luka_Danilovic 7:6c26fdb1d226 167 ultrasound_msg_cf2.header.frame_id = "uss_cf2";
Luka_Danilovic 7:6c26fdb1d226 168 ultrasound_msg_cf3.header.frame_id = "uss_cf3";
Luka_Danilovic 7:6c26fdb1d226 169 ultrasound_msg_cf4.header.frame_id = "uss_cf4";
Luka_Danilovic 3:cd1f2bde7ac2 170 ultrasound_msg_cb1.header.frame_id = "uss_cb1";
Luka_Danilovic 7:6c26fdb1d226 171 ultrasound_msg_cb2.header.frame_id = "uss_cb2";
Luka_Danilovic 7:6c26fdb1d226 172 ultrasound_msg_cb3.header.frame_id = "uss_cb3";
Luka_Danilovic 7:6c26fdb1d226 173 ultrasound_msg_cb4.header.frame_id = "uss_cb4";
Luka_Danilovic 3:cd1f2bde7ac2 174
Luka_Danilovic 3:cd1f2bde7ac2 175 // Radiation Type
Luka_Danilovic 3:cd1f2bde7ac2 176 ultrasound_msg_cf1.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 177 ultrasound_msg_cf2.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 178 ultrasound_msg_cf3.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 179 ultrasound_msg_cf4.radiation_type = ULTRASOUND;
Luka_Danilovic 3:cd1f2bde7ac2 180 ultrasound_msg_cb1.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 181 ultrasound_msg_cb2.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 182 ultrasound_msg_cb3.radiation_type = ULTRASOUND;
Luka_Danilovic 7:6c26fdb1d226 183 ultrasound_msg_cb4.radiation_type = ULTRASOUND;
Luka_Danilovic 3:cd1f2bde7ac2 184
Luka_Danilovic 3:cd1f2bde7ac2 185 // Field of View
Luka_Danilovic 3:cd1f2bde7ac2 186 ultrasound_msg_cf1.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 187 ultrasound_msg_cf2.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 188 ultrasound_msg_cf3.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 189 ultrasound_msg_cf4.field_of_view = FOV;
Luka_Danilovic 3:cd1f2bde7ac2 190 ultrasound_msg_cb1.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 191 ultrasound_msg_cb2.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 192 ultrasound_msg_cb3.field_of_view = FOV;
Luka_Danilovic 7:6c26fdb1d226 193 ultrasound_msg_cb4.field_of_view = FOV;
Luka_Danilovic 3:cd1f2bde7ac2 194
Luka_Danilovic 3:cd1f2bde7ac2 195 // Minumum Range
Luka_Danilovic 3:cd1f2bde7ac2 196 ultrasound_msg_cf1.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 197 ultrasound_msg_cf2.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 198 ultrasound_msg_cf3.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 199 ultrasound_msg_cf4.min_range = MIN_RANGE;
Luka_Danilovic 4:f74d88f5f629 200 ultrasound_msg_cb1.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 201 ultrasound_msg_cb2.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 202 ultrasound_msg_cb3.min_range = MIN_RANGE;
Luka_Danilovic 7:6c26fdb1d226 203 ultrasound_msg_cb4.min_range = MIN_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 204
Luka_Danilovic 3:cd1f2bde7ac2 205 // Maximum Range
Luka_Danilovic 3:cd1f2bde7ac2 206 ultrasound_msg_cf1.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 207 ultrasound_msg_cf2.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 208 ultrasound_msg_cf3.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 209 ultrasound_msg_cf4.max_range = MAX_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 210 ultrasound_msg_cb1.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 211 ultrasound_msg_cb2.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 212 ultrasound_msg_cb3.max_range = MAX_RANGE;
Luka_Danilovic 7:6c26fdb1d226 213 ultrasound_msg_cb4.max_range = MAX_RANGE;
Luka_Danilovic 3:cd1f2bde7ac2 214 }
Luka_Danilovic 3:cd1f2bde7ac2 215
Luka_Danilovic 8:49306a01c52a 216 void startSensors()
Luka_Danilovic 8:49306a01c52a 217 {
Luka_Danilovic 8:49306a01c52a 218 // Start the Sensors
Luka_Danilovic 8:49306a01c52a 219 USS_CF1.start();
Luka_Danilovic 8:49306a01c52a 220 USS_CF2.start();
Luka_Danilovic 8:49306a01c52a 221 USS_CF3.start();
Luka_Danilovic 8:49306a01c52a 222 USS_CF4.start();
Luka_Danilovic 8:49306a01c52a 223 USS_CB1.start();
Luka_Danilovic 8:49306a01c52a 224 USS_CB2.start();
Luka_Danilovic 8:49306a01c52a 225 USS_CB3.start();
Luka_Danilovic 8:49306a01c52a 226 USS_CB4.start();
Luka_Danilovic 8:49306a01c52a 227 }
Luka_Danilovic 8:49306a01c52a 228
Luka_Danilovic 8:49306a01c52a 229 void recordDist()
Luka_Danilovic 8:49306a01c52a 230 {
Luka_Danilovic 8:49306a01c52a 231 // Record the Distances (meters)
Luka_Danilovic 8:49306a01c52a 232 cfDist1 = USS_CF1.get_dist();
Luka_Danilovic 8:49306a01c52a 233 cfDist2 = USS_CF2.get_dist();
Luka_Danilovic 8:49306a01c52a 234 cfDist3 = USS_CF3.get_dist();
Luka_Danilovic 8:49306a01c52a 235 cfDist4 = USS_CF4.get_dist();
Luka_Danilovic 8:49306a01c52a 236 cbDist1 = USS_CB1.get_dist();
Luka_Danilovic 8:49306a01c52a 237 cbDist2 = USS_CB2.get_dist();
Luka_Danilovic 8:49306a01c52a 238 cbDist3 = USS_CB3.get_dist();
Luka_Danilovic 8:49306a01c52a 239 cbDist4 = USS_CB4.get_dist();
Luka_Danilovic 8:49306a01c52a 240 }
Luka_Danilovic 8:49306a01c52a 241
Luka_Danilovic 3:cd1f2bde7ac2 242 void checkCliff()
Luka_Danilovic 3:cd1f2bde7ac2 243 {
Luka_Danilovic 3:cd1f2bde7ac2 244 /* Check the Cliff Sensors (If distance more than clif treshold,
Luka_Danilovic 3:cd1f2bde7ac2 245 hardcode the cliff value in, else no obstacle)*/
Luka_Danilovic 3:cd1f2bde7ac2 246 if (static_cast<double>(cfDist1) <= CLIFF_TRH) {
Luka_Danilovic 6:40fc84f50432 247 cfDist1 = CLIFF_ZRO;
Luka_Danilovic 3:cd1f2bde7ac2 248 } else {
Luka_Danilovic 6:40fc84f50432 249 cfDist1 = EXCL_ZONE;
Luka_Danilovic 3:cd1f2bde7ac2 250 }
Luka_Danilovic 7:6c26fdb1d226 251 if (static_cast<double>(cfDist2) <= CLIFF_TRH) {
Luka_Danilovic 7:6c26fdb1d226 252 cfDist2 = CLIFF_ZRO;
Luka_Danilovic 7:6c26fdb1d226 253 } else {
Luka_Danilovic 7:6c26fdb1d226 254 cfDist2 = EXCL_ZONE;
Luka_Danilovic 7:6c26fdb1d226 255 }
Luka_Danilovic 7:6c26fdb1d226 256 if (static_cast<double>(cfDist3) <= CLIFF_TRH) {
Luka_Danilovic 7:6c26fdb1d226 257 cfDist3 = CLIFF_ZRO;
Luka_Danilovic 7:6c26fdb1d226 258 } else {
Luka_Danilovic 7:6c26fdb1d226 259 cfDist3 = EXCL_ZONE;
Luka_Danilovic 7:6c26fdb1d226 260 }
Luka_Danilovic 7:6c26fdb1d226 261 if (static_cast<double>(cfDist4) <= CLIFF_TRH) {
Luka_Danilovic 7:6c26fdb1d226 262 cfDist4 = CLIFF_ZRO;
Luka_Danilovic 7:6c26fdb1d226 263 } else {
Luka_Danilovic 7:6c26fdb1d226 264 cfDist4 = EXCL_ZONE;
Luka_Danilovic 7:6c26fdb1d226 265 }
Luka_Danilovic 3:cd1f2bde7ac2 266 }
Luka_Danilovic 3:cd1f2bde7ac2 267
Luka_Danilovic 8:49306a01c52a 268 void setRosDist()
Luka_Danilovic 3:cd1f2bde7ac2 269 {
Luka_Danilovic 3:cd1f2bde7ac2 270 // Put distance in ROS messages
Luka_Danilovic 3:cd1f2bde7ac2 271 ultrasound_msg_cf1.range = cfDist1;
Luka_Danilovic 7:6c26fdb1d226 272 ultrasound_msg_cf2.range = cfDist2;
Luka_Danilovic 7:6c26fdb1d226 273 ultrasound_msg_cf3.range = cfDist3;
Luka_Danilovic 7:6c26fdb1d226 274 ultrasound_msg_cf4.range = cfDist4;
Luka_Danilovic 3:cd1f2bde7ac2 275 ultrasound_msg_cb1.range = cbDist1;
Luka_Danilovic 7:6c26fdb1d226 276 ultrasound_msg_cb2.range = cbDist2;
Luka_Danilovic 7:6c26fdb1d226 277 ultrasound_msg_cb3.range = cbDist3;
Luka_Danilovic 7:6c26fdb1d226 278 ultrasound_msg_cb4.range = cbDist4;
Luka_Danilovic 3:cd1f2bde7ac2 279 }
Luka_Danilovic 3:cd1f2bde7ac2 280
Luka_Danilovic 8:49306a01c52a 281 void setRosStamp()
Luka_Danilovic 3:cd1f2bde7ac2 282 {
Luka_Danilovic 3:cd1f2bde7ac2 283 // Get current time and put into ROS messages
Luka_Danilovic 8:49306a01c52a 284 nh.spinOnce(); // Reccuring connect and synchronise
Luka_Danilovic 3:cd1f2bde7ac2 285 ultrasound_msg_cf1.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 286 ultrasound_msg_cf2.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 287 ultrasound_msg_cf3.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 288 ultrasound_msg_cf4.header.stamp = nh.now();
Luka_Danilovic 3:cd1f2bde7ac2 289 ultrasound_msg_cb1.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 290 ultrasound_msg_cb2.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 291 ultrasound_msg_cb3.header.stamp = nh.now();
Luka_Danilovic 7:6c26fdb1d226 292 ultrasound_msg_cb4.header.stamp = nh.now();
Luka_Danilovic 3:cd1f2bde7ac2 293 }
Luka_Danilovic 3:cd1f2bde7ac2 294
Luka_Danilovic 3:cd1f2bde7ac2 295 void publishRosMsg()
Luka_Danilovic 3:cd1f2bde7ac2 296 {
Luka_Danilovic 3:cd1f2bde7ac2 297 // Publish ROS messages
Luka_Danilovic 3:cd1f2bde7ac2 298 ultrasound_pub_cf1.publish(&ultrasound_msg_cf1);
Luka_Danilovic 4:f74d88f5f629 299 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 300 ultrasound_pub_cf2.publish(&ultrasound_msg_cf2);
Luka_Danilovic 7:6c26fdb1d226 301 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 302 ultrasound_pub_cf3.publish(&ultrasound_msg_cf3);
Luka_Danilovic 7:6c26fdb1d226 303 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 304 ultrasound_pub_cf4.publish(&ultrasound_msg_cf4);
Luka_Danilovic 7:6c26fdb1d226 305 wait_ms(wPeriod);
Luka_Danilovic 3:cd1f2bde7ac2 306 ultrasound_pub_cb1.publish(&ultrasound_msg_cb1);
Luka_Danilovic 3:cd1f2bde7ac2 307 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 308 ultrasound_pub_cb2.publish(&ultrasound_msg_cb2);
Luka_Danilovic 7:6c26fdb1d226 309 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 310 ultrasound_pub_cb3.publish(&ultrasound_msg_cb3);
Luka_Danilovic 7:6c26fdb1d226 311 wait_ms(wPeriod);
Luka_Danilovic 7:6c26fdb1d226 312 ultrasound_pub_cb4.publish(&ultrasound_msg_cb4);
Luka_Danilovic 7:6c26fdb1d226 313 wait_ms(wPeriod);
Luka_Danilovic 3:cd1f2bde7ac2 314 }
Luka_Danilovic 10:7d954fba5e7a 315
Luka_Danilovic 10:7d954fba5e7a 316
Luka_Danilovic 10:7d954fba5e7a 317
Luka_Danilovic 10:7d954fba5e7a 318 float Map(float x, float in_min, float in_max, float out_min, float out_max){
Luka_Danilovic 10:7d954fba5e7a 319 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
Luka_Danilovic 10:7d954fba5e7a 320 }