//Gets distance measurements from 4 sensors and prints those distances to the serial

port.
#include"DualMC33926MotorShield.h"
#include <NewPing.h>
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM9DS0.h>
#include <Adafruit_Simple_AHRS.h>

// Create LSM9DS0 board instance.

Adafruit_LSM9DS0 lsm(1000); // Use I2C, ID #1000

// Create simple AHRS algorithm using the LSM9DS0 instance's accelerometer and
magnetometer.
Adafruit_Simple_AHRS ahrs(&lsm.getAccel(), &lsm.getMag());

// Create motorShield object

DualMC33926MotorShield md;

#define delay1 1000

#define deg90 12
#define dist1 40
#define dist2 60
int i= 0;
#define deg45 6

// IR sensors
#define irLeft 11
#define irFrontLeft 12
#define irRight 13
#define irFrontRight 14
#define irFrontMiddle 15

//Trigger pins
#define ECHO_PIN23 23
#define ECHO_PIN25 25
#define ECHO_PIN27 27
#define ECHO_PIN29 29
#define ECHO_PIN31 31
#define ECHO_PIN33 33
#define ECHO_PIN35 35
#define ECHO_PIN37 37
#define ECHO_PIN39 39
#define ECHO_PIN41 41
//Echo pins
#define TRIGGER_PIN22 22
#define TRIGGER_PIN24 24
#define TRIGGER_PIN26 26
#define TRIGGER_PIN28 28
#define TRIGGER_PIN30 30
#define TRIGGER_PIN32 32
#define TRIGGER_PIN34 34
#define TRIGGER_PIN36 36
#define TRIGGER_PIN38 38
#define TRIGGER_PIN40 40

//Distance
#define MAX_DISTANCE 200//This might affect the time that it waits// max is 500

/////////////////////////////////////////////////////////////////////////////////////
///////INITITALIZE
VARIABLES//////////////////////////////////////////////////////////
//Ping variables
int USleftTop;
int USfrontTopLeft;
int USfrontTopMiddle;
int USfrontTopRight;
int USrightTop;
int USleftBottom;
int USfrontBottomLeft;
int USfrontBottomMiddle;
int USfrontBottomRight;
int USrightBottom;
//IR variables
double irleftdist;
double irfrontleftdist;
double irrightdist;
double irfrontrightdist;
double irfrontmiddledist;
//headings
float heading1;
float heading2;

//NewPing object array

NewPing sonar[10] = {
NewPing(TRIGGER_PIN22, ECHO_PIN23, MAX_DISTANCE),//Here we are initializing a class.
When a class is initiallized, an initial bit of code is run initially. This bit of
code is called the CONSTRUCTOR.//Must be fed an array for multiple sensors.
NewPing(TRIGGER_PIN24, ECHO_PIN25, MAX_DISTANCE),//sonar1 is the object name that we
created. It can be whatever we want it to be.
NewPing(TRIGGER_PIN26, ECHO_PIN27, MAX_DISTANCE),
NewPing(TRIGGER_PIN28, ECHO_PIN29, MAX_DISTANCE),
NewPing(TRIGGER_PIN30, ECHO_PIN31, MAX_DISTANCE),
NewPing(TRIGGER_PIN32, ECHO_PIN33, MAX_DISTANCE),
NewPing(TRIGGER_PIN34, ECHO_PIN35, MAX_DISTANCE),
NewPing(TRIGGER_PIN36, ECHO_PIN37, MAX_DISTANCE),
NewPing(TRIGGER_PIN38, ECHO_PIN39, MAX_DISTANCE),
NewPing(TRIGGER_PIN40, ECHO_PIN41, MAX_DISTANCE)};

/////////////////////////////////////////////////////////////////////////////FUNCTION
FOR GETTING PING DATA.////////////////////////////////////////////////////////
int Ping(int a)
{
//delay(50);
int microSec = sonar[a].ping();
//Serial.print("Ping: ");
//Serial.print(microSec / US_ROUNDTRIP_CM);

int cm = microSec / US_ROUNDTRIP_CM;

if (cm >= 120 || cm == 0){cm=120;}
Serial.println(cm);
return cm;
}

////////////////////////////////////////////////////////////////////////////FUNCTION
FOR GETTING IR DATA/////////////////////////////////////////////////////////////
double ir(int i)
{
int val = analogRead(i);
double dist = 187754 *pow(val, -1.51);
if(dist > 120){dist=120;}
//Serial.println(dist);
//delay(100);

return dist;
}
////////////////////////////////////////////////////////////////////////////FUNCTION
FOR GETTING MOTOR FAULT////////////////////////////////////////////////////////
void stopIfFault()
{
if (md.getFault())
{
Serial.println("Motor Fault");
 while(1);
}
}

///////////////////////////////////////////////////////////////// Function to
configure the sensors on the LSM9DS0 board.
////////////////////////////////////////////////////////
// You don't need to change anything here, but have the option to select different
range and gain values.
void configureLSM9DS0(void)
{
// 1.) Set the accelerometer range
lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_2G);

lsm.setupMag(lsm.LSM9DS0_MAGGAIN_2GAUSS);

lsm.setupGyro(lsm.LSM9DS0_GYROSCALE_245DPS);
}

////////////////////////////////////////////////////////////////////Function to get
heading(YAW)/////////////////////////////////////////////////////////////////////////
///

float getHeading(void)
{
sensors_vec_t orientation;//Not sure what this is for
if (ahrs.getOrientation(&orientation))
{
float headingVal = (-1)*orientation.heading;
//Serial.println(heading);
return headingVal;
}
}

////////////////////////////////////////////////////////Get data from all

sensors////////////////////////////////////////////////////////////////

void getAllSensorData(void)
{
//Call ping function
USleftTop = Ping(0);
USfrontTopLeft = Ping(1);
USfrontTopMiddle = Ping(2);
USfrontTopRight = Ping(3);
USrightTop = Ping(4);
USleftBottom = Ping(5);
 USfrontBottomLeft = Ping(6);
USfrontBottomMiddle = Ping(7);
USfrontBottomRight = Ping(8);
USrightBottom = Ping(9);
//Call IR function
irleftdist = ir(irLeft);
irfrontleftdist = ir(irFrontLeft);
irrightdist = ir(irRight);
irfrontrightdist = ir(irFrontRight);
// irfrontmiddledist = ir(irFrontMiddle);
}

/////////////////////////////////////////////////////////////////////////////////////
//////////////////////function of Sound1 (for object in
front)/////////////////////////////////////////////////////////////////
void sound1(){
while(true)
{
while ( i <300)
{
i++;
Serial.println("f");
digitalWrite(19, HIGH);
delayMicroseconds(400);
digitalWrite(19, LOW);
delayMicroseconds(400);
}
i = 0;
getAllSensorData();
if( ((USfrontBottomMiddle > dist1)&&(USfrontBottomMiddle != 0)) &&
((USfrontBottomLeft > dist1)&&(USfrontBottomLeft != 0)||(irfrontleftdist > dist1))
&& ((USfrontBottomRight > dist1)&&(USfrontBottomRight!=0)||(irfrontrightdist
>dist1)))
{break;}
delay(500);

}
}
/////////////////////////////////////////////////////////////////////////////////////
//////////////////////function of Sound (for going down narrow
hall)/////////////////////////////////////////////////////////////////

void sound2(){
while(true)
{
while ( i <300)
 {
i++;
Serial.println("f");
digitalWrite(19, HIGH);
delayMicroseconds(600);
digitalWrite(19, LOW);
delayMicroseconds(600);
}
i = 0;
getAllSensorData();
if ((((USrightTop > dist1) && (USrightTop !=0)) || ((irrightdist >
dist1)&&(irrightdist!=0))) && (((USleftTop > dist1) && (USleftTop !=0))
|| ((irleftdist > dist1)&&(irleftdist!=0))))
{break;}
delay(500);

}
}

/////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////SETUP////////////////////////////////////////
////////////////////////////////////

void setup() {
Serial.begin(115200);
md.init();
pinMode(19, OUTPUT);

// Initialise the LSM9DS0 board.

if(!lsm.begin())
{
// There was a problem detecting the LSM9DS0 ... check your connections
Serial.print(F("Ooops, no LSM9DS0 detected ... Check your wiring or I2C ADDR!"));
while(1);
}

configureLSM9DS0();// Setup the sensor gain and integration time.

md.setM1Speed(0);//SET INITIAL MOTOR SPEED
md.setM2Speed(0);
}

/////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////MAIN////////////////////////////////////////////////////
////////////////////////
void loop() {
getAllSensorData();
heading1 = getHeading(); //90DEG = ~12
heading2 = getHeading();
// Serial.println(heading2);
Serial.println(irleftdist);

/////////////////////////////////////////////////////////////////////////////////////
/////OBJECT SENSED BY LEFT
TOP//////////////////////////////////////////////////*/
if (((USleftTop < dist1) && (USleftTop !=0)) || (irleftdist <
dist1)&&(irleftdist!=0) || ((USleftBottom < dist1)&&(USleftBottom != 0)) )
{
heading1 = getHeading();//get heading before movement
md.setM1Speed(300);
while(true)//Repeat this loop until device moves given andgle
{
heading2 = getHeading();
int diff = (abs(heading2 - heading1));
getAllSensorData();
if ((diff > 12)||(USleftTop > dist1)||(irleftdist > dist1))
{
break;//break when device has turned 90 deg or the sensors that were sensing
are no longer sensing.
}
}
md.setM1Speed(0);
getAllSensorData();
}

/////////////////////////////////////////////////////////////////////////////////////
//////OBJECT SENSED BY RIGHT
TOP//////////////////////////////////////////////////*/
if (((USrightTop < dist1) && (USrightTop !=0)) || (irrightdist <
dist1)&&(irrightdist!=0) || ((USrightBottom < dist1) && (USrightBottom != 0)))
{
heading1 = getHeading();//get heading before movement
md.setM2Speed(300);
while(true)//Repeat this loop until device moves given andgle
{
heading2 = getHeading();
int diff = (abs(heading2 - heading1));
getAllSensorData();
 if ((diff > 12)||(USrightTop > dist1)||(irrightdist > dist1))
{
break;//break when device has turned 90 deg or the sensors that were sensing
are no longer sensing.
}
}
md.setM2Speed(0);
getAllSensorData();
}

/////////////////////////////////////////////////////////////////////////////////////
///OBJECT SENSED BY FRONT BOTTOM
LEFT//////////////////////////////////////////////////*/
if (((USfrontBottomLeft < dist1) && (USfrontBottomLeft !=0)) ||
(irfrontleftdist < dist1)&&(irfrontleftdist!=0) || ((USfrontTopLeft < dist1) &&
(USfrontTopLeft != 0)) )
{
heading1 = getHeading();//get heading before movement
md.setM1Speed(300);
while(true)//Repeat this loop until device moves given andgle
{
heading2 = getHeading();
int diff = (abs(heading2 - heading1));
getAllSensorData();
if ((diff > 12)||(USfrontBottomLeft > dist1)||(irfrontleftdist > dist1))
{
break;//break when device has turned 90 deg or the sensors that were sensing
are no longer sensing.
}
}
md.setM1Speed(0);
getAllSensorData();
}

///////////////////////////////////////////////////////////////////////////////OBJECT
SENSED BY FRONT BOTTOM
MIDDLE//////////////////////////////////////////////////*/
if ((USfrontBottomMiddle < dist2) && (USfrontBottomMiddle!=0 ) ||
((USfrontTopMiddle < dist2) && (USfrontTopMiddle != 0)))
{
heading1 = getHeading();//get heading before movement
md.setM1Speed(300);
while(true)//Repeat this loop until device moves given angle
{
heading2 = getHeading();
int diff = (abs(heading2 - heading1));
 getAllSensorData();
if ((diff > 12)||(USfrontBottomMiddle > dist1)||(USfrontBottomMiddle > dist1))
{
break;//break when device has turned 90 deg or the sensors that were sensing
are no longer sensing.
}
}
md.setM1Speed(0);
getAllSensorData();
}

/////////////////////////////////////////////////////////////////////////////////////
///////OBJECT SENSED BY FRONT BOTTOM
RIGHT//////////////////////////////////////////////////*/
if (((USfrontBottomRight < dist1) && (USfrontBottomRight !=0)) ||
((irfrontrightdist < dist1)&&(irfrontrightdist!=0)) || ((USfrontTopRight <
dist1)&&(USfrontTopRight != 0)))
{
heading1 = getHeading();//get heading before movement
md.setM2Speed(300);
while(true)//Repeat this loop until device moves given andgle
{
heading2 = getHeading();
int diff = (abs(heading2 - heading1));
getAllSensorData();
if ((diff > 12)||(USfrontBottomRight > dist1)||(irfrontrightdist > dist1))
{
break;//break when device has turned 90 deg or the sensors that were sensing
are no longer sensing.
}
}
md.setM2Speed(0);
getAllSensorData();
}

//////////////////////////////////////////////////////////////////////////////////OBJ
ECTS SENSED BY ALL FRONT
SENSORS//////////////////////////////////////////////////////////////////
//If object is detected by all front sensors
if ( ((USfrontBottomMiddle < dist2)&&(USfrontBottomMiddle!=0)) &&
((USfrontBottomLeft < dist1)&&(USfrontBottomLeft != 0)||(irfrontleftdist < dist1))
&& ((USfrontBottomRight < dist1)&&(USfrontBottomRight!=0)||(irfrontrightdist
<dist1)))
{
sound1();
}
//////////////////////////////////////////////////////////////////////////////////OBJ
ECTS ON BOTH SIDES//////////////////////////////////////////////////////////////////

if ((((USrightTop < dist1) && (USrightTop !=0)) || ((irrightdist <

dist1)&&(irrightdist!=0))) && (((USleftTop < dist1) && (USleftTop !=0))
|| ((irleftdist < dist1)&&(irleftdist!=0))))
{
sound2();
}

/////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////////////
//////OBJECT SENSED BY LEFT BOTTOM (facing
up)///////////////////////////////////////////////////
//if ((USleftBottom < dist1) && (USleftBottom != 0) )
//{
// }

/////////////////////////////////////////////////////////////////////////////////////
/////OBJECT SENSED BY RIGHT BOTTOM (facing
up)///////////////////////////////////////////////////
//if ((USrightBottom < dist1) && (USrightBottom != 0) )
//{
// }

/////////////////////////////////////////////////////////////////////////////////////
//////OBJECT SENSED BY FRONT TOP LEFT (facing
up)////////////////////////////////////////////////
//if ((USfrontTopLeft < dist1) && (USfrontTopLeft != 0) )
//{
// }

/////////////////////////////////////////////////////////////////////////////////////
//////OBJECT SENSED BY FRONT TOP MIDDLE (facing
up)///////////////////////////////////////////////////
//if ((USfrontTopMiddle < dist2) && (USfrontTopMiddle != 0) )
//{
// }

/////////////////////////////////////////////////////////////////////////////////////
//////OBJECT SENSED BY FRONT TOP RIGHT (facing
up)///////////////////////////////////////////////////
//if ((USfrontTopRight < dist1) && (USfrontTopRight != 0) )
//{
// }