ft. button press reset
Dependencies: mbed
Fork of BeaconDemo_RobotCodeNew by
Diff: programs.cpp
- Revision:
- 22:3643fc1c82b6
- Parent:
- 19:b5788427db67
- Child:
- 24:d98c6e733dd6
diff -r efe191c96cbb -r 3643fc1c82b6 programs.cpp --- 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); } }