Diff: programs.cpp

Revision:

22:3643fc1c82b6

Parent:

19:b5788427db67

--- 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);
         }  
     }