CHAPTER 7

APPENDIX A

The sensor array on action can be seen on this video;

Click Here (YouTube Link)

The Coding of The Sensor Array:

//Assistance was obtained from the Adafruit library samples

//Libraries

#include <Adafruit_MPU6050.h>

#include <Adafruit_Sensor.h>

#include <Wire.h>

#include Adafruit_MPU6050 mpu;

//Variables

//Variables for smoothing

float oldacc;

float avgacc;

float newacc;

//Ultrasonic sensor pins and variables

int trigPin1 = 4;

int echoPin1 = 3;

int trigPin2 = 7;

int echoPin2 = 6;

int trigPin3 = 9;

int echoPin3 = 8;

int trigPin4 = 10;

long duration1, duration2, duration3, duration4, cm1, cm2, cm3, cm4, inches1, inches2,
inches3, inches4;

//Motor driver pins

int enA = 22; //Enable pin for first motor

int in1 = 27; //control pin for first moto

int in2 = 26; //control pin for first motor

int in3 = 25; //control pin for second motor

int in4 = 24; //control pin for second motor

int enB = 23r; //Enable pin for second motor

//Motor speed value pins

int motor_speed;

int motor_speed1;

void setup(void) {

Serial.begin(115200); // To start serial consol at 115200 bit rate while (!Serial)

delay(10); // Will pause until serial console opens //Initializng the Motor Pins

pinMode(enA, OUTPUT);

pinMode(in1, OUTPUT);

pinMode(in2, OUTPUT);

pinMode(in3, OUTPUT);

pinMode(in4, OUTPUT);

pinMode(enB, OUTPUT);

//Initializing the HCSR04 Sensor Pins

pinMode(trigPin1, OUTPUT);

pinMode(echoPin1, INPUT);
pinMode(trigPin2, OUTPUT);

pinMode(echoPin2, INPUT);

pinMode(trigPin3, OUTPUT);

pinMode(echoPin3, INPUT);

pinMode(trigPin4, OUTPUT);

pinMode(echoPin4, INPUT);

Serial.println("MPU6050 test!");

// Try to initialize!

if (!mpu.begin()) {

Serial.println("Failed to find MPU6050 chip");

while (1) {

delay(10); } }

Serial.println("MPU6050 Found!");

//Calibration

mpu.setAccelerometerRange(MPU6050_RANGE_8_G);

Serial.print("Accelerometer range set to: ");

switch (mpu.getAccelerometerRange()) {

case MPU6050_RANGE_2_G:

Serial.println("+-2G");

break;

case MPU6050_RANGE_4_G:

Serial.println("+-4G");

break; \

case MPU6050_RANGE_8_G:
Serial.println("+-8G");

break;

case MPU6050_RANGE_16_G:

Serial.println("+-16G");

break;

mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

Serial.print("Filter bandwidth set to: ");

switch (mpu.getFilterBandwidth()) {

case MPU6050_BAND_260_HZ:

Serial.println("260 Hz");

break;

case MPU6050_BAND_184_HZ:

Serial.println("184 Hz

break;

case MPU6050_BAND_94_HZ:

Serial.println("94 Hz");

break;

case MPU6050_BAND_44_HZ:

Serial.println("44 Hz");

break;

case MPU6050_BAND_21_HZ:

Serial.println("21 Hz");

break;
case MPU6050_BAND_10_HZ:

Serial.println("10 Hz");

break;

case MPU6050_BAND_5_HZ:

Serial.println("5 Hz");

break;

Serial.println("");

delay(100);

void loop() {

/* Get new sensor events with the readings */

sensors_event_t a, g, temp;

mpu.getEvent(&a, &g, &temp);

/* Print out the values */

/* Serial.print("Acceleration X: ");

Serial.print(a.acceleration.x);

Serial.print(", Y: ");

Serial.print(a.acceleration.y);

Serial.print(", Z: ");

Serial.print(a.acceleration.z);

Serial.println(" m/s^2"); */

//SMOOTHING PART (cycles 25 times, adds them to each other to take average) for (int
cycle = 0; cycle < 25; cycle++) {

sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);

//Calculating the angle of acceleration

float accxy = (a.acceleration.z * a.acceleration.z) + (a.acceleration.y * a.acceleration.y);

float denominator = sqrt (accxy);

float angle = atan2(a.acceleration.x, denominator); //angle is in terms of radian

float degree = angle * 180 / 3.14159265; //converting radian to degree //

Serial.println(degree);

oldacc = newacc + degree;

newacc = oldacc;

avgacc = oldacc / 25;

newacc = 0;

//Decision making part

if (avgacc > 95) {

climbing();

else if (avgacc < 80) {

climbing();

} else {

detection();

Serial.println("");

delay(10);

}
// Detecion function

void detection() {

// Set motors to 55

// For PWM maximum possible values are 0 to 255

//Robot will turn right constantly to detect the stairs

analogWrite(enA, 55);

analogWrite(enB, 55);

digitalWrite(in1, HIGH);

digitalWrite(in2, LOW);

digitalWrite(in3, LOW);

digitalWrite(in4, HIGH);

//Ultrasonics sensors’ measuring part (each one for one sensor)

// The sensor is triggered by a HIGH pulse of 10 or more microseconds.

// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:

digitalWrite(trigPin1, LOW);

delayMicroseconds(5);

digitalWrite(trigPin1, HIGH);

delayMicroseconds(10);

digitalWrite(trigPin1, LOW);

pinMode(echoPin1, INPUT);

duration1 = pulseIn(echoPin1, HIGH);

cm1 = (duration1 / 2) / 29.1; // Divide by 29.1 or multiply by 0.0343

inches1 = (duration1 / 2) / 74; // Divide by 74 or multiply by 0.0135

digitalWrite(trigPin2, LOW);
delayMicroseconds(5);

digitalWrite(trigPin2, HIGH);

delayMicroseconds(10);

digitalWrite(trigPin2, LOW);

pinMode(echoPin2, INPUT); duration2 = pulseIn(echoPin2, HIGH);

cm2 = (duration2 / 2) / 29.1; // Divide by 29.1 or multiply by 0.0343

inches2 = (duration2 / 2) / 74; // Divide by 74 or multiply by 0.0135

digitalWrite(trigPin3, LOW);

delayMicroseconds(5);

digitalWrite(trigPin3, HIGH);

delayMicroseconds(10);
digitalWrite(trigPin3, LOW);

pinMode(echoPin3, INPUT); duration3 = pulseIn(echoPin3, HIGH);

cm3 = (duration3 / 2) / 29.1; // Divide by 29.1 or multiply by 0.0343

inches3 = (duration3 / 2) / 74; // Divide by 74 or multiply by 0.0135

digitalWrite(trigPin4, LOW);

delayMicroseconds(5);

digitalWrite(trigPin4, HIGH);

delayMicroseconds(10);

digitalWrite(trigPin4, LOW);

pinMode(echoPin4, INPUT);

duration4 = pulseIn(echoPin4, HIGH);

cm4 = (duration4 / 2) / 29.1; // Divide by 29.1 or multiply by 0.0343

inches4 = (duration4 / 2) / 74; // Divide by 74 or multiply by 0.0135

/*Serial.print(inches1); Serial.print("in1, ");

Serial.print(inches2);

Serial.print("in2, ");

Serial.print(inches3);

Serial.print("in3, ");

Serial.print(inches4);

Serial.print("in4, ");

Serial.print(cm1);

Serial.print("cm1 ");

Serial.print(cm2);

Serial.print("cm2 ");

Serial.print(cm3);

Serial.print("cm3 ");

Serial.print(cm4);

Serial.print("cm4 ");

Serial.println();*/

//Decision making part for if stairs found or not

Serial.print("SEARCHING");

if (cm1 == cm4 && cm3 == cm2)

{ Serial.print("Stairs found");

Serial.println("");

//Just to be sure that gyro is not zero and robot started to climbing the stairs

analogWrite(enA, 100);
analogWrite(enB, 100);

digitalWrite(in1, HIGH);

digitalWrite(in2, LOW);

digitalWrite(in3, HIGH);

digitalWrite(in4, LOW);

delay(300); digitalWrite(in1, LOW);

digitalWrite(in2, LOW);

digitalWrite(in3, LOW);

digitalWrite(in4, LOW);

void climbing() {

Serial.print("CLIMBING");

analogWrite(enA, 100);

analogWrite(enB, 100);

digitalWrite(in1, HIGH);

digitalWrite(in2, LOW);

digitalWrite(in3, HIGH);

digitalWrite(in4, LOW); }