Recovery for stuck while going up the ramp

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