Yeux animatroniques (avec Pi caméra ) avec suivi de couleur. Version ok et testée.

Description :

Yeux animatroniques pilotés à l’aide d’une PI caméra et Open CV pour le suivi de 2 couleurs (vert et jaune).

Un microcontrôleur Arduino est chargé d’orienter les servos grâce à la réception de coordonnées envoyées grâce à un code python.

La conception des yeux avec la caméra embarquée a été faite de façon à ce que chaque assemblage dispose d’un écrou et d’une vis afin de rendre le montage fiable et fluide dans le temps. Chaque pièce a été faite pour qu’elle soit facilement imprimable sur une imprimante résine voir filament (test pas encore effectué). Tous les fichiers sont disponibles ci-dessous en téléchargement.

Vidéo de démonstration :

Composants nécessaires :

Arduino Nano
Carte d’extension pour module de contrôle Nano
1 PCA9685
Des fils de connexion
1 alimentation 5v
4 vis M3x16
8 vis M3x8
4 vis M3x10
16 écrous hexagonal M3
14 Vis tête ronde auto taraudeuse M1.7X6
1 imprimante 3d résine ( imprimante filament test pas encore effectué)
1 Raspberry PI 3
1 Camera PI v2.1
1 LED RGB
3 résistances 220Ω
8 Servos Tower pro 9g sg90 (attention à la sortie des fils voir ci-dessous)

Fichiers SolidWorks et STL :

SDW-STL yeux animatronique Télécharger

Bibliothèque nécessaire :

#include « Adafruit_PWMServoDriver.h »

https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library

Logiciels nécessaires pour le Raspberry :

Python 2.7
OpenCV
pyserial
NumPy

Schéma de câblage :

Attention aux câblages des servos et à leur nom.

Par exemple le servo 2 est branché sur la sortie 3 du PCA9685.

Résumé :

Servo 0 –> Pin 0
Servo 1 –> Pin 7
Servo 2 –> Pin 3
Servo 3 –> Pin 4
Servo 4 –> Pin 1
Servo 5 –> Pin 2
Servo 6 –> Pin 5
Servo 7 –> Pin 6

Codes pour positionner les servos avant assemblage final :

#include "Wire.h"
#include "Adafruit_PWMServoDriver.h"
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);

#define MIN_PULSE_WIDTH 600
#define MAX_PULSE_WIDTH 2600
#define FREQUENCY 50

int analogPin = A0; 
int valeur = 0; 
 
void setup() {
  Serial.begin(9600);
  pwm.begin();
  pwm.setPWMFreq(FREQUENCY);
}

int pulseWidth(int angle) {
  int pulse_wide, analog_value;
  pulse_wide = map(angle, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
  analog_value = int(float(pulse_wide) / 1000000 * FREQUENCY * 4096);
  return analog_value;
}

void loop() {  
  valeur = analogRead(analogPin);  
  Serial.println(valeur);  
  int xd = map(valeur, 0, 1023,180, 110);
  int xg = map(valeur, 0, 1023,0, 70);
  int yd = map(valeur, 0, 1023,0, 70);
  int yg = map(valeur, 0, 1023,180, 110);
  pwm.setPWM(0, 0, pulseWidth(90));  //Servo 0
  pwm.setPWM(7, 0, pulseWidth(90));  //Servo 1
  pwm.setPWM(3, 0, pulseWidth(180)); //Servo 2
  pwm.setPWM(4, 0, pulseWidth(0));   //Servo 3
  pwm.setPWM(1, 0, pulseWidth(0));   //Servo 4
  pwm.setPWM(2, 0, pulseWidth(180)); //Servo 5
  pwm.setPWM(5, 0, pulseWidth(0));   //Servo 6
  pwm.setPWM(6, 0, pulseWidth(180)); //Servo 7  
}

Vous devez obtenir cette position une fois l’assemblage fini.

Codes Arduino pour le suivi de couleur :

int pinservo[]={  0,  7,  3,  4,  1,  2,  5,  6};
float poservo[]={95, 95, 140, 40, 0, 180, 0, 180};
float poservoinit[]={95, 95, 140, 40, 30, 135, 45, 140};
float servomin[]={55, 55, 100, 0, 0, 110, 0, 110};
float servomax[]={135, 135, 180, 80, 70, 180, 70, 180};

#include "Wire.h"
#include "Adafruit_PWMServoDriver.h"
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);

const unsigned int MAX_MESSAGE_LENGTH = 12;

#define MIN_PULSE_WIDTH 600
#define MAX_PULSE_WIDTH 2600
#define FREQUENCY 50

const int rouge = 6;
const int vert = 5;
const int bleue = 3;
float x;        
float y;        
int c;          
long temps;     
int start = 0;  
long tempspaupiere;

void setup() {
  Serial.begin(115200);
  pinMode(rouge, OUTPUT);
  pinMode(vert, OUTPUT);
  pinMode(bleue, OUTPUT); 
  analogWrite(rouge, 10);
  pwm.begin();
  pwm.setPWMFreq(FREQUENCY);
  initservo();
  delay(2000);
  digitalWrite(rouge, LOW);
  temps = millis();
  tempspaupiere = millis();
}

int pulseWidth(int angle) {
  int pulse_wide, analog_value;
  pulse_wide = map(angle, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
  analog_value = int(float(pulse_wide) / 1000000 * FREQUENCY * 4096);
  return analog_value;
}

void loop() {
  if (Serial.available() > 0) { 
    start = 1 ;
    temps = millis(); 
    static char message[MAX_MESSAGE_LENGTH];
    static unsigned int message_pos = 0;
    char inByte = Serial.read();
      if(inByte == 'C'){
        inByte = Serial.read();
        while (Serial.available() > 0 && inByte != 'C'){
          message[message_pos] = inByte;
          message_pos++;
          inByte = Serial.read();
          delay(3);
        }
        message[message_pos] = ' ';
        char * morceau[4];
        char * p;
        int n = -1;
        p = strtok(message, ",");
        while (p != NULL) {
          morceau[++n] = p;
          p = strtok(NULL, ",");
        }
        x = (atoi(morceau[0]))/4;
        y = (atoi(morceau[1]))/2;;
        c = atoi(morceau[2]);
        message_pos = 0;
      }
    }
  if(start == 0){
    paupiere(); 
  }
  else{ 
    if(c == 1){
      analogWrite(vert, 10);
      analogWrite(rouge, 10);
      digitalWrite(bleue, LOW);
    }
    if(c == 2){
      analogWrite(vert, 10);
      digitalWrite(rouge, LOW);
      digitalWrite(bleue, LOW);
    }
    while(Serial.available() == 0 && (millis() - temps) < 1000) {
      if(x > 90) {  
        float posx = map(x, 90, 160, 1, 180);
        poservo[1] = poservo[1]+(posx/1000);  
        poservo[0] = poservo[0]+(posx/1000);
      }
      if(x < 70) { 
        float posx = map(x, 0, 70, 180, 1); 
        poservo[1] = poservo[1]-(posx/1000); 
        poservo[0] = poservo[0]-(posx/1000); 
      }   
      if(y > 130) {
        float posy = map(y, 130, 240, 1, 200);
        poservo[3] = poservo[3]+(posy/1000); 
        poservo[2] = poservo[2]-(posy/1000); 
      } 
      if(y < 110) {
        float posy = map(y, 0, 110, 200, 1);
        poservo[3] = poservo[3]-(posy/1000);
        poservo[2] = poservo[2]+(posy/1000);  
      }
      for(int i= 0; i <= 3; i++) {
        if (poservo[i] >= servomax[i]) {
          poservo[i] = servomax[i];
        }
      }
      for(int i= 0; i <= 3; i++){
        if (poservo[i] <= servomin[i]) {
          poservo[i] = servomin[i];
        }
      } 
      posservo();
      paupiere();
    } 
    while(Serial.available() == 0) {
      analogWrite(rouge, 10);
      digitalWrite(vert, LOW);
      digitalWrite(bleue, LOW);
      for(int i= 0; i <= 3; i++) {
        if(poservo[i] < poservoinit[i]-1){  
        poservo[i] = poservo[i]+0.1;      
        } 
      }
      for(int i= 0; i <= 3; i++) {
        if(poservo[i] > poservoinit[i]+1){ 
        poservo[i]= poservo[i]-0.1;
        }
      } 
      posservo();
      if(start == 1 && poservo[1] > poservoinit[1]-1 &&  poservo[0] > poservoinit[0]-1 && poservo[1] < poservoinit[1]+1 && poservo[1] < poservoinit[1]+1 && poservo[3] > poservoinit[3]-1 && poservo[2] > poservoinit[2]-1 && poservo[3] < poservoinit[3]+1 && poservo[2] < poservoinit[2]+1){
        start = 0;
      }
    } 
  }  
}

void posservo(){
  for(int i= 0; i <= 3; i++){
    if(poservo[i] < servomax[i] && poservo[i] > servomin[i]){  
      pwm.setPWM(pinservo[i], 0, pulseWidth(poservo[i]));   
    }
  } 
  paupiere();  
}

void initservo(){
  for(int i= 0; i <= 7; i++){
    pwm.setPWM(pinservo[i], 0, pulseWidth(poservoinit[i])); 
  }
}

void paupiere(){
  if(((millis() - tempspaupiere) > 3500 && start == 0) || (x < 90 && x > 70 && y < 130 && y > 110 && (millis() - tempspaupiere) > 3500)){
    for(int i= 4; i <= 7; i++){
      pwm.setPWM(pinservo[i], 0, pulseWidth(poservo[i]));
    }  
    delay(200);
    for(int i= 4; i <= 7; i++){
      pwm.setPWM(pinservo[i], 0, pulseWidth(poservoinit[i]));
    }  
    tempspaupiere = millis();
  }    
}

Codes Python pour le suivi de couleur :

import cv2
import numpy as np
import picamera
import picamera.array
import time
import sys
import serial

lo_jaune=np.array([20, 100, 100])
hi_jaune=np.array([30, 255, 255])
color_info_jaune=(30, 255, 255)
lo_vert=np.array([40, 100, 100])
hi_vert=np.array([70, 255, 255])
color_info_vert=(88, 255, 51)
WIDTH=640
HEIGHT=480

arduino = serial.Serial('/dev/ttyUSB0', 115200)
time.sleep(2)
print("Connected to Arduino...")

with picamera.PiCamera() as camera:
    with picamera.array.PiRGBArray(camera) as stream:
        camera.resolution=(WIDTH, HEIGHT)
        camera.rotation = 180
        
        while True:
            camera.capture(stream, 'bgr', use_video_port=True)
            image=cv2.cvtColor(stream.array, cv2.COLOR_BGR2HSV)
            mask_jaune=cv2.inRange(image, lo_jaune, hi_jaune)
            mask_vert=cv2.inRange(image, lo_vert, hi_vert)
            image_jaune=cv2.blur(image, (7, 7))
            image_vert=cv2.blur(image, (7, 7))
            mask_jaune=cv2.erode(mask_jaune, None, iterations=4)
            mask_vert=cv2.erode(mask_vert, None, iterations=4)
            mask_jaune=cv2.dilate(mask_jaune, None, iterations=4)
            mask_vert=cv2.dilate(mask_vert, None, iterations=4)
            image_jaune=cv2.bitwise_and(stream.array, stream.array, mask=mask_jaune) 
            image_vert=cv2.bitwise_and(stream.array, stream.array, mask=mask_vert)
            element_jaune=cv2.findContours(mask_jaune, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]     
            elements_vert=cv2.findContours(mask_vert, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] 
            if len(element_jaune) > 0:
                c=max(element_jaune, key=cv2.contourArea)
                ((x, y), rayon)=cv2.minEnclosingCircle(c)
                
                if rayon>10:
                    cv2.circle(image_jaune, (int(x), int(y)), int(rayon), color_info_jaune, 2)
                    cv2.circle(stream.array, (int(x), int(y)), 5, color_info_jaune, 10)
                    cv2.line(stream.array, (int(x), int(y)), (int(x)+150, int(y)), color_info_jaune, 2)
                    cv2.putText(stream.array, "jaune", (int(x)+10, int(y) -10), cv2.FONT_HERSHEY_DUPLEX, 1, color_info_jaune, 1, cv2.LINE_AA)
            
                print ('X :' +str(x))
                print ('Y :'+str(y))
                xx = int(x)
                yy = int(y)
                cc = 1
                print (xx)
                print (yy)
                data = "C{0:d},{1:d},{2:d} ".format(xx, yy, cc)
                print ("output = '" +data+ "'")
                arduino.write(data.encode())
                time.sleep(0.04)
                
            if len(elements_vert) > 0:
                c=max(elements_vert, key=cv2.contourArea)
                ((x, y), rayon)=cv2.minEnclosingCircle(c)
                
                if rayon>10:
                    cv2.circle(image_vert, (int(x), int(y)), int(rayon), color_info_vert, 2)
                    cv2.circle(stream.array, (int(x), int(y)), 5, color_info_vert, 10)
                    cv2.line(stream.array, (int(x), int(y)), (int(x)+150, int(y)), color_info_vert, 2)
                    cv2.putText(stream.array, "vert", (int(x)+10, int(y) -10), cv2.FONT_HERSHEY_DUPLEX, 1, color_info_vert, 1, cv2.LINE_AA)
            
                print ('X :' +str(x))
                print ('Y :'+str(y))
                xx = int(x)
                yy = int(y)
                cc = 2
                print (xx)
                print (yy)
                data = "C{0:d},{1:d},{2:d} ".format(xx, yy, cc)
                print ("output = '" +data+ "'") 
                arduino.write(data.encode())
                time.sleep(0.04)
                
                
            cv2.line(stream.array,(640,240),(0,240),(0,255,0),1)
            cv2.line(stream.array,(320,0),(320,480),(0,255,0),1)    
            cv2.imshow('Camera', stream.array)
            #cv2.waitKey(10)
            #cv2.imshow('image_jaune', image_jaune)
            #cv2.imshow('image_vert', image_vert)
            #cv2.imshow('Mask_jaune', mask_jaune)
            #cv2.imshow('Mask_vert', mask_vert)
            if cv2.waitKey(1)&0xFF==ord('q'):
                break
            stream.seek(0)
            stream.truncate()                        
cv2.destroyAllWindows()