diff --git a/main/main.ino b/main/main.ino index 586a78a8e2e7c15abe3ae4d5292020c9760afc09..83f7bc9d5d1ca0aa40b78a82c13af035f3b205c2 100644 --- a/main/main.ino +++ b/main/main.ino @@ -3,10 +3,13 @@ //General uint8_t regions[2] = {BACKWARD, FORWARD}; uint8_t regions_opposite[2] = {FORWARD, BACKWARD}; -int region = 0; +int region = 1; #define floating_avg_num 3 int button_val = 0; int button_port = 0; +int cur_status = 0; +long cur_loop_count = 0; +bool status_repeat = false; //Motors Adafruit_MotorShield AFMS = Adafruit_MotorShield(); @@ -54,6 +57,29 @@ void loop() { if (button_val == 0) { update_IR_vals(); int IR_check_val = IR_check(IR_colours); + + //New code for stuck in a loop + if (IR_check_val != 0 && cur_status == IR_check_val) { + cur_loop_count += 1; + if (cur_loop_count > 800) { + Serial.println("RECOVERY CODE"); + status_repeat = true; + } + else { + status_repeat = false; + } + } + else { + status_repeat = false; + cur_loop_count = 0; + cur_status = IR_check_val; + } + + if (status_repeat) { + move_both(motors, 255); + continue; + } + if (IR_check_val == 0) { move_both(motors, 255); Serial.println("FORWARD");