Luka Danilovic / Mbed OS PROJ515_USS_GPS

Q

Dependencies:   SRF05 ros_lib_kinetic TinyGPSPlus

PROJ515.hpp@12:1a3272d67500, 2019-05-09 (annotated)

Committer:
Luka_Danilovic
Date:
Thu May 09 18:37:25 2019 +0000
Revision:
12:1a3272d67500
Parent:
11:955c2ed70de2
Child:
13:78b9acd2e873
ROS- Working; USS- Working; GPS - Working; (Pins of USS' remaped to match the URDF model)

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