Merge commit

main/main.ino

@@ -10,6 +10,8 @@ int button_port = 0;
 bool stuck = false;
 int amber_LED = 4;
 int blink_counter = 0;
+unsigned long t = 0;
+unsigned long prev_millis = 0;
 
 //Motors
 Adafruit_MotorShield AFMS = Adafruit_MotorShield();
@@ -22,7 +24,7 @@ Adafruit_DCMotor *motors[] = {AFMS.getMotor(3), AFMS.getMotor(4)};
 #define black 0
 #define num_IR 4
 #define IR_floating_avg_num 3
-int IR_threshold[num_IR] = {600, 200, 500, 400};
+int IR_threshold[num_IR] = {300, 300, 300, 300};
 float IR_avgs[num_IR] = {0}; //LRFB
 int IR_colours[num_IR] = {0};
 int IR_locns[num_IR] = {A0, A3, A1, A2};
@@ -34,12 +36,12 @@ long cur_loop_count = 0;
 //US
 #define echoPin 2
 #define trigPin 3
-#define US_float_avg_num 5
+#define US_float_avg_num 1
 float duration;
 float new_US_distance;
 int US_index = 0;
 float US_distance_vals[US_float_avg_num] = {0};
-float US_distance_avg;
+float US_distance_avg = 0;
 
 //Metal Detector
 int metal_detector = 1;
@@ -47,6 +49,7 @@ int metal_float_counter = 0;
 #define metal_floating_avg_num 100
 float metal_avg = 0;
 float metal_vals[metal_floating_avg_num] = {0};
+int is_metal = 0;
 
 
 //Servo
@@ -54,6 +57,11 @@ float metal_vals[metal_floating_avg_num] = {0};
 Servo servoright;
 Servo servoleft;
 
+//Ramp
+int ramp_detect_threshold = 500; //Up, Down
+int ramp_detect_counter = 0;
+bool ramp_on = false;
+int left_right_velocity = 200;
 
 void setup() {
   // put your setup code here, to run once:
@@ -89,35 +97,117 @@ void loop() {
   }
 
   //servo.write(servo_position);
-  
-  while (true) {
-    blink_counter += 1;
+  if (region == 0 || region == 2) {
+    Serial.print("We are in region ");
+    Serial.println(region);
+    while (true) {
+      int return_code = basic_region(700, false);
+      if (return_code) {
+        continue;
+      }
 
-    if (blink_counter % 700 == 0) {
-      blink_counter = 0;
-      int curVal = digitalRead(amber_LED);
-      digitalWrite(amber_LED, !curVal);
+      //Update region 
+      t = millis() - prev_millis; //remember to change prev_millis later
+      if (t > 15000) {
+        ramp_on = true;
+      }
+      if (not ramp_on) {
+        continue;
+      }
+      ramp_detect_counter += 1;
+      if (ramp_detect_counter % 3000 == 0) {
+        Serial.println("US");
+        float US = update_region(ramp_detect_threshold);
+        if (US) {
+          break;
+        }
+      }
     }
-    check_push_button(motors);
-
-    update_IR_vals();
-    int IR_check_val = IR_check(IR_colours);
-    
-    //New code for stuck in a loop
-    check_if_in_loop(IR_check_val);
-    if (stuck) {
-      Serial.println("RECOVERY CODE");
-      move_both(motors, 255);
-      continue;
+  }
+  else if (region == 1) { //Junction detect region
+    Serial.print("We are in region ");
+    Serial.println(region);
+    while (true) {
+      int return_code = basic_region(700, true);
+      if (return_code == 1) {
+        Serial.println("THIS SHOULD NOT HAPPEN");
+        continue;
+      }
+      else if (return_code == 2) {
+        //region += 1;
+        Serial.println("IT WORKED");
+        break;
+      }
     }
+    //Add code to put servo down
+    //Add code to metal detect
+    /*
+    for(int i = 0; i < 1000; i++) {
+      metal_avg += update_metal_detector();
+    }
+    */
+    //Add code to rotate
+    for (int i = 0; i<1; i++) {
+      turn_around(200, right);
+    }
+    while(1);
+  }
+}
+
+bool recovery() {
+  if (stuck) {
+    Serial.println("RECOVERY CODE");
+    move_both(motors, 255);
+    ramp_on = true;
+    left_right_velocity = 200;
+    ramp_detect_counter = 0;
+    return true;
+  }
+  return false;
+}
 
-    bool repeat = check_if_repeat_IR(IR_check_val);
+int basic_region(int clock_cycle, bool junction) {
+  blink_amber(clock_cycle);
+  check_push_button(motors);
+  update_IR_vals();
+  int IR_check_val = IR_check(IR_colours);
+  check_if_in_loop(IR_check_val);
+  bool rec = recovery();
+  if (rec) {
+    return 1;
+  }
+  bool repeat = check_if_repeat_IR(IR_check_val);
+  int line_sense_return_code = line_sensing(motors, IR_check_val, repeat, left_right_velocity, junction);
+  if (line_sense_return_code) {
+    return 2;
+  }
+  return 0;
+}
 
-    line_sensing(motors, IR_check_val, repeat);
+void blink_amber(int clock_cycle) {
+  blink_counter += 1;
+  if (blink_counter % clock_cycle == 0) {
+    blink_counter = 0;
+    int curVal = digitalRead(amber_LED);
+    digitalWrite(amber_LED, !curVal);
   }
 }
 
-int line_sensing(Adafruit_DCMotor *motors[], int IR_check_val, bool repeat) {
+int update_region(int ramp_detect_threshold) {
+  float dist = check_ultrasonic_distance();
+  if (dist > ramp_detect_threshold) {
+    ramp_detect_counter = 0;
+    ramp_on = false;
+    region += 1;
+    left_right_velocity = 200;
+    return 1;
+  }
+  else {
+    return 0;
+  }
+}
+
+int line_sensing(Adafruit_DCMotor *motors[], int IR_check_val, bool repeat, int velocity, bool junction) {
   if (repeat) {
     return 0;
   }
@@ -127,15 +217,25 @@ int line_sensing(Adafruit_DCMotor *motors[], int IR_check_val, bool repeat) {
     Serial.println("FORWARD");
   }
   else if (IR_check_val == 1) {
-    move_one_motor(motors, 200, left);
+    move_one_motor(motors, velocity, left);
     Serial.println("TURN LEFT");
   }
   else if (IR_check_val == 2) {
-    move_one_motor(motors, 200, right);
+    move_one_motor(motors, velocity, right);
     Serial.println("TURN RIGHT");
   }
   else if (IR_check_val == 3) {
     Serial.println("Junction");
+    if (junction) {
+      Serial.println("STOP JUNCTIONNNNNNNN");
+      for(int j = 0; j < 2; j++) {
+        motors[j]->run(RELEASE);
+      }
+    return 1;
+    }
+    else {
+      move_both(motors, 150);
+    }
   }
   else if (IR_check_val == 4) {
     Serial.println("Ramp off");
@@ -146,20 +246,8 @@ int line_sensing(Adafruit_DCMotor *motors[], int IR_check_val, bool repeat) {
   else {
     move_both(motors, 150);
     Serial.println("Else block fml");
-    /*
-    for(int j = 0; j < 2; j++) {
-      motors[j]->setSpeed(0);
-    }
-    */
-    /*
-    for(int j = 0; j < num_IR; j++) {
-      Serial.print(IR_avgs[j]);
-      Serial.print(" -> ");
-    }
-    */
-    //Serial.println();
-    //while (1);
   }
+  return 0;
 }
 
 
@@ -224,15 +312,22 @@ void update_IR_vals() {
   }
 }
 
-void update_metal_detector() {
+int update_metal_detector() {
   unsigned long new_val = pulseIn(metal_detector, HIGH);
-  float new_val_Hz = 1000.0/new_val;
+  float new_val_Hz = 1000.0/float(new_val);
   metal_avg = floating_average(&metal_float_counter, metal_vals, metal_avg, new_val_Hz, metal_floating_avg_num);
-  
+  /*
+  if metal_avg in range metal:
+    return 1
+  else if non_metal
+    return -1
+  else
+    return 0
+  */
 }
 void check_push_button(Adafruit_DCMotor *motors[]) {
   button_val = digitalRead(button_port);
-  if (button_val == 1) {
+  if (button_val == 0) {
     Serial.println("Button pushed. Exiting");
     for(int j = 0; j < 2; j++) {
       motors[j]->run(RELEASE);
@@ -278,6 +373,7 @@ float check_ultrasonic_distance(void){
   duration = pulseIn(echoPin, HIGH);
   new_US_distance = duration * 0.034 / 2;
   US_distance_avg = floating_average(&US_index, US_distance_vals, US_distance_avg, new_US_distance, US_float_avg_num);
+  Serial.println(US_distance_avg);
   return US_distance_avg;
 }
 
@@ -288,7 +384,7 @@ int turn_around(int velocity, int side){
   // Else returns 0 if it continues turning too long without detecting junction
   int turn_around_count = 0; //measures time spent turning
   rotate(motors, velocity, side); // starts turning both motors to facilitate turning around
-  delay(10); //small delay to avoid detecting the junction again in the start
+  delay(200); //small delay to avoid detecting the junction again in the start
   while(true){
     turn_around_count += 1;
     update_IR_vals();
@@ -303,6 +399,10 @@ int turn_around(int velocity, int side){
     }
     if (turn_around_count > 5000){
       Serial.println("Loop Completed, Junction Not Detected");
+      move_both(motors, 0);
+      for(int j=0; j<2; j++) {
+        motors[j]->run(regions[0]);
+      }
       return 1;
     }
   }