Diff: main.cpp

Revision:

21:2dcd6d0d004d

Parent:

20:074a3638c702

--- a/main.cpp	Tue Dec 10 10:23:54 2019 +0000
+++ b/main.cpp	Tue Dec 10 10:30:21 2019 +0000
@@ -47,7 +47,7 @@
 //Initialised motor speed
 double speed=0.6;
 //used to change control to set distances of continous movement
-bool control_type = 1;;
+bool control_type = 1;
 //used for the switch cases and the distance travelled
 int32_t motor_option;
 int32_t pulse = 6000;
@@ -83,7 +83,6 @@
         pulse = msg.distance * 200; // Est 200 pulses per cm
     }
     
-    //motor_option = motor_dir.data;
     thread2.signal_set(1);
 }
 
@@ -113,23 +112,17 @@
 {
     float current_pulse_reading = 0;
     
-    // - is forward on our robot
-    
-    for(int i = 0; i<4 ; i++)
-        
-        {
+    for(int i = 0; i<4 ; i++) {
         current_pulse_reading = 0;
-        while(abs(current_pulse_reading) <= 6000)
-        {
+        while(abs(current_pulse_reading) <= 6000){
             A.speed(-speed);
             B.speed(-speed);
             
             current_pulse_reading = wheel_A.getPulses();
             //Stops Robot moving forward if front sensor detects
-            if (sensor_forward_reading <255)
-                {
+            if (sensor_forward_reading <255){
                 return;
-                }
+            }
         }
         // need to set motor speed to 0 
         A.speed(0);
@@ -138,33 +131,35 @@
         
         A.speed(-speed);
         B.speed(speed);
-        while(abs(current_pulse_reading) <= 8688)
-        {
+        while(abs(current_pulse_reading) <= 8688){
             current_pulse_reading = wheel_A.getPulses();
         }
+        
         wheel_B.reset();
         wheel_A.reset();
-        }
-        A.speed(0);
-        B.speed(0);
-        wait(0.05);
+        
+    }
+    
+    // Stop motors
+    A.speed(0);
+    B.speed(0);
+    wait(0.05);
 }
 
 void dance()
 {
     wait(5);// required for audacity to play music
     
-    for (int i =0; i<5; i++)
-    {
+    for (int i =0; i<5; i++) {
         int distance  = (rand()%16000+2000);
         float A_speed = (rand()%2-1);
         float B_speed = (rand()%2-1);
         if((A_speed < 0.4) and (A_speed > -0.4)){
             A_speed = 0.4;
-            }
+        }
         if((B_speed < 0.4) and (B_speed > -0.4)){
             B_speed = -0.4;
-            }
+        }
         move(A_speed,B_speed,distance);
         wheel_B.reset();
         wheel_A.reset();
@@ -241,7 +236,7 @@
     wait(0.05);
     A.speed(0);
     B.speed(0);
-    */
+*/
     
 
 // Function to set robot direction and distance according to data published on motor_control topic
@@ -327,6 +322,7 @@
             square();
             break;
         
+        // Random dance
         case 10:
             dance();
             break;