Luka Danilovic / Mbed OS PROJ515_USS_GPS

Q

Dependencies:   SRF05 ros_lib_kinetic TinyGPSPlus

PROJ515.hpp@8:49306a01c52a, 2019-05-07 (annotated)

Committer:
Luka_Danilovic
Date:
Tue May 07 15:57:01 2019 +0000
Revision:
8:49306a01c52a
Parent:
6:40fc84f50432
Child:
9:2d9a0c9e5456
- ROS with functions; - All sensors working (With tuned timings); - Custom SRF05 class; - Functions not separated

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Luka_Danilovic 0:215bdd87b602 1 #ifndef __PROJ515_H__ //Inclusion safeguards
Luka_Danilovic 0:215bdd87b602 2 #define __PROJ515_H__ //Definition of the inclusion
Luka_Danilovic 0:215bdd87b602 3 /*============================================================================*/
Luka_Danilovic 0:215bdd87b602 4
Luka_Danilovic 0:215bdd87b602 5 /* Libraries */
Luka_Danilovic 0:215bdd87b602 6 #include "mbed.h" // Mbed RTOS
Luka_Danilovic 8:49306a01c52a 7 #include "SRF05.h" // SRF05 ultrasound sensor custom libary
Luka_Danilovic 0:215bdd87b602 8 #include "ros.h" // ROS Library
Luka_Danilovic 0:215bdd87b602 9 #include "sensor_msgs/Range.h" // ROS Sensor Messages component
Luka_Danilovic 0:215bdd87b602 10
Luka_Danilovic 0:215bdd87b602 11 /* Definitions */
Luka_Danilovic 0:215bdd87b602 12 #define LEDS_OFF 0x00 // LED bus all off
Luka_Danilovic 0:215bdd87b602 13 #define LEDS_ON 0x07 // LED bus all on
Luka_Danilovic 1:ea2ee36038e7 14
Luka_Danilovic 2:7288dd12186e 15 #define USP_CF1T PG_1 // Cliff ultrasound 1 Trigger
Luka_Danilovic 2:7288dd12186e 16 #define USP_CF1E PB_11 // Cliff ultrasound 1 Echo
Luka_Danilovic 2:7288dd12186e 17 #define USP_CF2T PF_9 // Cliff ultrasound 2 Trigger
Luka_Danilovic 2:7288dd12186e 18 #define USP_CF2E PB_10 // Cliff ultrasound 2 Echo
Luka_Danilovic 2:7288dd12186e 19 #define USP_CF3T PF_7 // Cliff ultrasound 3 Trigger
Luka_Danilovic 2:7288dd12186e 20 #define USP_CF3E PE_15 // Cliff ultrasound 3 Echo
Luka_Danilovic 2:7288dd12186e 21 #define USP_CF4T PF_8 // Cliff ultrasound 4 Trigger
Luka_Danilovic 2:7288dd12186e 22 #define USP_CF4E PE_12 // Cliff ultrasound 4 Echo
Luka_Danilovic 1:ea2ee36038e7 23
Luka_Danilovic 2:7288dd12186e 24 #define USP_CB1T PE_6 // Curb ultrasound 1 Trigger
Luka_Danilovic 2:7288dd12186e 25 #define USP_CB1E PE_7 // Curb ultrasound 1 Echo
Luka_Danilovic 2:7288dd12186e 26 #define USP_CB2T PE_5 // Curb ultrasound 2 Trigger
Luka_Danilovic 2:7288dd12186e 27 #define USP_CB2E PE_8 // Curb ultrasound 2 Echo
Luka_Danilovic 2:7288dd12186e 28 #define USP_CB3T PE_4 // Curb ultrasound 3 Trigger
Luka_Danilovic 2:7288dd12186e 29 #define USP_CB3E PG_9 // Curb ultrasound 3 Echo
Luka_Danilovic 2:7288dd12186e 30 #define USP_CB4T PE_2 // Curb ultrasound 4 Trigger
Luka_Danilovic 2:7288dd12186e 31 #define USP_CB4E PG_14 // Curb ultrasound 4 Echo
Luka_Danilovic 0:215bdd87b602 32
Luka_Danilovic 3:cd1f2bde7ac2 33 #define FOV 1.396 // Ultrasound sensor field of view
Luka_Danilovic 3:cd1f2bde7ac2 34 #define MIN_RANGE 0.2 // Ultrasound sensor min range
Luka_Danilovic 3:cd1f2bde7ac2 35 #define MAX_RANGE 2.5 // Ultrasound sensor max range
Luka_Danilovic 8:49306a01c52a 36 #define CLIFF_TRH 0.1 // Robot cliff treshold
Luka_Danilovic 3:cd1f2bde7ac2 37 #define CLIFF_ZRO 0 // Cliff not existent
Luka_Danilovic 3:cd1f2bde7ac2 38 #define EXCL_ZONE 0.255 // Robot footprint exclusion zone
Luka_Danilovic 3:cd1f2bde7ac2 39
Luka_Danilovic 6:40fc84f50432 40 #define ROS_Baud 460800 // ROS Baud Rate
Luka_Danilovic 0:215bdd87b602 41 #define frameID "ultrasound_range" // ROS frame ID
Luka_Danilovic 0:215bdd87b602 42
Luka_Danilovic 1:ea2ee36038e7 43 #define ULTRASOUND 0x00 // Ultrasound
Luka_Danilovic 0:215bdd87b602 44 #define PRINT_DATA 0x01 // Thread communication event to print data
Luka_Danilovic 0:215bdd87b602 45
Luka_Danilovic 1:ea2ee36038e7 46 /* Namespaces */
Luka_Danilovic 1:ea2ee36038e7 47 using namespace std; // Make all std symbols visible
Luka_Danilovic 1:ea2ee36038e7 48 using namespace ros; // Make all ros symbols visible
Luka_Danilovic 1:ea2ee36038e7 49 using namespace sensor_msgs; // Make all sensor_msgs symbols visible
Luka_Danilovic 1:ea2ee36038e7 50 using namespace std_msgs; // Make all sdt_msgs symbols visible
Luka_Danilovic 1:ea2ee36038e7 51
Luka_Danilovic 0:215bdd87b602 52 /* Declarations */
Luka_Danilovic 2:7288dd12186e 53 extern float cfDist1; // Distance returned by cliff u_sensor1 (m)
Luka_Danilovic 2:7288dd12186e 54 extern float cfDist2; // Distance returned by cliff u_sensor2 (m)
Luka_Danilovic 2:7288dd12186e 55 extern float cfDist3; // Distance returned by cliff u_sensor3 (m)
Luka_Danilovic 2:7288dd12186e 56 extern float cfDist4; // Distance returned by cliff u_sensor4 (m)
Luka_Danilovic 1:ea2ee36038e7 57
Luka_Danilovic 2:7288dd12186e 58 extern float cbDist1; // Distance returned by curb u_sensor1 (m)
Luka_Danilovic 2:7288dd12186e 59 extern float cbDist2; // Distance returned by curb u_sensor2 (m)
Luka_Danilovic 2:7288dd12186e 60 extern float cbDist3; // Distance returned by curb u_sensor3 (m)
Luka_Danilovic 2:7288dd12186e 61 extern float cbDist4; // Distance returned by curb u_sensor4 (m)
Luka_Danilovic 1:ea2ee36038e7 62
Luka_Danilovic 2:7288dd12186e 63 extern float wPeriod; // Wait period(ms) for ROS publisher
Luka_Danilovic 0:215bdd87b602 64
Luka_Danilovic 0:215bdd87b602 65 /* GPIO */
Luka_Danilovic 0:215bdd87b602 66 DigitalIn bttn(USER_BUTTON); // Onboard blue user button
Luka_Danilovic 0:215bdd87b602 67 BusOut leds(LED1, LED2, LED3); // Onboard LEDs bus
Luka_Danilovic 0:215bdd87b602 68
Luka_Danilovic 0:215bdd87b602 69 /* Instantiations */
Luka_Danilovic 8:49306a01c52a 70 // Instance for ultrasound sensors
Luka_Danilovic 8:49306a01c52a 71 SRF05 USS_CF1(USP_CF1T, USP_CF1E); // Ultrasound sensor cliff 1
Luka_Danilovic 8:49306a01c52a 72 SRF05 USS_CF2(USP_CF2T, USP_CF2E); // Ultrasound sensor cliff 2
Luka_Danilovic 8:49306a01c52a 73 SRF05 USS_CF3(USP_CF3T, USP_CF3E); // Ultrasound sensor cliff 3
Luka_Danilovic 8:49306a01c52a 74 SRF05 USS_CF4(USP_CF4T, USP_CF4E); // Ultrasound sensor cliff 4
Luka_Danilovic 1:ea2ee36038e7 75
Luka_Danilovic 8:49306a01c52a 76 SRF05 USS_CB1(USP_CB1T, USP_CB1E); // Ultrasound sensor cliff 1
Luka_Danilovic 8:49306a01c52a 77 SRF05 USS_CB2(USP_CB2T, USP_CB2E); // Ultrasound sensor cliff 2
Luka_Danilovic 8:49306a01c52a 78 SRF05 USS_CB3(USP_CB3T, USP_CB3E); // Ultrasound sensor cliff 3
Luka_Danilovic 8:49306a01c52a 79 SRF05 USS_CB4(USP_CB4T, USP_CB4E); // Ultrasound sensor cliff 4
Luka_Danilovic 2:7288dd12186e 80
Luka_Danilovic 8:49306a01c52a 81 // Instance for ROS node and messages
Luka_Danilovic 0:215bdd87b602 82 NodeHandle nh; // Instance for ROS Node Handler
Luka_Danilovic 3:cd1f2bde7ac2 83
Luka_Danilovic 3:cd1f2bde7ac2 84 Range ultrasound_msg_cf1; // Instance for ROS NavSatFix message 1
Luka_Danilovic 3:cd1f2bde7ac2 85 Range ultrasound_msg_cf2; // Instance for ROS NavSatFix message 2
Luka_Danilovic 3:cd1f2bde7ac2 86 Range ultrasound_msg_cf3; // Instance for ROS NavSatFix message 3
Luka_Danilovic 3:cd1f2bde7ac2 87 Range ultrasound_msg_cf4; // Instance for ROS NavSatFix message 4
Luka_Danilovic 3:cd1f2bde7ac2 88 Range ultrasound_msg_cb1; // Instance for ROS NavSatFix message 5
Luka_Danilovic 3:cd1f2bde7ac2 89 Range ultrasound_msg_cb2; // Instance for ROS NavSatFix message 6
Luka_Danilovic 3:cd1f2bde7ac2 90 Range ultrasound_msg_cb3; // Instance for ROS NavSatFix message 7
Luka_Danilovic 3:cd1f2bde7ac2 91 Range ultrasound_msg_cb4; // Instance for ROS NavSatFix message 8
Luka_Danilovic 1:ea2ee36038e7 92
Luka_Danilovic 3:cd1f2bde7ac2 93 // Instance for ROS publishers (Range message for cliff and curb sensors)
Luka_Danilovic 4:f74d88f5f629 94 Publisher ultrasound_pub_cf1("ultrasound/cf1", &ultrasound_msg_cf1);
Luka_Danilovic 4:f74d88f5f629 95 Publisher ultrasound_pub_cf2("ultrasound/cf2", &ultrasound_msg_cf2);
Luka_Danilovic 4:f74d88f5f629 96 Publisher ultrasound_pub_cf3("ultrasound/cf3", &ultrasound_msg_cf3);
Luka_Danilovic 4:f74d88f5f629 97 Publisher ultrasound_pub_cf4("ultrasound/cf4", &ultrasound_msg_cf4);
Luka_Danilovic 4:f74d88f5f629 98 Publisher ultrasound_pub_cb1("ultrasound/cb1", &ultrasound_msg_cb1);
Luka_Danilovic 4:f74d88f5f629 99 Publisher ultrasound_pub_cb2("ultrasound/cb2", &ultrasound_msg_cb2);
Luka_Danilovic 4:f74d88f5f629 100 Publisher ultrasound_pub_cb3("ultrasound/cb3", &ultrasound_msg_cb3);
Luka_Danilovic 4:f74d88f5f629 101 Publisher ultrasound_pub_cb4("ultrasound/cb4", &ultrasound_msg_cb4);
Luka_Danilovic 0:215bdd87b602 102
Luka_Danilovic 0:215bdd87b602 103 /* Threads and Signals */
Luka_Danilovic 0:215bdd87b602 104 //Thread myThread (osPriorityNormal);
Luka_Danilovic 0:215bdd87b602 105 //EventFlags myEventFlag;
Luka_Danilovic 0:215bdd87b602 106
Luka_Danilovic 0:215bdd87b602 107 /* Function Prototypes */
Luka_Danilovic 8:49306a01c52a 108 void startSensors(); // Start the Sensors
Luka_Danilovic 8:49306a01c52a 109 void recordDist(); // Record the Distances (meters)
Luka_Danilovic 8:49306a01c52a 110 void checkCliff(); // Check the Cliff Sensors Readings
Luka_Danilovic 8:49306a01c52a 111 void advRosPub(); // Adverstise Ultrasound Topic
Luka_Danilovic 8:49306a01c52a 112 void setupRosMsg(); // Setup the message constant values
Luka_Danilovic 8:49306a01c52a 113 void setRosDist(); // Put distance in ROS messages
Luka_Danilovic 8:49306a01c52a 114 void setRosStamp(); // Put time into ROS messages
Luka_Danilovic 8:49306a01c52a 115 void publishRosMsg(); // Publish ROS messages
Luka_Danilovic 0:215bdd87b602 116
Luka_Danilovic 0:215bdd87b602 117 /*============================================================================*/
Luka_Danilovic 0:215bdd87b602 118 #endif // End of inclusion