#include <Wire.

h>
#include "filters.h"
#define startpin 2
#define noOfSensor 6
#define echoTimeOut 12000
#define
#define
#define
#define

Pr
Pp
Pd
Pa

0.030 //maximum roll angle/1000 in degrees

0.030 //maximum pitch angle/1000 in degrees
0.5
1 //maximum ascend/decend speed

double front_distance, back_distance, left_distance, right_distance, up_distance

, down_distance;// all in mm
double raw_OA_roll = 0, raw_OA_pitch = 0, roll_OA_altitude = 0;//raw OA outputs
double median_OA_roll = 0, median_OA_pitch = 0;//median filtered OA outputs
double OA_roll_angle = 0, OA_pitch_angle = 0;//final OA outputs,
double roll_without_velocity = 0, final_pitch = 0;
double distance[noOfSensor];//{front, back, left, right, up, down}
double filtered_velocity[noOfSensor];
double raw_velocity[noOfSensor];
double last_distance[noOfSensor];
double last_time[noOfSensor];
//double OA_output_buffer[4];
double RC_scale_down_factor = 1;
int state = 0;
filters OA_roll, OA_pitch, left_speed, right_speed, forward_speed, backward_spee
d;
void requestEvent()
{
int16_t data = OA_roll_angle * 100;
byte bytes[2] = { data >> 8 & 0xFF, data & 0xFF };
Wire.write(bytes, 2); // respond with message of 6 bytes
}
void setup() {
// put your setup code here, to run once:
for (int i = startpin; i < (startpin + noOfSensor * 2); i += 2)
{
pinMode(i, OUTPUT);
pinMode(i + 1, INPUT);
}
Wire.begin(0x44);
// join i2c bus with address #2
Wire.onRequest(requestEvent); // register event
Serial.begin(115200);
}
void loop() {
uint8_t count = 0;
for (int i = startpin; i < (startpin + noOfSensor * 2); i += 2) {
distance[count] = US_read(i) * 0.17;
raw_velocity[count] = -(distance[count] - last_distance[count])/

(millis() - last_time[count]);
last_time[count] = millis();
last_distance[count] = distance[count];
Serial.print(distance[count]);
Serial.print("\t");
count++;
}
//Serial.println();
double temp = left_speed.Median_Filter(raw_velocity[2]);
filtered_velocity[2] = left_speed.Final_Output(temp);
temp = right_speed.Median_Filter(raw_velocity[3]);
filtered_velocity[3] = right_speed.Final_Output(temp);
temp = forward_speed.Median_Filter(raw_velocity[0]);
filtered_velocity[0] = forward_speed.Final_Output(temp);
temp = backward_speed.Median_Filter(raw_velocity[1]);
filtered_velocity[1] = backward_speed.Final_Output(temp);
calculate_OA_output(distance[0], distance[1], distance[0], distance[5],
distance[4], distance[5]);
temp = OA_roll.Median_Filter(raw_OA_roll);
OA_roll_angle = OA_roll.Final_Output(temp);
OA_roll_angle = constrain(OA_roll_angle, -30, 30);
//
//
//
//

Serial.print(raw_velocity[3]);
Serial.print("
");
Serial.print(filtered_velocity[3]);
Serial.print("
");
Serial.println(OA_roll_angle);

}
double US_read(int pin) {
digitalWrite(pin, LOW);
delayMicroseconds(2);
digitalWrite(pin, HIGH);
delayMicroseconds(10);
digitalWrite(pin, LOW);
unsigned long time = micros();
//delayMicroseconds(5);
unsigned long echo = pulseIn(pin + 1, HIGH, echoTimeOut);
return (micros() - time <= echoTimeOut) ? (double)echo : (double)echoTim
eOut;
}
void calculate_OA_output(double front_distance, double back_distance, double lef
t_distance, double right_distance, double up_distance, double down_distance)
{
if (left_distance + right_distance > 2000)//no obstacles on both left an
d right, OR, obstacles are only on one side
{
if ((left_distance < right_distance) && (left_distance < 1000))
//if obstacles on the left side only
{
state = 1;
RC_scale_down_factor = 0.9;

raw_OA_roll = (1000 - left_distance + filtered_velocity[

2] * Pd * 1000) * Pr;
roll_without_velocity = (1000 - left_distance) * Pr;
}
else if ((right_distance < left_distance) && (right_distance < 1
000)) //if obstacles on the right side only
{
state = 2;
RC_scale_down_factor = 0.9;
raw_OA_roll = -(1000 - right_distance + filtered_velocit
y[3] * Pd * 1000) * Pr;
roll_without_velocity = -(1000 - right_distance) * Pr;
}
else//there is no obstacles on both left and right
{
state = 3;
RC_scale_down_factor = 1;
raw_OA_roll = 0;
}
}
else if ((left_distance + right_distance <= 2000) && ((abs(left_distance
- right_distance) > 0.05*(left_distance + right_distance)))) //obstacles on bot
h left and right sides, AND the drone is NOT NEAR THE CENTRE
{
state = 4;
double horizontal_velocity = (filtered_velocity[2] - filtered_ve
locity[3]) * 0.5; //towards left is positive
RC_scale_down_factor = (2000 - left_distance - right_distance)/1
000 + 1;//to scale down RC input when the corridor is small (i.e. close to obsta
cles)
raw_OA_roll = (-(left_distance - right_distance) / (RC_scale_dow
n_factor * (left_distance + right_distance)) + 0.5 * horizontal_velocity) * Pr *
1000; //always trys to maintain at the centre of the two obstacles
roll_without_velocity = (-(left_distance - right_distance) / (RC
_scale_down_factor * (left_distance + right_distance))) * Pr * 1000;
}
else if ((left_distance + right_distance <= 2000) && ((abs(left_distance
- right_distance) <= 0.05*(left_distance + right_distance)))) //obstacles on bo
th left and right sides, BUT the drone is NEAR THE CENTRE
{
state = 5;
raw_OA_roll = 0;
RC_scale_down_factor = (2000 - left_distance - right_distance)/1
000 + 1;
}
}