Python to Arduino through Serial, can’t move servos

Question

I'm currently trying to have a camera mounted on a pan tilt made of two micro servos track a face. My python code has been working and has been successfully identifying a face, but non of my servos have been moving while the Arduino is constantly flashing as if it is receiving input from the Python code. I haven't been able to get the servos to move according to my python code, but I have made simple code on the side to make sure the servos have good connections and they work fine on their own. I'm not sure what is wrong...

Python Code

import numpy as np6
import serial
import time
import sys
import cv2

arduino = serial.Serial('COM3', 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()
    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: f} Y {1: f} Z” .format (xx, yy))
        print ("output = '" +data+ "'")
        arduino.write(data.encode())


    cv2.imshow('img',img)

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

Arduino Code

#include<Servo.h>

Servo servoVer; //Vertical Servo
Servo servoHor; //Horizontal Servo

int x;
int y;

int prevX;
int prevY;

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

void Pos()
{
  if(prevX != x || prevY != y)
  {
    int servoX = map(x, 600, 0, 70, 179); 
    int servoY = map(y, 450, 0, 179, 95); 

    servoX = min(servoX, 179);
    servoX = max(servoX, 70);
    servoY = min(servoY, 179);
    servoY = max(servoY, 95);

    servoHor.write(servoX);
    servoVer.write(servoY);
  }
}

void loop()
{
  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();
    }
  }
}

Assuming that your commands to your servos are working. Could you check the logic? Dummy the position and see if it is calling the correct tracking motion. — Jason Chia, Mar 04 '20 at 08:40

score 0 · Answer 1 · answered Mar 05 '20 at 02:33

One huge problem is the way you are using Serial.read. That function consumes the character out of the buffer. You don't get to read the same one twice. So let's say you send a 'Y'. The first if statement reads the Y out of the serial buffer and compares to 'X', that's false so it moves on. Then it reads something else from serial, probably a -1 if nothing is left to read. But it doesn't see the 'Y' because that was read in the first if.

What you need to do is to read from serial into a char variable and then use that char variable in your if statements.

    char c = Serial.read();
    if(c == 'X')...

...  if (c == 'Y')...

Python to Arduino through Serial, can’t move servos

1 Answers1