CODEWITHSUNDEEP
pythonarduinoservo
primitiveProgrammer
primitiveProgrammer

Reputation: 151

Servos wont track face due to being stuck in another function

My code is simple, when Serial.available() is equal to 0, meaning that no data is coming through, execute function something(). But when Serial.read() gets any other number, this means that face is detected and it should stop executing function something() and start tracking face, execute function track().

When I run the code, and the camera detects no face, it does as I want it to, the servo rotate back and forth. But as soon as it detects a face, the servo ignores to move with the face and continues to run the something() function.

How do I get it to just follow my face when detected, and then go back to rotating back and forth when no face is detected. if I get rid of the something() function, the code works fine and the servos move in respect to the position of my face. But when I add in the something() function, it does not leave that function, even when face if detected.

*.ino

#include <Servo.h> 

Servo servoVer; //Vertical Servo
Servo servoHor; //Horizontal Servo
int x;
int y;
int prevX;
int prevY;
int pos = 0;


void setup()
{
  Serial.begin(9600);
  servoVer.attach(5); //Attach Vertical Servo to Pin 5
  servoHor.attach(6); //Attach Horizontal Servo to Pin 6
  servoVer.write(90);
  servoHor.write(90);
} 

void Pos()
{
  if (prevX != x || prevY != y)
  {
    int servoX = map(x, 450, 0, 0, 180);
    int servoY = map(y, 450, 0, 180, 0);
    servoX = min(servoX, 180);
    servoX = max(servoX, 0);
    servoY = min(servoY, 180);
    servoY = max(servoY, 0);
    servoHor.write(servoX);
    servoVer.write(servoY);
  }
}

void something(){
   for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
    servoHor.write(pos);              // tell servo to go to position in variable 'pos'
    delay(5);                       // waits 15ms for the servo to reach the position
  }
}

void track(){
    if(Serial.available() > 0);
    {
      if (Serial.read() == 'X')
      {
        x = Serial.parseInt();
        if (Serial.read() == 'Y')
        {
          y = Serial.parseInt();
          Pos();
        }
      }
      while (Serial.available() > 0)
      {
        Serial.read();
      }
    }
  }

void loop()
{
  if ( ! Serial.available() ) 
  {
    something();
  }
  else
  {
    track();
  }
}

*.py

import numpy as np
import serial
import time
import sys
import cv2

arduino = serial.Serial('COM5', 9600)
time.sleep(2)
print("Connection to arduino...")


face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')

cap = cv2.VideoCapture(0)

while 1:
    ret, img = cap.read()
    img = cv2.flip(img,1)
    cv2.resizeWindow('img', 500,500)
    cv2.line(img,(500,250),(0,250),(0,255,0),1)
    cv2.line(img,(250,0),(250,500),(0,255,0),1)
    cv2.circle(img, (250, 250), 5, (255, 255, 255), -1)
    gray  = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    faces = face_cascade.detectMultiScale(gray, 1.3)

    for (x,y,w,h) in faces:
        cv2.rectangle(img,(x,y),(x+w,y+h),(0,255,0),5)
        roi_gray  = gray[y:y+h, x:x+w]
        roi_color = img[y:y+h, x:x+w]

        arr = {y:y+h, x:x+w}
        print (arr)

        print ('X :' +str(x))
        print ('Y :'+str(y))
        print ('x+w :' +str(x+w))
        print ('y+h :' +str(y+h))

        xx = int((x+(x+h))/2)
        yy = int((y+(y+w))/2)

        print (xx)
        print (yy)

        center = (xx,yy)

        print("Center of Rectangle is :", center)
        data = "X{0:d}Y{1:d}Z".format(xx, yy)
        print ("output = '" +data+ "'")
        # arduino.write(data)
        arduino.write(data.encode())

    cv2.imshow('img',img)

    k = cv2.waitKey(30) & 0xff
    if k == 27:
        break

Upvotes: 0

Views: 102

Answers (1)

XerXeX
XerXeX

Reputation: 792

I see multiple problems with your code.

  1. It will always return to something() because your arduino code runs faster than your python code, so your arduino checks for the Serial.available() more often than the python code can output a new character. Therefore, when it don't see a new character in the buffer it will return to something(). Try to add a small delay in the track() function in the end to overcome this.

  2. You never update prevX and prevY, therefore if (prevX != x || prevY != y) is executed every time. Also this part servoX = min(servoX, 180); servoX = max(servoX, 0); servoY = min(servoY, 180); servoY = max(servoY, 0);

    really isn't doing anything, it will always output the originally value of serveoX and servoY

But find out how long the python code takes to execute and add a delay a little longer than that in the track() function.

Upvotes: 1

Related Questions