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 :

 

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()

 

Photos :

 


 

Laisser un commentaire