Face Tracking Robot

/***************************************************
 HUSKYLENS An Easy-to-use AI Machine Vision Sensor
 <https://www.dfrobot.com/product-1922.html>
 
 ***************************************************
 This example shows the basic function of library for HUSKYLENS via Serial.
 
 Created 2020-03-13
 By [Angelo qiao](Angelo.qiao@dfrobot.com)
 
 GNU Lesser General Public License.
 See <http://www.gnu.org/licenses></http:> for details.
 All above must be included in any redistribution
 ****************************************************/

/***********Notice and Trouble shooting***************
 1.Connection and Diagram can be found here
 <https://wiki.dfrobot.com/HUSKYLENS_V1.0_SKU_SEN0305_SEN0336#target_23>
 2.This code is tested on Arduino Uno, Leonardo, Mega boards.
 ****************************************************/

#include "HUSKYLENS.h"
#include "SoftwareSerial.h"
#include <AFMotor.h>

AF_DCMotor motor1(1, MOTOR12_1KHZ);
AF_DCMotor motor2(2, MOTOR12_1KHZ);
AF_DCMotor motor3(3, MOTOR34_1KHZ);
AF_DCMotor motor4(4, MOTOR34_1KHZ);

HUSKYLENS huskylens;
SoftwareSerial mySerial(10, 9); // RX, TX
//HUSKYLENS green line >> Pin 10; blue line >> Pin 11
void printResult(HUSKYLENSResult result);

void setup() {
    Serial.begin(115200);
    mySerial.begin(9600);
    while (!huskylens.begin(mySerial))
    {
        Serial.println(F("Begin failed!"));
        Serial.println(F("1.Please recheck the \"Protocol Type\" in HUSKYLENS (General Settings>>Protocol Type>>Serial 9600)"));
        Serial.println(F("2.Please recheck the connection."));
        delay(100);
    }
}

void loop() {
    if (!huskylens.request()) Serial.println(F("Fail to request data from HUSKYLENS, recheck the connection!"));
    else if(!huskylens.isLearned()) Serial.println(F("Nothing learned, press learn button on HUSKYLENS to learn one!"));
    else if(!huskylens.available())

     {
    
    
     Serial.println(F("No block or arrow appears on the screen!"));
           
     Serial.println("vision body undetected.");
     motor1.setSpeed(0);
     motor1.run(RELEASE);
     motor2.setSpeed(0);
     motor2.run(RELEASE);
     motor3.setSpeed(0);
     motor3.run(RELEASE);
     motor4.setSpeed(0);
     motor4.run(RELEASE);
          }
    
    else
    {
        Serial.println(F("###########"));
        if (huskylens.available())
        {
            HUSKYLENSResult result = huskylens.read();
            printResult(result);

            int xAxis = result.xCenter;
            int yAxis = result.yCenter;
            int distance = result.height;
    

            Serial.print("xAxis");
            Serial.println(xAxis);
            Serial.print("yAxis");
            Serial.println(yAxis);
            Serial.print("distance");
            Serial.println(distance);
   
           if (xAxis > 200) {
            motor1.setSpeed(220);
            motor1.run(FORWARD);
            motor2.setSpeed(220);
            motor2.run(BACKWARD);
            motor3.setSpeed(220);
            motor3.run(FORWARD);
            motor4.setSpeed(220);
            motor4.run(BACKWARD);
            Serial.print("Left");
           // delay(10);
      
               }
           else if (xAxis < 140) {
            motor1.setSpeed(220);
            motor1.run(BACKWARD);
            motor2.setSpeed(220);
            motor2.run(FORWARD);
            motor3.setSpeed(220);
            motor3.run(BACKWARD);
            motor4.setSpeed(220);
            motor4.run(FORWARD);
            Serial.print("Right");
           // delay(10);
      
            }
           else if (distance > 110) {
            motor1.setSpeed(200);
            motor1.run(BACKWARD);
            motor2.setSpeed(200);
            motor2.run(BACKWARD);
            motor3.setSpeed(200);
            motor3.run(BACKWARD);
            motor4.setSpeed(200);
            motor4.run(BACKWARD);
            Serial.print("BackWard");
            //delay(10);
      

             }
           else if (distance < 80) {
            motor1.setSpeed(200);
            motor1.run(FORWARD);
            motor2.setSpeed(200);
            motor2.run(FORWARD);
            motor3.setSpeed(200);
            motor3.run(FORWARD);
            motor4.setSpeed(200);
            motor4.run(FORWARD);
            Serial.print("Forward");
            //delay(10);

             }
           else {
            motor1.setSpeed(0);
            motor1.run(RELEASE);
            motor2.setSpeed(0);
            motor2.run(RELEASE);
            motor3.setSpeed(0);
            motor3.run(RELEASE);
            motor4.setSpeed(0);
            motor4.run(RELEASE);
            Serial.print("Stop");
            //delay(10);
             }
  
    
     }
  

        }

            
    }

void printResult(HUSKYLENSResult result){
    if (result.command == COMMAND_RETURN_BLOCK){
        Serial.println(String()+F("Block:xCenter=")+result.xCenter+F(",yCenter=")+result.yCenter+F(",width=")+result.width+F(",height=")+result.height+F(",ID=")+result.ID);
    }
    else if (result.command == COMMAND_RETURN_ARROW){
        Serial.println(String()+F("Arrow:xOrigin=")+result.xOrigin+F(",yOrigin=")+result.yOrigin+F(",xTarget=")+result.xTarget+F(",yTarget=")+result.yTarget+F(",ID=")+result.ID);
    }
    else{
        Serial.println("Object unknown!");

    }
}