RF22 Distance

// #include <SPI.h>
#include <RF22.h>
#include <RF22Router.h>
#define MY_ADDRESS 100
#define DESTINATION_ADDRESS 1

#define echoPin 2 // attach pin D2 Arduino to pin Echo of HC-SR04
#define trigPin 3 //attach pin D3 Arduino to pin Trig of HC-SR04


RF22Router rf22(MY_ADDRESS);

// Aloha
int sensorVal = 10;
long randNumber;
boolean successful_packet = false;
int max_delay=3000;

// defines distance sensor variables
long duration; // variable for the duration of sound wave travel
int distance; // variable for the distance measurement


void setup() {

  pinMode(trigPin, OUTPUT); // Sets the trigPin as an OUTPUT
  pinMode(echoPin, INPUT); // Sets the echoPin as an INPUT

  Serial.begin(9600);
  if (!rf22.init())
    Serial.println("RF22 init failed");
  // Defaults after init are 434.0MHz, 0.05MHz AFC pull-in, modulation FSK_Rb2_4Fd36
  if (!rf22.setFrequency(431.0))
    Serial.println("setFrequency Fail");
  rf22.setTxPower(RF22_TXPOW_20DBM);
  //1,2,5,8,11,14,17,20 DBM
  //rf22.setModemConfig(RF22::OOK_Rb40Bw335  );
  rf22.setModemConfig(RF22::GFSK_Rb125Fd125);
  //modulation

  // Manually define the routes for this network
  rf22.addRouteTo(DESTINATION_ADDRESS, DESTINATION_ADDRESS);
  sensorVal = analogRead(A0);                               // isws den xreiastei
  randomSeed(sensorVal);// (μία μόνο φορά μέσα στην setup)
}

void loop() {

  // Clears the trigPin condition
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigPin HIGH (ACTIVE) for 10 microseconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  // Calculating the distance
  distance = duration * 0.034 / 2; // Speed of sound wave divided by 2 (go and back)

  Serial.print("sensor Value: ");
  Serial.println(distance);
  Serial.println("1");

  char data_read[RF22_ROUTER_MAX_MESSAGE_LEN];
  uint8_t data_send[RF22_ROUTER_MAX_MESSAGE_LEN];
  memset(data_read, '\0', RF22_ROUTER_MAX_MESSAGE_LEN);
  memset(data_send, '\0', RF22_ROUTER_MAX_MESSAGE_LEN);
  sprintf(data_read, "%d", distance);
  data_read[RF22_ROUTER_MAX_MESSAGE_LEN - 1] = '\0';
  memcpy(data_send, data_read, RF22_ROUTER_MAX_MESSAGE_LEN);

  Serial.println("11");
  successful_packet = false;
  while (!successful_packet)
  {
    Serial.println("111");
    /*
    if (rf22.sendtoWait(data_send, sizeof(data_send), DESTINATION_ADDRESS) != RF22_ROUTER_ERROR_NONE)
    {
      Serial.println("2");
      Serial.println("sendtoWait failed");
      randNumber=random(200,max_delay);
      //Serial.println(randNumber);
      delay(randNumber);
    }
    else
    {
      Serial.println("3");
      successful_packet = true;
      Serial.println("sendtoWait Succesful");
    }
    */
  }
  Serial.println("1111");
}