Diff: main.cpp

Revision:

20:074a3638c702

Parent:

19:56bc8226b967

Child:

21:2dcd6d0d004d

Child:

23:d1dc248b4e54

diff -r 56bc8226b967 -r 074a3638c702 main.cpp
--- a/main.cpp	Tue Dec 03 09:39:58 2019 +0000
+++ b/main.cpp	Tue Dec 10 10:23:54 2019 +0000
@@ -80,7 +80,7 @@
     if( (msg.distance <= 0) or (msg.distance > 120) ) {
         pulse = 6000;
     } else {
-        pulse = msg.distance * 200;
+        pulse = msg.distance * 200; // Est 200 pulses per cm
     }
     
     //motor_option = motor_dir.data;
@@ -109,6 +109,141 @@
     }
 }
 
+void square()
+{
+    float current_pulse_reading = 0;
+    
+    // - is forward on our robot
+    
+    for(int i = 0; i<4 ; i++)
+        
+        {
+        current_pulse_reading = 0;
+        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)
+                {
+                return;
+                }
+        }
+        // need to set motor speed to 0 
+        A.speed(0);
+        B.speed(0);
+        wait(0.05);
+        
+        A.speed(-speed);
+        B.speed(speed);
+        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);
+}
+
+void dance()
+{
+    wait(5);// required for audacity to play music
+    
+    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();
+    }    
+}
+
+/*    float current_pulse_reading = 0;
+    while(abs(current_pulse_reading) <= 5388)
+    {
+    current_pulse_reading = wheel_A.getPulses();
+    A.speed(-speed);
+    B.speed(0.0);
+    }
+    wait(0.05);
+    current_pulse_reading = 0;
+    wheel_B.reset();
+    wheel_A.reset();
+    while(abs(current_pulse_reading) <= 5388)
+    {
+    current_pulse_reading = wheel_A.getPulses();
+    A.speed(speed);
+    B.speed(0);
+    }
+    wait(0.05);
+    wheel_B.reset();
+    wheel_A.reset();
+    current_pulse_reading = 0;
+    while(abs(current_pulse_reading) <= 5388)
+    {
+    current_pulse_reading = wheel_B.getPulses();
+    A.speed(0);
+    B.speed(-speed);
+    }
+    wait(0.05);
+    wheel_B.reset();
+    wheel_A.reset();
+    current_pulse_reading = 0;
+    while(abs(current_pulse_reading) <=5388)
+    {
+    current_pulse_reading = wheel_B.getPulses();
+    A.speed(0);
+    B.speed(speed);
+    }
+    wait(0.05);
+    wheel_B.reset();
+    wheel_A.reset();
+    current_pulse_reading = 0;
+    while(abs(current_pulse_reading) <21504)
+    {
+    current_pulse_reading = wheel_A.getPulses();
+    A.speed(-speed);
+    B.speed(speed);
+    }
+    wait(0.05);
+    wheel_B.reset();
+    wheel_A.reset();
+    current_pulse_reading = 0;
+    while(abs(current_pulse_reading) <6000)
+    {
+    current_pulse_reading = wheel_A.getPulses();
+    A.speed(-speed);
+    B.speed(-speed);
+    }
+    wait(0.05);
+    wheel_B.reset();
+    wheel_A.reset();
+    current_pulse_reading = 0;
+    while(abs(current_pulse_reading) <6000)
+    {
+    current_pulse_reading = wheel_A.getPulses();
+    A.speed(speed);
+    B.speed(speed);
+    }
+    wait(0.05);
+    A.speed(0);
+    B.speed(0);
+    */
+    
+
 // Function to set robot direction and distance according to data published on motor_control topic
 void control_motor(void) {
     
@@ -121,7 +256,7 @@
         case 0: 
             break;
             
-        //Forward for pulse/200cm
+        //Forward
         case 1://49
             if (control_type == 1){
                 move(-speed, -speed, pulse);
@@ -131,17 +266,17 @@
             } 
             break;
 
-        //Left pulse pulse/200/6.7 degrees 
+        //Left 
         case 2://50
             if (control_type == 1 ){
-                move(speed, -speed, pulse/6.7);
+                move(speed, -speed, pulse/6.7); // Divide by 6.7 to convert to degrees
             }
             else if (control_type == 0){
                 move(speed, -speed, 100);
             } 
             break;
                     
-        //Right pulse/200/6.7 degrees
+        //Right 
         case 3://51
             if (control_type == 1 ){
                 move(-speed, speed, pulse/6.7);
@@ -151,7 +286,7 @@
             }     
             break;
                     
-        // Reverse for pulse/200cm
+        // Reverse
         case 4://52
             if (control_type ==1 ){
                 move(speed, speed, pulse);
@@ -189,13 +324,14 @@
     
         // Draw a square        
         case 9:
-            for(int i = 0;i<4;i++){
-                move(-speed, -speed, 6000);
-                move(-speed,speed,896);
-            }
+            square();
+            break;
+        
+        case 10:
+            dance();
             break;
         }
-                    
+                   
         //Reset speed and pulse count
         A.speed(0);
         B.speed(0);