Luka Danilovic / Mbed OS PROJ515_USS_GPS

Q

Dependencies:   SRF05 ros_lib_kinetic TinyGPSPlus

PROJ515.hpp@10:7d954fba5e7a, 2019-05-08 (annotated)

Committer:
Luka_Danilovic
Date:
Wed May 08 13:55:17 2019 +0000
Revision:
10:7d954fba5e7a
Parent:
9:2d9a0c9e5456
Child:
11:955c2ed70de2
ROS - Working; USS - Working; GPS - Working; LED - Not supported by the board

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