#include <Adafruit_MotorShield.h> #include <Servo.h> //General uint8_t regions[1] = {BACKWARD}; uint8_t regions_opposite[1] = {FORWARD}; int region = 0; int button_val = 0; int button_port = 0; bool stuck = false; int amber_LED = 4; int blink_counter = 0; unsigned long t = 0; unsigned long prev_millis = 0; int GREEN_LED = 5; //Motors Adafruit_MotorShield AFMS = Adafruit_MotorShield(); Adafruit_DCMotor *motors[] = {AFMS.getMotor(3), AFMS.getMotor(4)}; #define left 0 //index value in motor pointer array #define right 1 //IR #define white 1 #define black 0 #define num_IR 4 #define IR_floating_avg_num 3 int IR_threshold[num_IR] = {300, 300, 1024, 1024}; float IR_avgs[num_IR] = {0}; //LRFB int IR_colours[num_IR] = {0}; int IR_locns[num_IR] = {A0, A3, A1, A2}; float IR_vals[num_IR][IR_floating_avg_num] = {0}; //check int IR_float_counter[num_IR] = {0}; int cur_status = 0; long cur_loop_count = 0; //US #define echoPin 2 #define trigPin 3 #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 = 0; //Metal Detector int metal_detector = 13; int metal_float_counter = 0; #define metal_floating_avg_num 1 float metal_avg = 0; float metal_vals[metal_floating_avg_num] = {0}; bool is_metal = false; //int is_metal = 0; //#define metal_lower_bound 30.0 //#define metal_upper_bound 34.0 //#define non_metal_lower_bound 36.0 //#define non_metal_upper_bound 40.0 #define metal_threshold 9.7 float min_val = 100; //Servo #define servo_position 180 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; //Turning around int turn_around_time = 2800; long turn_around_time_start; long turn_around_time_end; void setup() { // put your setup code here, to run once: Serial.begin(9600); if (!AFMS.begin()) { // create with the default frequency 1.6KHz // if (!AFMS.begin(1000)) { // OR with a different frequency, say 1KHz Serial.println("Could not find Motor Shield. Check wiring."); while (1); } for(int j=0; j<2; j++) { motors[j]->run(RELEASE); } for(int i=0; i< num_IR; i++) { pinMode(IR_locns[i], INPUT); } pinMode(button_port, INPUT); pinMode(trigPin, OUTPUT); pinMode(echoPin, INPUT); pinMode(amber_LED, OUTPUT); pinMode(GREEN_LED, OUTPUT); digitalWrite(GREEN_LED, LOW); servoright.attach(10); servoleft.attach(9); servoright.write(90); servoleft.write(90); } void loop() { // put your main code here, to run repeatedly: for(int i=0; i < 2*IR_floating_avg_num; i++) { //Get some values in before evaluation starts update_IR_vals(); } for(int j = 0; j<2; j++) { motors[j]->run(regions[0]); } move_both(motors, 150); //servo.write(servo_position); if (region == 0 || region == 2) { Serial.print("We are in region "); Serial.println(region); prev_millis = millis(); while (true) { int return_code = basic_region(700, false); if (return_code) { continue; } //Update region t = millis() - prev_millis; 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; } } } } else if (region == 1 || region == 3) { //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; } } //Moving robot back /* for(int j = 0; j < 2; j++) { motors[j]->run(regions_opposite[0]); } move_both(motors, 200); delay(200); move_both(motors, 0); for(int j = 0; j < 2; j++) { motors[j]->run(regions[0]); } */ //Move Servo bring_down_servos(65); for(int i = 0; i< 10000*metal_floating_avg_num; i++) { update_metal_detector(); } //Metal Detect for(int i = 0; i< 10000; i++) { update_metal_detector(); if (metal_avg < min_val) { min_val = metal_avg; Serial.println(min_val); } } if (min_val <= metal_threshold) { is_metal = true; Serial.println("METAL DETECTED"); } else { digitalWrite(GREEN_LED, HIGH); Serial.println("NOT DETECTED"); } turn_around(turn_around_time, 255); region += 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; } 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; } 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 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; } if (IR_check_val == 0) { move_both(motors, 255); Serial.println("FORWARD"); } else if (IR_check_val == 1) { move_one_motor(motors, velocity, left); Serial.println("TURN LEFT"); } else if (IR_check_val == 2) { 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"); move_both(motors, 0); return 1; } else { move_both(motors, 150); } } else if (IR_check_val == 4) { Serial.println("Ramp off"); } else if (IR_check_val == 5) { Serial.println("Ramp on"); } else { move_both(motors, 150); Serial.println("Else block fml"); } return 0; } void move_one_motor(Adafruit_DCMotor *motors[], int velocity, int side) { //May need to account for direction later motors[(side+1)%2]->setSpeed(velocity); motors[side]->setSpeed(0); } void move_both(Adafruit_DCMotor *motors[], int velocity) { for(int j=0; j<2; j++) { motors[j]->setSpeed(velocity); } } void rotate(Adafruit_DCMotor *motors[], int velocity, int side) { //May need to account for direction later //If you want to turn right, the left motor moves forward but the right motor moves in the other direction motors[side]->run(regions_opposite[0]); motors[(side+1)%2]->run(regions[0]); move_both(motors, velocity); } float floating_average(int *index, float vals[], float curAvg, float newVal, int arr_len) { curAvg = ((curAvg * float(arr_len)) - vals[*index] + newVal)/float(arr_len); vals[*index] = newVal; *index = (*index + 1) % arr_len; return curAvg; } int IR_check(int LRFB[]) { if (LRFB[0] == black && LRFB[1] == black && LRFB[2] == white && LRFB[3] == white) { return 0; } else if (LRFB[0] == white && LRFB[1] == black) { return 1; } else if (LRFB[0] == black && LRFB[1] == white) { return 2; } else if (LRFB[0] == white && LRFB[1] == white && LRFB[2] == white && LRFB[3] == white) { return 3; } else if (LRFB[0] == black && LRFB[1] == black && LRFB[2] == white && LRFB[3] == black) { return 4; } else if (LRFB[0] == black && LRFB[1] == black && LRFB[2] == black && LRFB[3] == white) { return 5; } else { return 6; } } void update_IR_vals() { for(int i=0; i<num_IR; i++) { int new_val = analogRead(IR_locns[i]); IR_avgs[i] = floating_average(&IR_float_counter[i], IR_vals[i], IR_avgs[i], new_val, IR_floating_avg_num); if (IR_avgs[i] > IR_threshold[i]) { IR_colours[i] = black; } else { IR_colours[i] = white; } } } int update_metal_detector() { unsigned long new_val = pulseIn(metal_detector, HIGH); 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); } void check_push_button(Adafruit_DCMotor *motors[]) { button_val = digitalRead(button_port); if (button_val == 0) { Serial.println("Button pushed. Exiting"); for(int j = 0; j < 2; j++) { motors[j]->run(RELEASE); } while (1); } } bool check_if_repeat_IR(int IR_check_val) { bool is_repeat_IR = false; if (IR_check_val == cur_status) { is_repeat_IR = true; } else { cur_status = IR_check_val; } return is_repeat_IR; } void check_if_in_loop(int IR_check_val) { if (IR_check_val != 0 && cur_status == IR_check_val) { //Remember to change value after binary cur_loop_count += 1; if (cur_loop_count > 5000) { stuck = true; } else { stuck = false; } } else { stuck = false; cur_loop_count = 0; } } float check_ultrasonic_distance(void){ digitalWrite(trigPin, LOW); delayMicroseconds(2); digitalWrite(trigPin, HIGH); delayMicroseconds(10); digitalWrite(trigPin, LOW); 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; } int turn_around(int turn_around_time, int turn_around_velocity){ turn_around_time_start = millis(); rotate(motors, turn_around_velocity, right); while(true){ turn_around_time_end = millis(); if (turn_around_time_end > turn_around_time_start + turn_around_time){ move_both(motors, 0); motors[0]->run(regions[0]); motors[1]->run(regions[0]); return 0; } } } void bring_down_servos(int angle){ servoright.write(angle); servoleft.write(180 - angle); } void bring_up_servos(void){ servoright.write(90); servoleft.write(90); }