Diff: main.cpp

Revision:

10:be9de79cf6b0

Parent:

8:57aa8a35d983

Child:

11:35809512ec11

--- a/main.cpp	Wed Oct 30 15:02:42 2019 +0000
+++ b/main.cpp	Tue Nov 12 13:55:17 2019 +0000
@@ -2,6 +2,7 @@
 #include <ros.h>
 #include <std_msgs/Empty.h>
 #include <std_msgs/Int32.h>
+#include <std_msgs/String.h>
 #include <mbed_custom_msgs/lidar_msg.h>
 
 #include "Sensor.h"
@@ -35,7 +36,7 @@
 Motor motor_left(PC_6, PB_15, PB_13);
 Motor motor_right(PA_15, PC_7, PB_4);
     
-    /*
+/*
 void avoidObstacle(const std_msgs::Empty& avoid_obstacle_msg)
 {    
     // When obstacle ahead
@@ -72,41 +73,57 @@
 }
 */
 
-//ros::Subscriber<std_msgs::Empty> avoid_obstacle_sub("avoid_obstacle", &avoidObstacle);
+/*
+TODO REMOVE COMMENT WHEN CUSTOM MSGS READY
+void controlMotor(const std_msgs::Int32& motor_dir)
+{
+    switch (motor_dir)
+    {
+        // Move forward
+        case 1:
+            motor_left.moveForward();
+            motor_right.moveForward();
+            break;
+    
+        // Move left
+        case 2:
+            motor_left.moveBackward();
+            motor_right.moveForward();      
+            break;
+            
+        // Move right
+        case 3:
+            motor_left.moveForward();
+            motor_right.moveBackward();
+            break;
+    }
+}
+*/
+
+/*
+TODO REMOVE COMMENT WHEN CUSTOM MSGS READY
+// ROS subscriber for motors control
+ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &controlMotor);
+*/
 
 int main() 
 {
     ros::NodeHandle  nh;
     nh.initNode();
     
+    // ROS publisher for sensor readings
     mbed_custom_msgs::lidar_msg int_lidar_msg;
     ros::Publisher lidar_pub("lidar_reading", &int_lidar_msg);
+    /*
+    std_msgs::Int32 int_msg;
+    ros::Publisher lidar_pub("lidar_reading", &int_msg);
+    */
     nh.advertise(lidar_pub);
-  
-  /*
-    std_msgs::Int32 int1_sensor_msg;
-    ros::Publisher range_forward_pub("sensor_forward", &int1_sensor_msg);
-    nh.advertise(range_forward_pub);
-    
-    std_msgs::Int32 int2_sensor_msg;
-    ros::Publisher range_back_pub("sensor_back", &int2_sensor_msg);
-    nh.advertise(range_forward_pub);
     
-    std_msgs::Int32 int3_sensor_msg;
-    ros::Publisher range_left_pub("sensor_left", &int3_sensor_msg);
-    nh.advertise(range_forward_pub);
-    
-    std_msgs::Int32 int4_sensor_msg;
-    ros::Publisher range_right_pub("sensor_right", &int4_sensor_msg);
-    nh.advertise(range_forward_pub);
-    
-    nh.subscribe(avoid_obstacle_sub);
-
-    int range1; 
-    int range2;
-    int range3;
-    int range4; 
-*/
+    /*
+    TODO REMOVE COMMENT WHEN CUSTOM MSGS READY
+    nh.subscribe(motor_sub);
+    */
 
     // load settings onto VL6180X sensors 
     sensor_forward.init();
@@ -128,29 +145,26 @@
     motor_right.setSpeed(0.5f);
   
     while (1)
-    {            
+    {        
         int_lidar_msg.sensor_forward  = sensor_forward.read();
         int_lidar_msg.sensor_right  = sensor_right.read();
         int_lidar_msg.sensor_back  = sensor_back.read();
         int_lidar_msg.sensor_left  = sensor_left.read();         
 
         lidar_pub.publish(&int_lidar_msg);
-        //pc.printf("Range forward = %d | range back = %d | range right = %d | range left = %d\r\n",range1, range3, range2, range4); 
-                  
+
         /*
-        int1_sensor_msg.data = range1;
-        int2_sensor_msg.data = range3;
-        int3_sensor_msg.data = range4;
-        int4_sensor_msg.data = range2;  
-        range_forward_pub.publish(&int1_sensor_msg);     
-        range_back_pub.publish(&int2_sensor_msg);  
-        range_left_pub.publish(&int3_sensor_msg);     
-        range_right_pub.publish(&int4_sensor_msg);  
+        int_msg.data = sensor_forward.read();
+        lidar_pub.publish(&int_msg);
+        */
+        
+        /*
+        int range;
+        range = sensor_forward.read();
+        pc.printf("Range = %d\r\n", range);
         */
         
         nh.spinOnce();
-        
-        //avoidObstacle();
               
         wait_ms(10); 
     }