Swarm Robot Test

//Lukas Klett © 10.04.2016
//Swarm Robot Network

#include <NewPing.h>
#include <SPI.h>
#include "RF24Network.h"
#include "RF24.h"
#include "RF24Mesh.h"


//Maximale Distanz in cm Festlegen, alles was weiter entfernt ist ergibt den Wert 0
#define Max_Distance 50

//Ping Pin für Ultraschall (Ping und Echo sind kurzgeschlossen)
#define Ping_Pin 11

//Motor Pins
#define Motor_Right A1
#define Motor_Left A2

//Photo Dioden zum suchen von Lichtquellen
#define Led_Input_Left 7 //Analog Input
#define Led_Input_Right 8 //Analog Input

//Photo Dioden zum suchen von anderen Robotern
#define Led_Input_Left2 4
#define Led_Input_Right2 5

//Kommunikations LEDS
#define No_Connection_LED 2
#define Connection_LED 3

//Festlegen das beim Ultraschall Sensor der Ping und Echo Pin kurzgeschlossen sind
NewPing sonar(Ping_Pin, Ping_Pin, Max_Distance);

//Festlegen des CE und CS Pins
RF24 radio(7,8);
RF24Network network(radio);
RF24Mesh mesh(radio,network);

//Festlegen der Identifikations Namen
#define nodeID 1
#define otherNodeID 2
#define otherNodeID 3
#define otherNodeID 4
#define otherNodeID 5

struct payload_t {
  unsigned long ms;
  unsigned long counter;
};

uint32_t ctr=0;
uint32_t millisTimer=0;

//Variablen
int distance = 0;
int timer = 0; // Zähler damit der Robo abwechseln links und rechts herum fährt
int led_right_value = 0;
int led_left_value = 0;
int led_right_value2;
int led_left_value2;
int LD = 0; //Gesendete Licht Stärke
int LD1 = 0; //Empfangene Licht Stärke

void setup(){
  //LEDs zur Navigation
  pinMode(Led_Input_Left, INPUT);
  pinMode(Led_Input_Right, INPUT);
  pinMode(Led_Input_Left2, INPUT);
  pinMode(Led_Input_Right2, INPUT);


  //Kommunikations LEDS
  pinMode(No_Connection_LED, OUTPUT);
  pinMode(Connection_LED, OUTPUT);

  //Festlegen der Node IDs
  mesh.setNodeID(nodeID);
  mesh.begin();
}

void loop(){
  //Berechnen Ultraschall Sensor und Festlegen der Photo Dioden Inputs
  {
  unsigned int uS = sonar.ping(); //Sende Ping, bekomme ping Zeit in Microsekunden
  distance =(uS / US_ROUNDTRIP_CM); //Kovertiere Zeit zu Distanz, Achtung wenn Distanz == 0 dann Ziel außerhalb der Reichweite
  delay(50);
  }


  //Netzwerk Zeug
  mesh.update();
  mesh.DHCP();
  if(network.available()){
    RF24NetworkHeader header;
    network.peek(header);
    uint32_t LD=0;
    network.read(header,&LD,sizeof(LD));
    int _ID = mesh.getNodeID(header.from_node);

    if(LD = 1){
      LD1 = 1;
    }
    else{
      LD1 = 0;
    }
    }


  //Ausweich Funktion
  if (distance > 0 && distance < 5){ //Ausweich Funktion
    if (timer == 0){ // nach Links ausweichen
    analogWrite(Motor_Left, 0);
    analogWrite(Motor_Right, 100);
    }
    if (timer == 1){ // nach Rechts ausweichen
    analogWrite(Motor_Left, 100);
    analogWrite(Motor_Right, 0);
    }
  }
  else{
  //Wenn ein Signal von den anderen Robotern empfangen wurde das sie eine Lichtquelle empfangen haben dann fahr zu dieser, wenn nicht dann suche nach hellster Stelle
  if(LD1 == 1){
    led_left_value2 = analogRead(Led_Input_Left2);
    led_right_value2 = analogRead(Led_Input_Right2);
    if(led_left_value2 > led_right_value2){
       analogWrite(Motor_Left, 0);
       analogWrite(Motor_Right, 100);
    }
    if(led_left_value2 < led_right_value2){
      analogWrite(Motor_Left, 100);
      analogWrite(Motor_Right, 0);
    }
    }

    else{
      led_left_value = analogRead(Led_Input_Left);
      led_right_value = analogRead(Led_Input_Right);

    if(led_left_value > led_right_value){
       analogWrite(Motor_Left, 0);
       analogWrite(Motor_Right, 100);
    }

    if(led_left_value < led_right_value){
      analogWrite(Motor_Left, 100);
      analogWrite(Motor_Right, 0);
    }

    }
  }


  //Datenübertragung zu anderen Robotern
  if((millis() - millisTimer >= 1000) && LD1 == 0){
    millisTimer = millis();

    if(!mesh.write(&LD, 'LightDensity', sizeof(LD), otherNodeID)){

        if(!mesh.checkConnection()){
            mesh.renewAddress();
            {
            digitalWrite(No_Connection_LED, HIGH);
            digitalWrite(Connection_LED, LOW);
            delay(100);
            }
           }

        else{
            digitalWrite(No_Connection_LED, LOW);
            digitalWrite(Connection_LED, HIGH);
             }
           }
         }

         delay(1000);
       }