Brainv01

#include <NewPing.h>
#include <Wire.h>

//L293D
//Motor A
const int motorPin1  = 5;  // Pin 14 of L293
const int motorPin2  = 6;  // Pin 10 of L293
//Motor B
const int motorPin3  = 10; // Pin  7 of L293
const int motorPin4  = 9;  // Pin  2 of L293


#define PAYLOAD_SIZE 14 // how many bytes to expect from each I2C salve node
#define NODE_MAX 6 // maximum number of slave nodes (I2C addresses) to probe
//#define START_NODE 2 // The starting I2C address of slave nodes
#define NODE_READ_DELAY 100 // Some delay between I2C node reads

int nodeArray[6][16] = {   // Node / Modules array
  {0x01,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, //Hallynode( i2c address, Mode, millis MSB, millis, millis, millis LSB, Data 0, 1, 2, 3, 4,5,6,7,8
  {0x02,3,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, //Ping Sensors Hallynode( i2c address, Mode, millis MSB, millis, millis, millis LSB, Data 0, 1, 2, 3, 4,5,6,7,8
  {0x03,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, //Hallynode( i2c address, Mode, millis MSB, millis, millis, millis LSB, Data 0, 1, 2, 3, 4,5,6,7,8
  {0x68,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, //Accelerometer
  {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, //spare
  {0x3C,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} //Screen
};

//NodeArray



//This will run only one time.
void setup(){

  //Set motorpins as outputs
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);
  pinMode(motorPin3, OUTPUT);
  pinMode(motorPin4, OUTPUT);

  Serial.begin(9600);
  Serial.println("MASTER READER NODE");
  Serial.print("Maximum Slave Nodes: ");
  Serial.println(NODE_MAX);
  Serial.print("Payload size: ");
  Serial.println(PAYLOAD_SIZE);
  Serial.println("***********************");

  Wire.begin();        // Activate I2C link

}



void loop(){
  getDataFromNodes(); //read data from nodes

  if(nodeArray[1][1]==3){ // if 3 ping sensors
    int leftsensor = nodeArray[1][7];//left 7
    int frontsensor = nodeArray[1][8];//front 8
    int rightsensor = nodeArray[1][9];//right 9
    Serial.println("sensors");
    Serial.println(leftsensor);
    Serial.println(frontsensor);
    Serial.println(rightsensor);

    int mindistance = 30;

    if(frontsensor >= mindistance || frontsensor == 0){ //if nothing infront

       if(leftsensor <= mindistance && leftsensor != 0){ //unless something to left
        right();
        if(rightsensor <= mindistance && rightsensor != 0){ //and something to right
          hardright();                            // spin right
        }
       }
       if(rightsensor <= mindistance && rightsensor != 0){ //unless something to right
        left();
        if(leftsensor <= mindistance && leftsensor != 0){ //and something to left
          hardright();                            // spin right
        }
       }
       if(rightsensor >= mindistance || rightsensor == 0){
        if(leftsensor >= mindistance || leftsensor == 0){
          forward(); //if nothing to the sides
        }
       }
    }

    if(frontsensor <= mindistance && frontsensor != 0){ //if something infront
      hardright();
       if(leftsensor <= mindistance && leftsensor != 0){ //and something to left
        hardright();
        if(rightsensor <= mindistance && rightsensor !=0){ //and something to right
          backward();                            // go backwards
        }
       }
       if(rightsensor <= mindistance && rightsensor != 0){ //and something to right
        hardleft();
        if(leftsensor <= mindistance && leftsensor !=0){ //and something to left
          backward();                            // go backwards
        }
       }
    }
  }
  //displayOnScreen();

  delay(NODE_READ_DELAY);
}


void getDataFromNodes(){
  for (int k=0; k < NODE_MAX-1; k++){
    if(nodeArray[k][1] != 0){
      Wire.requestFrom(nodeArray[k][0],PAYLOAD_SIZE); //request data from node
      if(Wire.available() == PAYLOAD_SIZE){
        for (int i = 0; i < PAYLOAD_SIZE; i++){
          nodeArray[k][i+2] = Wire.read();  // get nodes data
        }
      }//if wire available
    }//if mode != 0
  }//for #nodes
}



    //Motor Control -
    //Motor A: motorPin1,motorpin2
    //Motor B: motorpin3,motorpin4
void left(){
    //
    digitalWrite(motorPin1, HIGH);
    digitalWrite(motorPin2, LOW);
    digitalWrite(motorPin3, LOW);
    digitalWrite(motorPin4, LOW);
}
void hardleft(){
    //
    digitalWrite(motorPin1, HIGH);
    digitalWrite(motorPin2, LOW);
    digitalWrite(motorPin3, LOW);
    digitalWrite(motorPin4, HIGH);
    Serial.println("Hard Left");
}
void forward(){
    //
    digitalWrite(motorPin1, HIGH);
    digitalWrite(motorPin2, LOW);
    digitalWrite(motorPin3, HIGH);
    digitalWrite(motorPin4, LOW);
    Serial.println("Forward");
}
void backward(){
    //
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, HIGH);
    digitalWrite(motorPin3, LOW);
    digitalWrite(motorPin4, HIGH);
    Serial.println("Backward");
}
void right(){
    //
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, LOW);
    digitalWrite(motorPin3, HIGH);
    digitalWrite(motorPin4, LOW);
}
void hardright(){
    //
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, HIGH);
    digitalWrite(motorPin3, HIGH);
    digitalWrite(motorPin4, LOW);
    Serial.println("Hard Right");
}
void explode(){
    //And this code will stop motors
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, LOW);
    digitalWrite(motorPin3, LOW);
    digitalWrite(motorPin4, LOW);
}

void example(){
  delay(1000);
  forward();
  delay(100);
  left();
  delay(1000);
  hardleft();
  delay(1000);
  explode();
  delay(1000);
  hardright();
  delay(1700);
  right();
  delay(1000);
  backward();
  delay(2000);
  explode();
  delay(4000);
}