Luka Danilovic / Mbed OS PROJ515_USS_GPS

Q

Dependencies:   SRF05 ros_lib_kinetic TinyGPSPlus

PROJ515.hpp@14:b972d0d0f63d, 2019-05-14 (annotated)

Committer:
Luka_Danilovic
Date:
Tue May 14 15:34:59 2019 +0000
Revision:
14:b972d0d0f63d
Parent:
13:78b9acd2e873 
Added two extra ultrasound sensors

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 9:2d9a0c9e5456 8 #include "TinyGPSPlus.h" // GPS Module Library
Luka_Danilovic 0:215bdd87b602 9 #include "ros.h" // ROS Library
Luka_Danilovic 0:215bdd87b602 10 #include "sensor_msgs/Range.h" // ROS Sensor Messages component
Luka_Danilovic 9:2d9a0c9e5456 11 #include "sensor_msgs/NavSatFix.h" // ROS Sensor Messages component
Luka_Danilovic 12:1a3272d67500 12
Luka_Danilovic 9:2d9a0c9e5456 13
Luka_Danilovic 0:215bdd87b602 14 /* Definitions */
Luka_Danilovic 0:215bdd87b602 15 #define LEDS_OFF 0x00 // LED bus all off
Luka_Danilovic 0:215bdd87b602 16 #define LEDS_ON 0x07 // LED bus all on
Luka_Danilovic 1:ea2ee36038e7 17
Luka_Danilovic 12:1a3272d67500 18 #define USP_CF1T PF_7 // Cliff ultrasound 1 Trigger
Luka_Danilovic 12:1a3272d67500 19 #define USP_CF1E PE_15 // Cliff ultrasound 1 Echo
Luka_Danilovic 12:1a3272d67500 20 #define USP_CF2T PF_8 // Cliff ultrasound 2 Trigger
Luka_Danilovic 12:1a3272d67500 21 #define USP_CF2E PE_12 // Cliff ultrasound 2 Echo
Luka_Danilovic 12:1a3272d67500 22 #define USP_CF3T PF_9 // Cliff ultrasound 3 Trigger
Luka_Danilovic 12:1a3272d67500 23 #define USP_CF3E PB_10 // Cliff ultrasound 3 Echo
Luka_Danilovic 12:1a3272d67500 24 #define USP_CF4T PG_1 // Cliff ultrasound 4 Trigger
Luka_Danilovic 12:1a3272d67500 25 #define USP_CF4E PB_11 // Cliff ultrasound 4 Echo
Luka_Danilovic 1:ea2ee36038e7 26
Luka_Danilovic 14:b972d0d0f63d 27 #define USP_CB1T PE_2 // Curb ultrasound 1 Trigger
Luka_Danilovic 14:b972d0d0f63d 28 #define USP_CB1E PG_14 // Curb ultrasound 1 Echo
Luka_Danilovic 14:b972d0d0f63d 29 #define USP_CB2T PE_4 // Curb ultrasound 2 Trigger
Luka_Danilovic 14:b972d0d0f63d 30 #define USP_CB2E PG_9 // Curb ultrasound 2 Echo
Luka_Danilovic 12:1a3272d67500 31 #define USP_CB3T PE_6 // Curb ultrasound 3 Trigger
Luka_Danilovic 12:1a3272d67500 32 #define USP_CB3E PE_7 // Curb ultrasound 3 Echo
Luka_Danilovic 12:1a3272d67500 33 #define USP_CB4T PE_5 // Curb ultrasound 4 Trigger
Luka_Danilovic 12:1a3272d67500 34 #define USP_CB4E PE_8 // Curb ultrasound 4 Echo
Luka_Danilovic 14:b972d0d0f63d 35 #define USP_CB5T PF_14 // Curb ultrasound 5 Trigger
Luka_Danilovic 14:b972d0d0f63d 36 #define USP_CB5E PE_11 // Curb ultrasound 5 Echo
Luka_Danilovic 14:b972d0d0f63d 37 #define USP_CB6T PE_9 // Curb ultrasound 6 Trigger
Luka_Danilovic 14:b972d0d0f63d 38 #define USP_CB6E PF_13 // Curb ultrasound 6 Echo
Luka_Danilovic 0:215bdd87b602 39
Luka_Danilovic 12:1a3272d67500 40 #define GPS_TX PD_5 // GPS Transmit Pin
Luka_Danilovic 12:1a3272d67500 41 #define GPS_RX PD_6 // GPS Recieve Pin
Luka_Danilovic 9:2d9a0c9e5456 42
Luka_Danilovic 9:2d9a0c9e5456 43 #define FOV 1.396 // Ultrasound sensor field of view
Luka_Danilovic 13:78b9acd2e873 44 #define MIN_RANGE 0.025 // Ultrasound sensor min range
Luka_Danilovic 14:b972d0d0f63d 45 #define MAX_RANGE 0.400 // Ultrasound sensor max range
Luka_Danilovic 14:b972d0d0f63d 46 #define CLIFF_TRH 0.120 // Robot cliff treshold
Luka_Danilovic 14:b972d0d0f63d 47 #define CLIFF_ZRO 0.000 // Cliff not existent
Luka_Danilovic 14:b972d0d0f63d 48 #define EXCL_ZONE 0.150 // Robot footprint exclusion zone
Luka_Danilovic 9:2d9a0c9e5456 49 #define ULTRASOUND 0x00 // Ultrasound
Luka_Danilovic 3:cd1f2bde7ac2 50
Luka_Danilovic 9:2d9a0c9e5456 51 #define UNAGUMENTED 0x00 // Unagumented
Luka_Danilovic 9:2d9a0c9e5456 52 #define GPS_CONSTELL 0x01 // GPS constellation
Luka_Danilovic 9:2d9a0c9e5456 53 #define UNNWN 0x00 // Unknown covariance
Luka_Danilovic 9:2d9a0c9e5456 54 #define CVX 0x01 // Covariance value for X from the datasheet
Luka_Danilovic 9:2d9a0c9e5456 55 #define CVY 0x01 // Covariance value for X from the datasheet
Luka_Danilovic 9:2d9a0c9e5456 56 #define CVZ 0x01 // Covariance value for X from the datasheet
Luka_Danilovic 9:2d9a0c9e5456 57 #define CVO 0x00 // Covariance value for zero components
Luka_Danilovic 0:215bdd87b602 58
Luka_Danilovic 9:2d9a0c9e5456 59 #define ROS_Baud 460800 // ROS Baud Rate
Luka_Danilovic 9:2d9a0c9e5456 60 #define GPS_Baud 9600 // GPS Baud Rate
Luka_Danilovic 12:1a3272d67500 61 //#define frameID_u "ultrasound_range"// ROS frame ID
Luka_Danilovic 9:2d9a0c9e5456 62 #define frameID_g "gps_odom" // ROS frame ID
Luka_Danilovic 9:2d9a0c9e5456 63
Luka_Danilovic 9:2d9a0c9e5456 64 //#define PRINT_DATA 0x01 // Thread communication event to print data
Luka_Danilovic 0:215bdd87b602 65
Luka_Danilovic 1:ea2ee36038e7 66 /* Namespaces */
Luka_Danilovic 1:ea2ee36038e7 67 using namespace std; // Make all std symbols visible
Luka_Danilovic 1:ea2ee36038e7 68 using namespace ros; // Make all ros symbols visible
Luka_Danilovic 1:ea2ee36038e7 69 using namespace sensor_msgs; // Make all sensor_msgs symbols visible
Luka_Danilovic 12:1a3272d67500 70 using namespace std_msgs; // Make all sdt_msgs symbols visible
Luka_Danilovic 1:ea2ee36038e7 71
Luka_Danilovic 0:215bdd87b602 72 /* Declarations */
Luka_Danilovic 2:7288dd12186e 73 extern float cfDist1; // Distance returned by cliff u_sensor1 (m)
Luka_Danilovic 2:7288dd12186e 74 extern float cfDist2; // Distance returned by cliff u_sensor2 (m)
Luka_Danilovic 2:7288dd12186e 75 extern float cfDist3; // Distance returned by cliff u_sensor3 (m)
Luka_Danilovic 2:7288dd12186e 76 extern float cfDist4; // Distance returned by cliff u_sensor4 (m)
Luka_Danilovic 1:ea2ee36038e7 77
Luka_Danilovic 2:7288dd12186e 78 extern float cbDist1; // Distance returned by curb u_sensor1 (m)
Luka_Danilovic 2:7288dd12186e 79 extern float cbDist2; // Distance returned by curb u_sensor2 (m)
Luka_Danilovic 2:7288dd12186e 80 extern float cbDist3; // Distance returned by curb u_sensor3 (m)
Luka_Danilovic 2:7288dd12186e 81 extern float cbDist4; // Distance returned by curb u_sensor4 (m)
Luka_Danilovic 14:b972d0d0f63d 82 extern float cbDist5; // Distance returned by curb u_sensor5 (m)
Luka_Danilovic 14:b972d0d0f63d 83 extern float cbDist6; // Distance returned by curb u_sensor6 (m)
Luka_Danilovic 1:ea2ee36038e7 84
Luka_Danilovic 2:7288dd12186e 85 extern float wPeriod; // Wait period(ms) for ROS publisher
Luka_Danilovic 0:215bdd87b602 86
Luka_Danilovic 12:1a3272d67500 87 extern char gps_c; // GPS stream character - Defined in PROJ515.cpp
Luka_Danilovic 12:1a3272d67500 88 extern int sats_n; // GPS satelite number - Defined in PROJ515.cpp
Luka_Danilovic 9:2d9a0c9e5456 89
Luka_Danilovic 0:215bdd87b602 90 /* GPIO */
Luka_Danilovic 0:215bdd87b602 91 DigitalIn bttn(USER_BUTTON); // Onboard blue user button
Luka_Danilovic 0:215bdd87b602 92 BusOut leds(LED1, LED2, LED3); // Onboard LEDs bus
Luka_Danilovic 0:215bdd87b602 93
Luka_Danilovic 0:215bdd87b602 94 /* Instantiations */
Luka_Danilovic 8:49306a01c52a 95 // Instance for ultrasound sensors
Luka_Danilovic 8:49306a01c52a 96 SRF05 USS_CF1(USP_CF1T, USP_CF1E); // Ultrasound sensor cliff 1
Luka_Danilovic 8:49306a01c52a 97 SRF05 USS_CF2(USP_CF2T, USP_CF2E); // Ultrasound sensor cliff 2
Luka_Danilovic 8:49306a01c52a 98 SRF05 USS_CF3(USP_CF3T, USP_CF3E); // Ultrasound sensor cliff 3
Luka_Danilovic 8:49306a01c52a 99 SRF05 USS_CF4(USP_CF4T, USP_CF4E); // Ultrasound sensor cliff 4
Luka_Danilovic 1:ea2ee36038e7 100
Luka_Danilovic 8:49306a01c52a 101 SRF05 USS_CB1(USP_CB1T, USP_CB1E); // Ultrasound sensor cliff 1
Luka_Danilovic 8:49306a01c52a 102 SRF05 USS_CB2(USP_CB2T, USP_CB2E); // Ultrasound sensor cliff 2
Luka_Danilovic 8:49306a01c52a 103 SRF05 USS_CB3(USP_CB3T, USP_CB3E); // Ultrasound sensor cliff 3
Luka_Danilovic 8:49306a01c52a 104 SRF05 USS_CB4(USP_CB4T, USP_CB4E); // Ultrasound sensor cliff 4
Luka_Danilovic 14:b972d0d0f63d 105 SRF05 USS_CB5(USP_CB5T, USP_CB5E); // Ultrasound sensor cliff 5
Luka_Danilovic 14:b972d0d0f63d 106 SRF05 USS_CB6(USP_CB6T, USP_CB6E); // Ultrasound sensor cliff 6
Luka_Danilovic 2:7288dd12186e 107
Luka_Danilovic 9:2d9a0c9e5456 108 // Instance for GPS
Luka_Danilovic 12:1a3272d67500 109 TinyGPSPlus gpsModule; // Instance of GPS class
Luka_Danilovic 12:1a3272d67500 110 Serial gpsSer(GPS_TX, GPS_RX); // Instance of Serial class to gps
Luka_Danilovic 9:2d9a0c9e5456 111
Luka_Danilovic 8:49306a01c52a 112 // Instance for ROS node and messages
Luka_Danilovic 0:215bdd87b602 113 NodeHandle nh; // Instance for ROS Node Handler
Luka_Danilovic 3:cd1f2bde7ac2 114
Luka_Danilovic 9:2d9a0c9e5456 115 Range ultrasound_msg_cf1; // Instance for ROS Range message 1
Luka_Danilovic 9:2d9a0c9e5456 116 Range ultrasound_msg_cf2; // Instance for ROS Range message 2
Luka_Danilovic 9:2d9a0c9e5456 117 Range ultrasound_msg_cf3; // Instance for ROS Range message 3
Luka_Danilovic 9:2d9a0c9e5456 118 Range ultrasound_msg_cf4; // Instance for ROS Range message 4
Luka_Danilovic 9:2d9a0c9e5456 119 Range ultrasound_msg_cb1; // Instance for ROS Range message 5
Luka_Danilovic 9:2d9a0c9e5456 120 Range ultrasound_msg_cb2; // Instance for ROS Range message 6
Luka_Danilovic 9:2d9a0c9e5456 121 Range ultrasound_msg_cb3; // Instance for ROS Range message 7
Luka_Danilovic 9:2d9a0c9e5456 122 Range ultrasound_msg_cb4; // Instance for ROS Range message 8
Luka_Danilovic 14:b972d0d0f63d 123 Range ultrasound_msg_cb5; // Instance for ROS Range message 9
Luka_Danilovic 14:b972d0d0f63d 124 Range ultrasound_msg_cb6; // Instance for ROS Range message 10
Luka_Danilovic 1:ea2ee36038e7 125
Luka_Danilovic 9:2d9a0c9e5456 126 NavSatFix gps_odom_msg; // Instance for ROS NavSatFix message
Luka_Danilovic 9:2d9a0c9e5456 127
Luka_Danilovic 9:2d9a0c9e5456 128 // Instance for ROS publishers
Luka_Danilovic 4:f74d88f5f629 129 Publisher ultrasound_pub_cf1("ultrasound/cf1", &ultrasound_msg_cf1);
Luka_Danilovic 4:f74d88f5f629 130 Publisher ultrasound_pub_cf2("ultrasound/cf2", &ultrasound_msg_cf2);
Luka_Danilovic 4:f74d88f5f629 131 Publisher ultrasound_pub_cf3("ultrasound/cf3", &ultrasound_msg_cf3);
Luka_Danilovic 4:f74d88f5f629 132 Publisher ultrasound_pub_cf4("ultrasound/cf4", &ultrasound_msg_cf4);
Luka_Danilovic 4:f74d88f5f629 133 Publisher ultrasound_pub_cb1("ultrasound/cb1", &ultrasound_msg_cb1);
Luka_Danilovic 4:f74d88f5f629 134 Publisher ultrasound_pub_cb2("ultrasound/cb2", &ultrasound_msg_cb2);
Luka_Danilovic 4:f74d88f5f629 135 Publisher ultrasound_pub_cb3("ultrasound/cb3", &ultrasound_msg_cb3);
Luka_Danilovic 4:f74d88f5f629 136 Publisher ultrasound_pub_cb4("ultrasound/cb4", &ultrasound_msg_cb4);
Luka_Danilovic 14:b972d0d0f63d 137 Publisher ultrasound_pub_cb5("ultrasound/cb5", &ultrasound_msg_cb5);
Luka_Danilovic 14:b972d0d0f63d 138 Publisher ultrasound_pub_cb6("ultrasound/cb6", &ultrasound_msg_cb6);
Luka_Danilovic 0:215bdd87b602 139
Luka_Danilovic 9:2d9a0c9e5456 140 Publisher gps_odom_pub("gps_odom", &gps_odom_msg);
Luka_Danilovic 9:2d9a0c9e5456 141
Luka_Danilovic 0:215bdd87b602 142 /* Threads and Signals */
Luka_Danilovic 0:215bdd87b602 143 //Thread myThread (osPriorityNormal);
Luka_Danilovic 0:215bdd87b602 144 //EventFlags myEventFlag;
Luka_Danilovic 0:215bdd87b602 145
Luka_Danilovic 0:215bdd87b602 146 /* Function Prototypes */
Luka_Danilovic 8:49306a01c52a 147 void startSensors(); // Start the Sensors
Luka_Danilovic 8:49306a01c52a 148 void recordDist(); // Record the Distances (meters)
Luka_Danilovic 8:49306a01c52a 149 void checkCliff(); // Check the Cliff Sensors Readings
Luka_Danilovic 8:49306a01c52a 150 void advRosPub(); // Adverstise Ultrasound Topic
Luka_Danilovic 8:49306a01c52a 151 void setupRosMsg(); // Setup the message constant values
Luka_Danilovic 8:49306a01c52a 152 void setRosDist(); // Put distance in ROS messages
Luka_Danilovic 8:49306a01c52a 153 void setRosStamp(); // Put time into ROS messages
Luka_Danilovic 8:49306a01c52a 154 void publishRosMsg(); // Publish ROS messages
Luka_Danilovic 0:215bdd87b602 155
Luka_Danilovic 0:215bdd87b602 156 /*============================================================================*/
Luka_Danilovic 0:215bdd87b602 157 #endif // End of inclusion