Revision 22:3643fc1c82b6, committed 2017-03-08

Comitter:

GusZ92

Date:

Wed Mar 08 11:49:39 2017 +0000

Parent:

21:efe191c96cbb

Commit message:

1. Flocking tune up threshold value; 2. Change the random walk to obs_avoid(new added)

Changed in this revision

--- a/main.cpp	Mon Mar 06 13:52:55 2017 +0000
+++ b/main.cpp	Wed Mar 08 11:49:39 2017 +0000
@@ -98,7 +98,7 @@
                 if(target_reached == 1) set_program(1);
                 break;
             case 3:
-                curved_random_walk_with_interaction_program();
+                obs_avoid();
                 break;
             case 4:
                 straight_random_walk_with_interaction_program();
@@ -260,7 +260,7 @@
         display.write_string("FLOCKING");
         }
     else if(buffer[0] == 'E'){
-        main_program_state = 4;
+        main_program_state = 3;
         display.write_string("RANDOM WALK");
         }
     else if(buffer[0] == 'F'){

--- a/programs.cpp	Mon Mar 06 13:52:55 2017 +0000
+++ b/programs.cpp	Wed Mar 08 11:49:39 2017 +0000
@@ -6,6 +6,7 @@
 // programs.cpp - Various PsiSwarm Programs for Beautiful Meme Project
 
 #include "main.h"
+#include "psiswarm.h"
 
 int obstacle_avoidance_threshold = 300;
 int robot_avoidance_threshold = 2000;
@@ -59,8 +60,48 @@
     }
 }
 
+int ticks = 0;
+float c = 0.25;
+float avoidRange = 400;
+float roboSpeed = 0.5;
+int lightNum = 0;
+float side_factor = 0.5; //side ir sensor weight factor 
 
 
+void obs_avoid()
+{
+    float left_motor_speed = 0.5;
+    float right_motor_speed = 0.5;
+    
+    if (reflected_sensor_data[0] > 600){ //adjust because of high reading from robot 3
+        left_motor_speed = -0.5;
+        set_left_motor_speed(left_motor_speed);
+        set_right_motor_speed(right_motor_speed);
+        //display.write_string("Walking R");      
+    }
+    else if (reflected_sensor_data[7] > 600){
+        right_motor_speed = -0.5;
+        set_left_motor_speed(left_motor_speed);
+        set_right_motor_speed(right_motor_speed); 
+        //display.write_string("Walking L");     
+    }
+    else {
+        display.clear_display();
+        display.set_position(0,0);
+        float left_motor_speed = 0.5;
+        float right_motor_speed = 0.5;
+        int turnChance = rand() %100;
+        if (turnChance < 20) {
+            left_motor_speed = 0;
+        } else if (turnChance > 80) {
+            right_motor_speed = 0;
+        }
+        
+        set_left_motor_speed(left_motor_speed);
+        set_right_motor_speed(right_motor_speed);
+    }
+}
+
 
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 /// recharging_program
@@ -734,15 +775,17 @@
     char buffer[255];
     
         
-    if (reflected_sensor_data[0] > 300){
+    if (reflected_sensor_data[0] > 600){ //adjust because of high reading from robot 3
         left_motor_speed = -0.5;
         set_left_motor_speed(left_motor_speed);
-        set_right_motor_speed(right_motor_speed);      
+        set_right_motor_speed(right_motor_speed);
+        //display.write_string("Walking R");      
     }
-    else if (reflected_sensor_data[7] > 300){
+    else if (reflected_sensor_data[7] > 600){
         right_motor_speed = -0.5;
         set_left_motor_speed(left_motor_speed);
-        set_right_motor_speed(right_motor_speed);      
+        set_right_motor_speed(right_motor_speed); 
+        //display.write_string("Walking L");     
     }
     else if (robot_id == 1) {
         display.clear_display();
@@ -771,8 +814,8 @@
         } 
         else {
             //used for time based turn instead of location based
-            set_left_motor_speed(left_motor_speed);
-            set_right_motor_speed(right_motor_speed);
+            set_left_motor_speed(0.5);
+            set_right_motor_speed(0.5);
         }  
     }
 

--- a/programs.h	Mon Mar 06 13:52:55 2017 +0000
+++ b/programs.h	Wed Mar 08 11:49:39 2017 +0000
@@ -18,5 +18,7 @@
 void role_reset(void);
 void tag_game_program(void);
 void mockFlocking(void);
+void obs_avoid(void);
+
 
 #endif
\ No newline at end of file