Luka Danilovic / Mbed OS PROJ515_USS_GPS

Q

Dependencies:   SRF05 ros_lib_kinetic TinyGPSPlus

PROJ515.cpp@2:7288dd12186e, 2019-05-02 (annotated)

Committer:
Luka_Danilovic
Date:
Thu May 02 11:37:55 2019 +0000
Revision:
2:7288dd12186e
Parent:
1:ea2ee36038e7
Child:
3:cd1f2bde7ac2
No ROS - All U_Sensors working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Luka_Danilovic 0:215bdd87b602 1 #include "PROJ515.hpp" // Contains all Libraries, Definitions & Function Prototypes
Luka_Danilovic 0:215bdd87b602 2
Luka_Danilovic 2:7288dd12186e 3 float cfDist1 = 0; // Distance returned by cliff u_sensor1 (m)
Luka_Danilovic 2:7288dd12186e 4 float cfDist2 = 0; // Distance returned by cliff u_sensor2 (m)
Luka_Danilovic 2:7288dd12186e 5 float cfDist3 = 0; // Distance returned by cliff u_sensor3 (m)
Luka_Danilovic 2:7288dd12186e 6 float cfDist4 = 0; // Distance returned by cliff u_sensor4 (m)
Luka_Danilovic 2:7288dd12186e 7
Luka_Danilovic 2:7288dd12186e 8 float cbDist1 = 0; // Distance returned by curb u_sensor1 (m)
Luka_Danilovic 2:7288dd12186e 9 float cbDist2 = 0; // Distance returned by curb u_sensor2 (m)
Luka_Danilovic 2:7288dd12186e 10 float cbDist3 = 0; // Distance returned by curb u_sensor3 (m)
Luka_Danilovic 2:7288dd12186e 11 float cbDist4 = 0; // Distance returned by curb u_sensor4 (m)
Luka_Danilovic 2:7288dd12186e 12
Luka_Danilovic 2:7288dd12186e 13 float wPeriod = 5; // Wait period(ms) for ROS publisher to finish publishing
Luka_Danilovic 0:215bdd87b602 14
Luka_Danilovic 0:215bdd87b602 15
Luka_Danilovic 0:215bdd87b602 16 void setupRosMsg()
Luka_Danilovic 0:215bdd87b602 17 {/*CODE*/}
Luka_Danilovic 0:215bdd87b602 18
Luka_Danilovic 0:215bdd87b602 19 int main()
Luka_Danilovic 0:215bdd87b602 20 {
Luka_Danilovic 2:7288dd12186e 21 //nh.getHardware()->setBaud(921600); // Set Baud Rate for ROS Serial
Luka_Danilovic 2:7288dd12186e 22 // nh.initNode(); // Initialise ROS Node Handler
Luka_Danilovic 2:7288dd12186e 23 // nh.advertise(ultrasound_pub); // Adverstise Odometry topic
Luka_Danilovic 2:7288dd12186e 24 //
Luka_Danilovic 2:7288dd12186e 25 // ultrasound_msg.header.frame_id = "uss_cb1";
Luka_Danilovic 2:7288dd12186e 26 // ultrasound_msg.radiation_type = 0x00;
Luka_Danilovic 2:7288dd12186e 27 // ultrasound_msg.field_of_view = ((80*3.14159)/180); //3.14159265358979323846
Luka_Danilovic 2:7288dd12186e 28 // ultrasound_msg.min_range = 0.02f;
Luka_Danilovic 2:7288dd12186e 29 // ultrasound_6msg.max_range = 2.5;
Luka_Danilovic 2:7288dd12186e 30 //
Luka_Danilovic 2:7288dd12186e 31 // while(!nh.connected()) { // While node handler is not connected
Luka_Danilovic 2:7288dd12186e 32 // nh.spinOnce(); // Attempt to connect and synchronise
Luka_Danilovic 2:7288dd12186e 33 // }
Luka_Danilovic 1:ea2ee36038e7 34
Luka_Danilovic 0:215bdd87b602 35 while(true) {
Luka_Danilovic 1:ea2ee36038e7 36
Luka_Danilovic 2:7288dd12186e 37 USS_CF1.start();
Luka_Danilovic 2:7288dd12186e 38 USS_CF2.start();
Luka_Danilovic 2:7288dd12186e 39 USS_CF3.start();
Luka_Danilovic 2:7288dd12186e 40 USS_CF4.start();
Luka_Danilovic 2:7288dd12186e 41
Luka_Danilovic 2:7288dd12186e 42 USS_CB1.start();
Luka_Danilovic 2:7288dd12186e 43 USS_CB2.start();
Luka_Danilovic 2:7288dd12186e 44 USS_CB3.start();
Luka_Danilovic 2:7288dd12186e 45 USS_CB4.start();
Luka_Danilovic 2:7288dd12186e 46
Luka_Danilovic 2:7288dd12186e 47 cfDist1 = USS_CF1.get_dist_cm();
Luka_Danilovic 2:7288dd12186e 48 cfDist2 = USS_CF2.get_dist_cm();
Luka_Danilovic 2:7288dd12186e 49 cfDist3 = USS_CF3.get_dist_cm();
Luka_Danilovic 2:7288dd12186e 50 cfDist4 = USS_CF4.get_dist_cm();
Luka_Danilovic 2:7288dd12186e 51
Luka_Danilovic 2:7288dd12186e 52 cbDist1 = USS_CB1.get_dist_cm();
Luka_Danilovic 2:7288dd12186e 53 cbDist2 = USS_CB2.get_dist_cm();
Luka_Danilovic 2:7288dd12186e 54 cbDist3 = USS_CB3.get_dist_cm();
Luka_Danilovic 2:7288dd12186e 55 cbDist4 = USS_CB4.get_dist_cm();
Luka_Danilovic 2:7288dd12186e 56
Luka_Danilovic 2:7288dd12186e 57 cfDist1 = cfDist1/100;
Luka_Danilovic 2:7288dd12186e 58 cfDist2 = cfDist2/100;
Luka_Danilovic 2:7288dd12186e 59 cfDist3 = cfDist3/100;
Luka_Danilovic 2:7288dd12186e 60 cfDist4 = cfDist4/100;
Luka_Danilovic 2:7288dd12186e 61
Luka_Danilovic 2:7288dd12186e 62 cbDist1 = cbDist1/100;
Luka_Danilovic 2:7288dd12186e 63 cbDist2 = cbDist2/100;
Luka_Danilovic 2:7288dd12186e 64 cbDist3 = cbDist3/100;
Luka_Danilovic 2:7288dd12186e 65 cbDist4 = cbDist4/100;
Luka_Danilovic 2:7288dd12186e 66
Luka_Danilovic 2:7288dd12186e 67 usbSer.printf("Cliff 1 = %f\n", cfDist1);
Luka_Danilovic 2:7288dd12186e 68 usbSer.printf("Cliff 2 = %f\n", cfDist2);
Luka_Danilovic 2:7288dd12186e 69 usbSer.printf("Cliff 3 = %f\n", cfDist3);
Luka_Danilovic 2:7288dd12186e 70 usbSer.printf("Cliff 4 = %f\n", cfDist4);
Luka_Danilovic 2:7288dd12186e 71
Luka_Danilovic 2:7288dd12186e 72 usbSer.printf("Curb 1 = %f\n", cbDist1);
Luka_Danilovic 2:7288dd12186e 73 usbSer.printf("Curb 2 = %f\n", cbDist2);
Luka_Danilovic 2:7288dd12186e 74 usbSer.printf("Curb 3 = %f\n", cbDist3);
Luka_Danilovic 2:7288dd12186e 75 usbSer.printf("Curb 4 = %f ", cbDist4);
Luka_Danilovic 2:7288dd12186e 76
Luka_Danilovic 2:7288dd12186e 77 wait(1);
Luka_Danilovic 2:7288dd12186e 78
Luka_Danilovic 2:7288dd12186e 79 puts("");
Luka_Danilovic 2:7288dd12186e 80 puts("");
Luka_Danilovic 2:7288dd12186e 81 puts("");
Luka_Danilovic 2:7288dd12186e 82 puts("");
Luka_Danilovic 2:7288dd12186e 83 puts("");
Luka_Danilovic 2:7288dd12186e 84 puts("");
Luka_Danilovic 2:7288dd12186e 85 puts("");
Luka_Danilovic 2:7288dd12186e 86 puts("");
Luka_Danilovic 2:7288dd12186e 87
Luka_Danilovic 2:7288dd12186e 88 //ultrasound_msg.range = cmDistance1;// m
Luka_Danilovic 2:7288dd12186e 89 // nh.spinOnce(); // Reccuring connect and synchronise
Luka_Danilovic 2:7288dd12186e 90 // ultrasound_msg.header.stamp = nh.now(); // Get current time
Luka_Danilovic 2:7288dd12186e 91 // ultrasound_pub.publish(&ultrasound_msg);
Luka_Danilovic 2:7288dd12186e 92
Luka_Danilovic 2:7288dd12186e 93 wait_ms(wPeriod);
Luka_Danilovic 0:215bdd87b602 94 }
Luka_Danilovic 0:215bdd87b602 95 }