trying to move robot using the distance from camera to face-CodePudding

So, i am trying to move a dc motor using the distance from my face to a raspberry camera. I have the raspberry connected to an arduino mega via serial comunication. Currently i am testing only one dc motor. I should say that i am using an raspberry pi 3b with 1 gb of ram. This is the raspberry pi code:

`import numpy as np
import cv2 

USB_PORT = "/dev/ttyUSB0"  # Arduino Uno WiFi Rev2
# Imports
import serial

try:
   usb = serial.Serial(USB_PORT, 9600, timeout=2)
except:
   print("ERROR - Could not open USB serial port.  Please check your port name and permissions.")
   print("Exiting program.")
   exit()

Known_distance = 76.2

Known_width = 14.3


GREEN = (0, 255, 0)
RED = (0, 0, 255)
WHITE = (255, 255, 255)
BLACK = (0, 0, 0)


fonts = cv2.FONT_HERSHEY_COMPLEX

face_detector = cv2.CascadeClassifier("haarcascade_frontalface_default.xml")

def Focal_Length_Finder(measured_distance, real_width, width_in_rf_image):

    
    focal_length = (width_in_rf_image * measured_distance) / real_width
    return focal_length


def Distance_finder(Focal_Length, real_face_width, face_width_in_frame):

    distance = (real_face_width * Focal_Length)/face_width_in_frame

    
    return distance


def face_data(image):

    face_width = 0 

    
    gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)

    faces = face_detector.detectMultiScale(gray_image, 1.3, 5)

    for (x, y, h, w) in faces:

        cv2.rectangle(image, (x, y), (x w, y h), GREEN, 2)

        face_width = w


    return face_width


ref_image = cv2.imread("Ref_image.png")


ref_image_face_width = face_data(ref_image)

Focal_length_found = Focal_Length_Finder(
    Known_distance, Known_width, ref_image_face_width)




cap = cv2.VideoCapture(0)
if not cap.isOpened():
    print("Cannot open camera")
    exit()
while True:
    # Capture frame-by-frame
    ret, frame = cap.read()
    # if frame is read correctly ret is True
    if not ret:
        print("Can't receive frame (stream end?). Exiting ...")
        break
    # Our operations on the frame come here
    face_width_in_frame = face_data(frame)

    if face_width_in_frame != 0:
        Distance = Distance_finder(Focal_length_found, Known_width, face_width_in_frame)
        cv2.line(frame,(30,30),(230,30),RED,32)
        cv2.line(frame, (30, 30), (230, 30), BLACK, 28)
        cv2.putText(
            frame, f"Distance: {round(Distance,2)} CM", (30, 35),
        fonts, 0.6, GREEN, 2)

        if(Distance<50 ):
            print("back")
        elif(50<Distance<80):
            print("stop")
            usb.write(b'of') 
        elif(Distance>80):
            print("forward")
            usb.write(b'on')


    # Display the resulting frame
    cv2.imshow('frame', frame)
    
    if cv2.waitKey(1) == ord('q'):
        break
# When everything done, release the capture
cap.release()

cv2.destroyAllWindows()`

and the arduino code is this:

`#include <AFMotor.h>

AF_DCMotor motor(1);

void setup() {
  //mstop();
  Serial.begin(9600);

}

void loop() {
   if (Serial.available()) {  // check for incoming serial data
      String command = Serial.readString();
      if (command == "on"){
        forward();
      }else if(command=="of"){
       mstop();
      }}

}

void forward(){
  motor.setSpeed(200);
  motor.run(FORWARD);
}

void mstop(){
  motor.run(RELEASE);
}`

So, the problem is that when i run the python code it's working till the if statements, those one :

        `if(Distance<50 ):
            print("back")
        elif(50<Distance<80):
            print("stop")
            usb.write(b'of') 
        elif(Distance>80):
            print("forward")
            usb.write(b'on')`

and i know that the print statements work just fine, but the usb.write(b'of') and usb.write(b'on') parts are only working sometimes. I want them to send info to the arduino smoothly to the arduino, i tried using the sleep function but it won' t work.

I am expecting to start and to stop the motor based on the distance from the camera to my face, and to do this smoothly.

CodePudding user response：

Im bad at Python but maybe check that Distance is an int

CodePudding user response：

Try using 1 character like '1' or '0' (use single quote instead of double, Serial.read() in arduino instead of Serial.readString(), and char instead of String.

char command = Serial.read();
if (command == '1') {

Then usb.write(b'1') and usb.write(b'0') in Python

From my past experience, I had arduino readString()s like "on " instead of "on"