Newer
Older
#include <Adafruit_MotorShield.h>
uint8_t regions[1] = {BACKWARD};
uint8_t regions_opposite[1] = {FORWARD};
//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
#define IR_floating_avg_num 3
float IR_avgs[num_IR] = {0}; //LRFB
int IR_colours[num_IR] = {0};
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;
int US_index = 0;
float US_distance_vals[US_float_avg_num] = {0};
//Metal Detector
int metal_detector = 1;
int metal_float_counter = 0;
#define metal_floating_avg_num 100
float metal_avg = 0;
float metal_vals[metal_floating_avg_num] = {0};
//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:
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);
}
}
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]);
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 (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) { //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
}
}
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) {
Serial.println("TURN LEFT");
}
else if (IR_check_val == 2) {
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);
}
}
else if (IR_check_val == 4) {
Serial.println("Ramp off");
}
else if (IR_check_val == 5) {
Serial.println("Ramp on");
}
else {
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]);
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) {
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);
IR_colours[i] = black;
}
else {
IR_colours[i] = white;
}
}
}
unsigned long new_val = pulseIn(metal_detector, HIGH);
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);
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) {
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);
int turn_around(int velocity, int side){
// will be called when block collection junction is detected
// velocity and side variables control speed and sense of rotation
// returns 0 if it successfully turns around (i.e. detects junction again.
// 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(200); //small delay to avoid detecting the junction again in the start
Serial.println("Junction detected, turn around completed successfully");
move_both(motors, 0);
for(int j=0; j<2; j++) {
motors[j]->run(regions[0]);
}
return 0;
}
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]);
}