#include <Arduino.h> #include “BluetoothSerial.h” #include <ESP32Servo.h> #include <HardwareSerial.h> //#include <esp_now.h> //#include <Wire.h> // Check if Bluetooth configs are enabled /#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED) #error Bluetooth is not enabled! Please run make menuconfig to and enable it #endif/

#define RXD2 16 #define TXD2 17 static const int horzServo = 13; static const int vertServo = 18; #define SIGNAL_PIN 19 int const LED_Green = 4; int const LED_Red = 21; // Servo servo_1; Servo servo_2; float x11, y11, x22, y22; float orginaldist;

#define distance_limit 500 bool detect_status = 0; //bool detectflag = 0; BluetoothSerial SerialBT; String Command = “None”; String received = “"; String message; int move_counter = 0; int position_Count = 0; //TF mini data int dist; unsigned char check; unsigned char uart[9]; /* -- --save data measured by LiDAR-- -- -- -- -- -- - */ const int HEADER = 0x59; /* -- --frame header of data package-- -- -- -- -- --*/ int rec_debug_state = 0x01; // receive state for frame HardwareSerial TFmini(2); int ang = 0; int threshold = 0; const int arraysize = 12; bool command_flag = false; // int horzPos[10] = { 10, 15, 20, 25, 30, 35, 40, 45, 50, 60 }; // int vertPos[10] = { 150, 150, 150, 150, 150, 150, 150, 150, 150, 150 }; int horzPos[arraysize]; int vertPos[arraysize]; bool isConnected = false;

void startLaserBlink() { Serial.println(“Laser Starting Countdown!"); for (int i = 0; i < 3; i++) { digitalWrite(SIGNAL_PIN, HIGH); delay(100); digitalWrite(SIGNAL_PIN, LOW); delay(100); } }

void motorSelfCheck() { for (int posDegrees = 0; posDegrees <= 180; posDegrees++) { servo_1.write(posDegrees); delay(10); } for (int posDegrees = 180; posDegrees >= 0; posDegrees--) { servo_1.write(posDegrees); delay(10); }

for (int posDegrees = 180; posDegrees >= 120; posDegrees--) { servo_2.write(posDegrees); delay(10); } for (int posDegrees = 120; posDegrees <= 160; posDegrees++) { servo_2.write(posDegrees); delay(10); } Serial.println(“Motor self check done”); } //Not in use in code void moveServoPoints(int Pos_x[], int Pos_y[]) { for (int i = 0; i < 10; i++) { servo_1.write(Pos_x[i]); // Move horizontal servo delay(10); servo_2.write(Pos_y[i]); // Move vertical servo delay(10); // Small delay for servo to reach position } } //function in use in the c mainode void moveServoPositions(int i) { servo_1.write(horzPos[i]); // Move horizontal servo Serial.print(“horizantal position: “); Serial.println(horzPos[i]); delay(10); servo_2.write(vertPos[i]); // Move vertical servo Serial.print(“vertical position: “); Serial.println(vertPos[i]); delay(10); // Small delay for servo to reach position }

int Get_Lidar_data() {

if (TFmini.available()) // check if serial port has data input { if (rec_debug_state == 0x01) { // the first byte uart[0] = TFmini.read(); if (uart[0] == 0x59) { check = uart[0]; rec_debug_state = 0x02; } } else if (rec_debug_state == 0x02) { // the second byte uart[1] = TFmini.read(); if (uart[1] == 0x59) { check += uart[1]; rec_debug_state = 0x03; } else { rec_debug_state = 0x01; } } else if (rec_debug_state == 0x03) { uart[2] = TFmini.read(); check += uart[2]; rec_debug_state = 0x04; } else if (rec_debug_state == 0x04) { uart[3] = TFmini.read(); check += uart[3]; rec_debug_state = 0x05; } else if (rec_debug_state == 0x05) { uart[4] = TFmini.read(); check += uart[4]; rec_debug_state = 0x06; } else if (rec_debug_state == 0x06) { uart[5] = TFmini.read(); check += uart[5]; rec_debug_state = 0x07; } else if (rec_debug_state == 0x07) { uart[6] = TFmini.read(); check += uart[6]; rec_debug_state = 0x08; } else if (rec_debug_state == 0x08) { uart[7] = TFmini.read(); check += uart[7]; rec_debug_state = 0x09; } else if (rec_debug_state == 0x09) { uart[8] = TFmini.read(); if (uart[8] == check) { dist = uart[2] + uart[3] * 256; // the distance // strength = uart[4] + uart[5]*256;//the strength // temprature = uart[6] + uart[7] *256;//calculate chip temprature // temprature = temprature/8 - 256; Serial.print(“dist = “); Serial.println(dist); // output measure distance value of LiDAR //if() // Serial.print(“strength = “); // Serial.print(strength); //output signal strength value // Serial.print('\n’); // Serial.print("\t Chip Temprature = “); // Serial.print(temprature); // Serial.println(” celcius degree”); //output chip temperature of Lidar while (TFmini.available()) { TFmini.read(); // break; } // This part is added becuase some previous packets are there in the buffer so to clear serial buffer and get fresh data. delay(10); } rec_debug_state = 0x01; } } else Serial.println(“NO TFMinis data found”); return dist; }

int setReferenceDist() { int refVal = 0; for (int i = 0; i < 250; i++) {

refVal = Get_Lidar_data();
if (refVal == 0 || refVal > distance_limit)  //safe /protection logic not to get zero or very high ref value
  i--;
// Serial.print(" set reference :" );
//Serial.println( i);
delay(5);

} // Serial.print(” return refVal :” ); // Serial.println( refVal); return refVal; }

int calcRangeOfDetection(int refVal) { int startRefVal = refVal - 6; Serial.print(“Reference distance Value = “); Serial.println(refVal); Serial.print(“Start Value = “); Serial.println(startRefVal); Serial.print(“End Value = “); Serial.println(refVal - 2); return startRefVal; }

void detection(int startRefVal, int endrefVal) {

dist = 0; delay(10); uint32_t startMillisVal = millis(); // Initialize initial distance while (detect_status == 0) { int TFData = Get_Lidar_data();

// if (TFData >= abs(startRefVal) && TFData < abs(endrefVal))
if (SerialBT.available()) {
  break;
} else {
  if (TFData >= startRefVal && TFData < endrefVal) {
    // digitalWrite(SIGNAL_PIN, HIGH);
    detect_status = 1;
    //detectflag = 1;
    Serial.println("Object detected!");
    Serial.print("Object dist: ");
    Serial.println(TFData);
    uint32_t endMillisVal = millis();
    float responseTime = (endMillisVal - startMillisVal);  /// 1000.0;
    Serial.print("end millis: ");
    Serial.println(endMillisVal);
    Serial.print("start millis: ");
    Serial.println(startMillisVal);
    Serial.print("Response Time: ");
    Serial.println(responseTime);
    //ResponseTime[i] = responseTime;
  }
  delay(10);
}

} } //not in use void moveServo(int i, int j) { servo_1.write(i); servo_2.write(j); delay(100); } // int speed(float t1, float t2) { // } void ledBlink(int led, int n) { for (int i = 1; i <= n; i++) { digitalWrite(led, HIGH); delay(50); digitalWrite(led, LOW); delay(50); } } // Function to convert polar to Cartesian coordinates void polarToCartesian(float distance, float angle) { if (move_counter == 1) { x11 = distance * cos(radians(angle)); Serial.print(“x1 distance: “); Serial.println(x11); y11 = distance * sin(radians(angle)); Serial.print(“y1 distance: “); Serial.println(y11); } else if (move_counter == 2) { x22 = distance * cos(radians(angle)); Serial.print(“x2 distance: “); Serial.println(x22); y22 = distance * sin(radians(angle)); Serial.print(“y2 distance: “); Serial.println(y22); } }

// Function to calculate the hypotenuse (distance between two points) float calculateHypotenuse() { return sqrt(pow(x22 - x11, 2) + pow(y22 - y11, 2)); }

float speed(unsigned long originaltime) {

return (orginaldist / originaltime); }

void setMode() {

Serial.print(“Command :"); Serial.println(Command);

digitalWrite(SIGNAL_PIN, LOW); digitalWrite(LED_Red, LOW); if (Command == “Mode_1”) //Two predefined positions {

horzPos[0] = 90;
vertPos[0] = 140;
//moveServo( horzPos[0],  vertPos[0]);
//////////////////////////////


horzPos[1] = 0;
vertPos[1] = 160;
move_counter = 0;
position_Count = 2;
Serial.println("Mode_1 set.....");

} else if (Command == “Mode_2”) //Three predefined positions { // digitalWrite(SIGNAL_PIN, LOW); // digitalWrite(LED_Red,LOW); horzPos[0] = 90; horzPos[1] = 130; horzPos[2] = 0;

vertPos[0] = 150;
vertPos[1] = 90;
vertPos[2] = 160;
move_counter = 0;
position_Count = 3;
Serial.println("Mode_2 set.....");

} else if (Command == “rand”) { // digitalWrite(SIGNAL_PIN, LOW); // digitalWrite(LED_Red,LOW); for (int i = 0; i < 10; i++) {

  horzPos[i] = random(0, 180);
  vertPos[i] = random(120,160);
}
move_counter = 0;
position_Count = 10;
Serial.println("Mode_10 set.....");

} move_counter = 0; delay(10); }

void mode1() { float angle; float x, y; unsigned long starttime, endtime, originalTime; //Get_Lidar_data(); // Object detection range set up startLaserBlink(); digitalWrite(SIGNAL_PIN, HIGH); while (true) { int refVal = setReferenceDist(); // Serial.println(“refVal :” + refVal); int startRefVal = calcRangeOfDetection(refVal); int endrefVal = refVal - 2; //digitalWrite(SIGNAL_PIN, HIGH); // Detection begin detection(startRefVal, endrefVal); Serial.println(“detected the object”); if (detect_status) { if (move_counter == 2) { break; } if (move_counter == 0) {

    starttime = millis();
    x = Get_Lidar_data();
    angle = 90;
  } else if (move_counter == 1) {
    endtime = millis();
    x = Get_Lidar_data();
    angle = 0;
  }
  //motor rotation code here
  moveServoPositions(move_counter);
  move_counter++;
  ///////////////////////////////////////////////
  /*if (move_counter < position_Count - 1) {
    move_counter += 1;
  } else {
    move_counter = 0;
  }*/

  // detectflag = 0;
  detect_status = 0;
}

} /startLaserBlink(); int a[10] = { 90 }, b[10] = { 160 }; moveServoPoints(a, b);/ originalTime = endtime - starttime; Serial.print(“originalTime: “); Serial.println(originalTime); polarToCartesian(x, angle); orginaldist = calculateHypotenuse(); Serial.print(“speed of person:"); Serial.println(speed(originalTime)); digitalWrite(SIGNAL_PIN, LOW); Serial.print(“position_Count: “); Serial.println(position_Count); Serial.print(“move_counter: “); Serial.println(move_counter); } void mode2() { //Get_Lidar_data(); // Object detection range set up startLaserBlink(); digitalWrite(SIGNAL_PIN, HIGH); while (true) { int refVal = setReferenceDist(); // Serial.println(“refVal :” + refVal); int startRefVal = calcRangeOfDetection(refVal); int endrefVal = refVal - 2; //digitalWrite(SIGNAL_PIN, HIGH); // Detection begin detection(startRefVal, endrefVal); Serial.println(“detected the object”); if (detect_status) { if (move_counter == 3) { break; } //motor rotation code here moveServoPositions(move_counter); move_counter++; /if (move_counter < position_Count - 1) { move_counter += 1; } else { move_counter = 0; }/ // detectflag = 0; detect_status = 0; } } /startLaserBlink(); int a[10] = { 90 }, b[10] = { 160 }; moveServoPoints(a, b);/ digitalWrite(SIGNAL_PIN, LOW); Serial.print(“position_Count: “); Serial.println(position_Count); Serial.print(“move_counter: “); Serial.println(move_counter); }

void mode3() { startLaserBlink(); digitalWrite(SIGNAL_PIN, HIGH); while (true) { int refVal = setReferenceDist(); // Serial.println(“refVal :” + refVal); int startRefVal = calcRangeOfDetection(refVal); int endrefVal = refVal - 2; //digitalWrite(SIGNAL_PIN, HIGH); // Detection begin detection(startRefVal, endrefVal); Serial.println(“detected the object”); if (detect_status) { if (move_counter == 10) { break; } //motor rotation code here moveServoPositions(move_counter); move_counter++; /if (move_counter < position_Count - 1) { move_counter += 1; } else { move_counter = 0; }/ // detectflag = 0; detect_status = 0; } } /startLaserBlink(); int a[10] = { 90 }, b[10] = { 160 }; moveServoPoints(a, b);/ digitalWrite(SIGNAL_PIN, LOW); Serial.print(“position_Count: “); Serial.println(position_Count); Serial.print(“move_counter: “); Serial.println(move_counter); }

void setup() { Serial.begin(115200); SerialBT.begin(“HomeAgility_BT”); Serial.println(“Bluetooth Device is Ready to Pair”); //randomSeed(analogRead(0)); pinMode(SIGNAL_PIN, OUTPUT);

pinMode(LED_Green, OUTPUT); pinMode(LED_Red, OUTPUT);

TFmini.begin(115200, SERIAL_8N1, RXD2, TXD2); servo_1.attach(horzServo); servo_2.attach(vertServo); motorSelfCheck(); startLaserBlink(); ledBlink(LED_Green, 5); //Green LED self check # 5 time blinks ledBlink(LED_Red, 5); //Red LED self check

// digitalWrite(LED_Red, HIGH); // delay(500); // digitalWrite(LED_Red, LOW); delay(1000); }

void loop() {

if (SerialBT.hasClient()) { if (!isConnected) { isConnected = true; Serial.println(“Bluetooth connected!"); } } else { if (isConnected) { isConnected = false; Serial.println(“Bluetooth disconnected!"); } }

// Serial.println(“Starting of the loop...."); // int threshold=0; //Serial.println(“intial loop threshold:"); //Serial.println(threshold); if (SerialBT.available()) { // if (!command_flag) command_flag = true; digitalWrite(LED_Red, LOW); ledBlink(LED_Green, 3); Serial.println(“Mode change request...."); // String received = SerialBT.readStringUntil('\n’); String received = SerialBT.readStringUntil('\r’); SerialBT.read(); /* char incomingChar = SerialBT.read(); // if (incomingChar != ‘\n’) // if (incomingChar != ‘\n’ && incomingChar != ‘\r’) //{ // if (incomingChar ==’\r’) // Serial.println(“it is : N”); // if(incomingChar !=’\r’) // received+= String(incomingChar); // received= String(incomingChar);

// } else { // char incomingChar = SerialBT.read(); // char incomingChar = SerialBT.read(); // Serial.println(“ignore the non printable char.... " ); // received = “"; // }

while( incomingChar!= '\r')
{

   received+= String(incomingChar);
    incomingChar = SerialBT.read();
   
    delay(100);
}
    SerialBT.read();// ignore non printable char
   // SerialBT.read();
   //  Serial.println("ignore the non printable char.... " );

*/ Serial.println(“Received command: " + received); // digitalWrite(LED_Green, HIGH);

delay(100);


if (received == "m1") {
  Command = "Mode_1";
} else if (received == "m2") {
  Command = "Mode_2";
} else if (received == "r") {
  Command = "rand";
} else {
  Command = "Invalid";
}
Serial.print("command is:");
Serial.println(Command);
received = "";
Serial.println("####################################");
delay(10);
if (Command != "Invalid") {
  setMode();
} else {
  //invalid command section
  //   digitalWrite(SIGNAL_PIN, HIGH);
  // digitalWrite(LED_Red,HIGH);
  Serial.println("Invalid command");
}
//else {
if (command_flag == true) {
  if (Command == "Invalid") {
    Serial.println("Invalid command");
    digitalWrite(LED_Red, HIGH);
    Serial.println("laser blink");
    startLaserBlink();

    delay(10);
  }
  /*else {
    // Get_Lidar_data();
    // Object detection range set up
    startLaserBlink();
    int refVal = setReferenceDist();
    // Serial.println("refVal :" + refVal);
    int startRefVal = calcRangeOfDetection(refVal);
    int endrefVal = refVal - 2;
    digitalWrite(SIGNAL_PIN, HIGH);
    // Detection begin
    detection(startRefVal, endrefVal);
    if (detect_status) {
      digitalWrite(SIGNAL_PIN, LOW);
      //motor rotation code here
      moveServoPositions(move_counter);
      if (move_counter < position_Count - 1) {
        move_counter += 1;
      } else {
        move_counter = 0;
      }
      // detectflag = 0;
      detect_status = 0;
    }
    startLaserBlink();
    int a[10] = { 90 }, b[10] = { 160 };
    moveServoPoints(a, b);*/

  else if (Command == "Mode_1") {
    mode1();
  } else if (Command == "Mode_2") {
    mode2();
  } else if (Command == "rand") {
    mode3();
  }
  /* else if (Command == "rand") {

    digitalWrite(SIGNAL_PIN, LOW);
    delay(10);
    digitalWrite(SIGNAL_PIN, HIGH);
    delay(10);
    threshold = Get_Lidar_data();
    while (threshold == 0) {
      digitalWrite(SIGNAL_PIN, LOW);
      delay(10);
      digitalWrite(SIGNAL_PIN, HIGH);
      delay(10);
      threshold = Get_Lidar_data();
    }
    Serial.print("threshold :");
    Serial.println(threshold);
    //  int count = 0;
    while (true) {
      int lidarDistance = Get_Lidar_data();
      Serial.print("threshold :");
      Serial.println(threshold);
      Serial.print("lidarDistance :");
      Serial.println(lidarDistance);
      if (lidarDistance > 0 && lidarDistance < threshold) {
        // lidarDistance = Get_Lidar_data();
        ang = random(0, 181);
        moveServo(ang, ang);
        delay(10);
        //       Serial.print("servo moved.................");
        //       //ang += 30;
        //       //if(ang==180)
        //       // {
        //       //   ang=0;
        //       // }
        //       break;
      }

      //     delay(10);
    }
  }*/
  //   /*Serial.print("lidarDistance :");
  //   Serial.println(lidarDistance);*/
  /* else {
  // Get_Lidar_data();
  // Object detection range set up
  startLaserBlink();
  int refVal = setReferenceDist();
  // Serial.println("refVal :" + refVal);
  int startRefVal = calcRangeOfDetection(refVal);
  int endrefVal = refVal - 2;
  digitalWrite(SIGNAL_PIN, HIGH);
  // Detection begin
  detection(startRefVal, endrefVal);
  if (detect_status) {
    digitalWrite(SIGNAL_PIN, LOW);
    //motor rotation code here
    moveServoPositions(move_counter);
    if (move_counter < position_Count - 1) {
      move_counter += 1;
    } else {
      move_counter = 0;
    }
    // detectflag = 0;
    detect_status = 0;
  }
}*/
} else {

  //Command = None  section
  // if (Command == "None")
  // {
  digitalWrite(SIGNAL_PIN, LOW);
  digitalWrite(LED_Red, HIGH);

  Serial.println("No  command...system ready ..");
  // }
}

} // delay(500); }