Diff: main.cpp

Revision:

17:8831c676778b

Parent:

16:11282b7ee726

Child:

19:56bc8226b967

--- a/main.cpp	Sat Nov 23 15:33:23 2019 +0000
+++ b/main.cpp	Sat Nov 30 15:13:50 2019 +0000
@@ -43,9 +43,13 @@
 float sensor_back_reading;
 float sensor_left_reading;
 float sensor_right_reading;
+//initialised motor speed
 double speed=0.6;
+//used to change control to set distances of continous movement
 bool control_type = 1;;
+//used for the switch cases and the distance travelled
 int32_t motor_option;
+int32_t pulse = 6000;
 
 // Set motorpins for driving
 Motor A(PB_13, PB_15, PC_6, 1); // pwm, fwd, rev, can brake
@@ -97,6 +101,18 @@
     thread2.signal_set(1);
 }
 
+void pulse_callback(const std_msgs::Int32& distance){
+    
+    //default value of approx 30cm if no distance is recieved or to great
+    if((distance.data <= 0) or (distance.data>120)){
+        pulse = 6000;
+        }
+    else {
+        pulse = distance.data*200;
+    }
+    
+}
+
 void control_motor(void) {
     while (1)
         {   
@@ -105,7 +121,7 @@
                 //Forward 30cm
                 case 0://49
                     if (control_type == 1){
-                        move(-speed, -speed, 5960);
+                        move(-speed, -speed, pulse);
                         }
                     else if (control_type ==0){
                         move(-speed, -speed, 50);                    
@@ -115,27 +131,27 @@
                 //Left 30 degrees
                 case 1://50
                     if (control_type == 1 ){
-                        move(speed, -speed, 894);
+                        move(speed, -speed, 896);
                         }
                     else if (control_type == 0){
-                        move(speed, -speed, 50);
+                        move(speed, -speed, 100);
                         } 
                     break;
                     
                 //Right 30 degrees
                 case 2://51
                     if (control_type == 1 ){
-                        move(-speed, speed, 894);
+                        move(-speed, speed, 896);
                         }
                     else if (control_type == 0 ){
-                        move(-speed, speed, 30);
+                        move(-speed, speed, 60);
                         }     
                     break;
                     
                 // Reverse  30cm
                 case 3://52
                     if (control_type ==1 ){
-                        move(speed, speed, 5960);
+                        move(speed, speed, pulse);
                         }
                     else if (control_type ==0 ){
                         move(speed, speed, 50); 
@@ -157,12 +173,16 @@
                 case 6: // switch control type
                     control_type =! control_type;
                     break;
-                }
+                
+                case 7: //turn around
+                    if (control_type == 1 ){
+                        move(-speed, speed, 5376);
+                    break; 
     
-                /*
-                case 8: //control type 3
-                    control_type = 3;
-                */
+                case 8: //To show case 8 does nothing
+                    break;
+            }
+                    
             //Reset speed and pulse count
             A.speed(0);
             B.speed(0);
@@ -179,9 +199,12 @@
     nh.initNode();
     
     // ROS subscriber for motors control
+    ros::Subscriber<std_msgs::Int32> pulse_sub("distance_setting", &pulse_callback);
     ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &motor_callback);
 
+
     nh.subscribe(motor_sub);
+    nh.subscribe(pulse_sub);
     
     // load settings onto VL6180X sensors 
     sensor_forward.init();