Diff: main.cpp

Revision:

13:d41144f89195

Parent:

12:817da876ae2f

diff -r 817da876ae2f -r d41144f89195 main.cpp
--- a/main.cpp	Fri Nov 15 17:38:04 2019 +0000
+++ b/main.cpp	Tue Nov 19 09:28:39 2019 +0000
@@ -37,13 +37,16 @@
 Motor motor_left(PC_6, PB_15, PB_13);
 Motor motor_right(PA_15, PC_7, PB_4);
 
+// Distance settings
+int nbPulse = 5691;
 
-void pulse(int distance)
+
+void pulse(int pulse)
 {
     // Variable to allow for any pulse reading   
     float current_pulse_reading = wheel_B.getPulses();
     
-    while(abs(current_pulse_reading) <= distance)
+    while(abs(current_pulse_reading) <= pulse)
     {
         current_pulse_reading = wheel_A.getPulses();
     }       
@@ -68,7 +71,7 @@
         case 1:
             motor_left.moveForward();
             motor_right.moveForward();
-            pulse(5691); 
+            pulse(nbPulse); 
             break;
     
         // Move left
@@ -89,11 +92,16 @@
         case 4:
             motor_left.moveBackward();
             motor_right.moveBackward();
-            pulse(5691);
+            pulse(nbPulse);
             break;
     }
 }
 
+void setDistance(const std_msgs::Int32& distance)
+{
+    nbPulse = (int) 189.7 * distance.data;   
+}
+
 int main() 
 {
     ros::NodeHandle  nh;
@@ -105,9 +113,13 @@
     
     // ROS subscriber for motors control
     ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &controlMotor);
+    
+    // ROS subscriber for distance setting
+    ros::Subscriber<std_msgs::Int32> distance_sub("distance_setting", &setDistance);
 
     nh.advertise(lidar_pub);
     nh.subscribe(motor_sub);
+    nh.subscribe(distance_sub);
 
     // load settings onto VL6180X sensors 
     sensor_forward.init();