Luka Danilovic / Mbed OS PROJ515_USS_GPS

Q

Dependencies:   SRF05 ros_lib_kinetic TinyGPSPlus

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