Commit 7eec0e33 authored by William Michalski's avatar William Michalski
Browse files

Initial commit

parents
# -*- coding: utf-8 -*-
"""
Created on Mon Mar 22 17:11:24 2021
@author: Thomas
Programme de simulation de la loi de commande u et de la sortie y du systeme en
fonction d'une consigne yc
"""
import numpy as np
import matplotlib.pyplot as plt
plt.rcParams["figure.figsize"] = (15,8)
### PARAMETRES MODELE ET PERFORMANCES ASSERVISSEMENT
kb = 6.1; # Parametre de la modelisation du systeme
Te = 0.1; # Periode d'echantillonnage
alpha = 0.5; # Double zero polynome C(z) = (z-alpha)**2 (D(z) = C(z)*P(z))
tps_rep = 1.0; # Temps de reponse
tau = tps_rep/5.0; # Constante de temps tau
z1 = np.exp(-Te/tau); # Double zero polynome P(z) = (z-z1)**2 (double pole de la boucle fermee)
### COEFFICIENTS POLYNOMES
K = kb*Te*Te/2.0; # Gain du systeme echantillone
# Coefs polynome S
s0 = 1.0;
s1 = 2.0 - 2.0*(alpha + z1);
s2 = (1.0/4.0)*(z1*z1+4.0*z1*alpha+alpha*alpha-1.0+6.0*(1-alpha-z1)+2.0*z1*alpha*(alpha+z1)+z1*z1*alpha*alpha);
# Coefs polynome R
r0 = (1.0/K)*(-2.0*z1*alpha*(alpha+z1)-2.0*(1-alpha-z1)+3.0*s2-z1*z1*alpha*alpha);
r1 = (1.0/K)*(z1*z1*alpha*alpha-s2);
Q = (r0 + r1)/((1-alpha)*(1-alpha));
### PARAMETRES SIMULATION
nb_pas = 50; # Nombre de pas (k) simules
simu = np.zeros((3, nb_pas)); # ligne 0 : yc, ligne 1 : u et ligne 2 : y
### FONCTIONS DE CALCUL DES ECHANTILLONS TEMPORELS
def u(k, saturation):
commande = ((-s1*simu[1,k-1] - s2*simu[1,k-2] + Q*simu[0,k] - 2*alpha*Q*simu[0,k-1] + Q*alpha*alpha*simu[0,k-2] - r0*simu[2,k-1] - r1*simu[2,k-2])/s0)
if(commande > saturation):
return saturation
elif (commande < -saturation):
return -saturation
else:
return commande
def y1(k):
return (2*z1*simu[2,k-1] - z1*z1*simu[2,k-2] + Q*K*(simu[0,k-1] + simu[0,k-2]))
def y2(k):
return (2*simu[2,k-1] - simu[2,k-2] + K*(simu[1,k-1] + simu[1,k-2]))
### SIMULATION ET CALCUL DES ECHANTILLONS TEMPORELS
sat = 2; # saturation pour eviter que les moteurs casseny la maquette
# Entree (echelon)
for i in range(5, nb_pas-1):
simu[0,i] = 1
"""
# Simulation de y en fonction de yc uniquement
# Sortie
for j in range(2, nb_pas-1):
simu[2,j] = y1(j)
# Commande
for k in range(2, nb_pas-1):
simu[1,k] = u(k,sat)
"""
# Simulation conjointe de u et y en fonction de yc
for k in range(2, nb_pas-1):
# Commande
simu[1,k] = u(k,sat)
# Sortie
simu[2,k] = y2(k)
### AFFICHAGE DE LA SIMULATION
plt.figure(1)
temps_k = np.linspace(0,nb_pas-3, nb_pas-3)
plt.step(temps_k, simu[2,2:-1])
plt.step(temps_k, simu[1,2:-1])
plt.xlabel("Temps discret k")
plt.ylabel("Amplitude")
plt.legend(["sortie y","commande u"])
plt.show()
\ No newline at end of file
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Thu Feb 18 16:20:04 2021
@author: william
@ref :
https://www.geeksforgeeks.org/multiple-color-detection-in-real-time-using-python-opencv/
https://learnopencv.com/find-center-of-blob-centroid-using-opencv-cpp-python/
https://docs.opencv.org/master/d9/d61/tutorial_py_morphological_ops.html
"""
import numpy as np
import cv2
import matplotlib
import matplotlib.pyplot as plt
def centroid(img):
M = cv2.moments(img)
if M["m00"] != 0:
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
else:
cX, cY = 0, 0
return cX, cY
img = cv2.imread("labyTest.jpg")
img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)#Espace HSV, plus simple de separer les couleurs
#Paramètre du seuillage par couleur, à adapter au besoin
red_lower = np.array([136, 50, 50], np.uint8)
red_upper = np.array([180, 255, 255], np.uint8)
green_lower = np.array([25, 52, 72], np.uint8)
green_upper = np.array([102, 255, 255], np.uint8)
blue_lower = np.array([94, 0, 0], np.uint8)
blue_upper = np.array([120, 255, 255], np.uint8)
#%% Seuillage par couleurs
img_red = cv2.inRange(img_hsv, red_lower, red_upper)
img_green = cv2.inRange(img_hsv, green_lower, green_upper)
img_blue = cv2.inRange(img_hsv, blue_lower, blue_upper)
#%% Ouverture et fermeture morphologique, pour éliminer bruit éventuel, à adapter au besoin
SE_opening = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (20,20))
#SE_closing = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5,5))
img_green_cln = cv2.morphologyEx(img_green, cv2.MORPH_OPEN, SE_opening)
img_green_cln = cv2.morphologyEx(img_green_cln, cv2.MORPH_OPEN, SE_opening)
plt.figure(1)
plt.subplot(1,2,1),plt.title("green before filter"), plt.imshow(img_green)
plt.subplot(1,2,2),plt.title("green after filter"), plt.imshow(img_green_cln)
#%% Detection du barycentre
#rX,rY = centroid(img_red)
gX,gY = centroid(img_green_cln)
#bX,bY = centroid(img_blue)
cv2.circle(img, (gX,gY), 4, (0,233,255),-1)#Ajout d'un point sur l'image d'origine, pour montrer ou ce trouve le centre detecte
#%%
plt.plot()
plt.figure(2)
plt.subplot(1,4,1),plt.title("maze"), plt.imshow(img)
plt.subplot(1,4,2),plt.title("red"), plt.imshow(img_red)
plt.subplot(1,4,3),plt.title("green"), plt.imshow(img_green)
plt.subplot(1,4,4),plt.title("blue"), plt.imshow(img_blue)
#%%
plt.figure(3)
plt.imshow(img)
cv2.imwrite("testLaby_res.jpg",img)
\ No newline at end of file
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Sun Feb 21 09:10:51 2021
@author: william
"""
import numpy as np
import cv2
import matplotlib
import matplotlib.pyplot as plt
def centroid(img):
M = cv2.moments(img)
if M["m00"] != 0:
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
else:
cX, cY = 0, 0
return cX, cY
img = cv2.imread("labyTest.jpg")
#%% Premier recadrage, valeur pré-définie, taille maximal du laby, soit du support
# h,l = 2700, 1800 # A définir
# x,y, _ = img.shape
# img = img[(x-h)//2:(x+h)//2, (y-l)//2:(y+l)//2]
img = img[530:2175,550:1885]
#%% Création d'un masque pour éliminer le départ et l'arrivée (on conserve leurs positions cependant)
img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
red_lower = np.array([136, 50, 50], np.uint8)
red_upper = np.array([180, 255, 255], np.uint8)
green_lower = np.array([25, 52, 72], np.uint8)
green_upper = np.array([102, 255, 255], np.uint8)
img_red = cv2.inRange(img_hsv, red_lower, red_upper)
img_green = cv2.inRange(img_hsv, green_lower, green_upper)
SE_opening = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (20,20))
#SE_closing = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5,5))
SE_dilate = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (40,40))
img_red = cv2.morphologyEx(img_red, cv2.MORPH_OPEN, SE_opening)
img_green = cv2.morphologyEx(img_green, cv2.MORPH_OPEN, SE_opening)
#Calcul du barycentre
rX,rY = centroid(img_red)
gX,gY = centroid(img_green)
#On aggrandit l'objet pour être sur de capturer toute la bille
img_red = cv2.morphologyEx(img_red, cv2.MORPH_DILATE, SE_dilate)
img_green = cv2.morphologyEx(img_green, cv2.MORPH_DILATE, SE_dilate)
mask = cv2.bitwise_or(img_red,img_green)
#%%
plt.figure(1)
plt.subplot(1,4,1),plt.title("original"), plt.imshow(img)
plt.subplot(1,4,2),plt.title("red channel"), plt.imshow(img_red)
plt.subplot(1,4,3),plt.title("green channel"), plt.imshow(img_green)
plt.subplot(1,4,4),plt.title("mask"), plt.imshow(mask)
#%%
img_bin = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
th, img_th = cv2.threshold(img_bin, 0 , 255, cv2.THRESH_BINARY+cv2.THRESH_OTSU)
print("Threshold = " + str(th))
plt.figure(2), plt.title("Thresholded img"), plt.imshow(img_th)
#Application du masque
img_masked = cv2.bitwise_or(img_th,mask)
#%%
plt.figure(3), plt.title("masked"), plt.imshow(img_masked)
cv2.imwrite("testLaby_masked.jpg",img_masked)
#%%
contours , hierarchy = cv2.findContours(img_masked, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
imgc = cv2.drawContours(img_masked, contours, -1, 0, 40)
plt.figure(4), plt.title("Contours"),plt.imshow(imgc)
cv2.imwrite("testLaby_bin.jpg",imgc)
#%%
# -*- coding: utf-8 -*-
"""
Calcul de la loi de commande u en fonction de la sortie y du systeme et
de la consigne yc
"""
import numpy as np
import matplotlib.pyplot as plt
import time
plt.rcParams["figure.figsize"] = (15,8)
### PARAMETRES MODELE ET PERFORMANCES ASSERVISSEMENT
kb = 6.1; # Parametre de la modelisation du systeme
Te = 0.1; # Periode d'echantillonnage
alpha = 0.5; # Double zero polynome C(z) = (z-alpha)**2 (D(z) = C(z)*P(z))
tps_rep = 1.0; # Temps de reponse
tau = tps_rep/5.0; # Constante de temps tau
z1 = np.exp(-Te/tau); # Double zero polynome P(z) = (z-z1)**2 (double pole de la boucle fermee)
### COEFFICIENTS POLYNOMES
K = kb*Te*Te/2.0; # Gain du systeme echantillone
# Coefs polynome S
s0 = 1.0;
s1 = 2.0 - 2.0*(alpha + z1);
s2 = (1.0/4.0)*(z1*z1+4.0*z1*alpha+alpha*alpha-1.0+6.0*(1-alpha-z1)+2.0*z1*alpha*(alpha+z1)+z1*z1*alpha*alpha);
# Coefs polynome R
r0 = (1.0/K)*(-2.0*z1*alpha*(alpha+z1)-2.0*(1-alpha-z1)+3.0*s2-z1*z1*alpha*alpha);
r1 = (1.0/K)*(z1*z1*alpha*alpha-s2);
Q = (r0 + r1)/((1-alpha)*(1-alpha));
### PARAMETRES SIMULATION
nb_pas = 3; # Nombre de pas (k) simules
simu = np.zeros((3, nb_pas)); # ligne 0 : yc, ligne 1 : u et ligne 2 : y
#[[yc[k-2],yc[k-1],y[k]],
# [u[k-2], u[k-1], u[k]],
# [y[k-2], y[k-1], y[k]]]
### FONCTIONS DE CALCUL DES ECHANTILLONS TEMPORELS
def u(k, saturation):
commande = ((-s1*simu[1,k-1] - s2*simu[1,k-2] + Q*simu[0,k] - 2*alpha*Q*simu[0,k-1] + Q*alpha*alpha*simu[0,k-2] - r0*simu[2,k-1] - r1*simu[2,k-2])/s0)
if(commande > saturation):
return saturation
elif (commande < -saturation):
return -saturation
else:
return commande
def y1(k):
return (2*z1*simu[2,k-1] - z1*z1*simu[2,k-2] + Q*K*(simu[0,k-1] + simu[0,k-2]))
def y2(k):
return (2*simu[2,k-1] - simu[2,k-2] + K*(simu[1,k-1] + simu[1,k-2]))
def swap():
simu[0,0] = simu[0,1]
simu[0,1] = simu[0,2]
simu[1,0] = simu[1,1]
simu[1,1] = simu[1,2]
simu[2,0] = simu[2,1]
simu[2,1] = simu[2,2]
return;
### SIMULATION ET CALCUL DES ECHANTILLONS TEMPORELS
sat = 2; # saturation pour eviter que les moteurs casseny la maquette
consigne = input()
while(True):
# Entree (echelon)
simu[0,2] = consigne
# Simulation conjointe de u et y en fonction de yc
# Commande
simu[1,2] = u(2,sat)
# Sortie
simu[2,2] = y2(2)
#simu[2,2] = resultat mesure position de la bille
print('yc = ', simu[0,2], ', u = ', simu[1,2], ' et y = ', simu[2,2])
time.sleep(0.1)
swap()
import serial
import time
if __name__ == '__main__':
ser = serial.Serial('/dev/ttyACM0', 57600, timeout = 1) # Connection a l'Arduino
ser.flush()
pos_mid = 512; # Correspond a 0 degree en inclinaison
range_max = 100; # Inclinaison maximale en nombre de pas (1024 pas au total pour 300 degrees)
pos_min = pos_mid - range_max;
pos_max = pos_mid + range_max;
pos = pos_mid
time.sleep(1) # Pour la synchronisation des communications
while True:
# calcul de pos1 et pos2... (entre 0 et 1023)
# pos = pos1 + pos2*1024
ser.write((str(pos)+'\0').encode('utf-8'))
time.sleep(0.08)
/*
Communication avec le Raspberry Pi
*/
#include "AX12A.h"
#define DirectionPin (10)
#define BaudRate (1000000ul)
#define ID1 (1u)
#define ID2 (2u)
int initial_pos = 512; // Correspond a une inclinaison de 0 degree
int range_max = 100; // Inclinaison maximale en nombre de pas (1024 pas au total pour 300 degrees)
int pos_max = initial_pos + range_max;
int pos_min = initial_pos - range_max;
int pos = initial_pos;
int pos1 = initial_pos;
int pos2 = initial_pos;
// Fonction de rotation syncrone des deux moteurs (version avec plusieurs fonctions)
void moveSync2(unsigned char id1, unsigned char id2, int Position1, int Position2)
{
ax12a.moveRW(id1, Position1);
ax12a.moveRW(id2, Position2);
ax12a.action();
return;
}
void setup()
{
Serial.begin(57600); // Demarrage communication serie avec le Raspberry Pie
Serial.setTimeout(600); // Temps maximale avant de passer a la reception d'une autre valeur entiere
ax12a.begin(BaudRate, DirectionPin, &Serial1); // Demarrage communication serie avec les servomoteurs
}
void loop()
{
if (Serial.available() > 0)
{
pos = Serial.parseInt(SKIP_ALL, '\n'); // Recuperation de la consigne
//Serial.print("You sent me the position ");
//Serial.println(pos, DEC);
pos1 = pos & 0x3FF; // Recuperation des deux positions
pos2 = (pos / 1024) & 0x3FF; // envoyees avec un seul entier
// Saturation forcee pour ne pas briser la structure
if(pos1 == 0){pos1 = initial_pos;}
if(pos1 < pos_min){pos1 = pos_min;}
if(pos1 > pos_max){pos1 = pos_max;}
if(pos2 == 0){pos2 = initial_pos;}
if(pos2 < pos_min){pos2 = pos_min;}
if(pos2 > pos_max){pos2 = pos_max;}
ax12a.moveSync(ID1, ID2, pos1, pos2);
//moveSync2(ID1, ID2, pos1, pos2);
//ax12a.move(ID1, pos1);
//ax12a.move(ID2, pos2);
//delay(5);
}
}
# -*- coding: utf-8 -*-
"""
Created on
@author:
"""
import matplotlib.pyplot as plt
import scipy.signal as si
import numpy as np
import math as m
import skimage.io as skio
import skimage as sk
from skimage.transform import hough_line, hough_line_peaks, rotate
plt.rcParams['image.cmap'] = 'gray' #A supprimer plus tard, j'ai juste fait en niveau de gris pour le moment
lecture = skio.imread('image_test_1.png') #Nom de mon image de test (note : le rectangle n'a pas des angles parfaitement droits)
image = sk.img_as_float(lecture)
Taille_image = np.shape(image)
# --- Correction angle de rotation de l'image --- #
# On remet l'image droite de sorte que les bords du plateau soient parallèles #
# aux bords de l'image -> tranformée de Hough (voir séquence 4 exercice 3) #
#Les filtres pour déterminer les gradients
Sobel_X =1/4 * np.array([[-1,0,1],[-2,0,2],[-1,0,1]])
Sobel_Y =1/4 * np.array([[-1,-2,-1],[0,0,0],[1,2,1]])
#Luminence, pas besoin de la couleur pour pivoter l'image
image_L=0.3*image[:,:,0].copy()+0.58*image[:,:,1].copy()+0.11*image[:,:,2].copy()
#Calcul des gradients 1D
Gx_image = si.convolve2d(image_L, Sobel_X, mode='same', boundary='symm', fillvalue=0)
Gy_image = si.convolve2d(image_L, Sobel_Y, mode='same', boundary='symm', fillvalue=0)
#Calcul du gradient et de theta
G=np.zeros([Taille_image[0],Taille_image[1]])
T=np.zeros([Taille_image[0],Taille_image[1]])
G = (Gx_image**2 + Gy_image**2)**(1/2)
for i in range(Taille_image[0]):
for j in range(Taille_image[1]):
T[i,j] = m.atan(Gy_image[i,j]/(Gx_image[i,j]+0.001))
#Affichage pour vérifier que tout se passe bien
plt.subplot(2,2,1)
plt.imshow(Gx_image)
plt.title('Gx')
plt.subplot(2,2,2)
plt.imshow(Gy_image)
plt.title('Gy')
plt.subplot(2,2,3)
plt.imshow(T)
plt.title('T')
plt.subplot(2,2,4)
plt.imshow(G)
plt.title('G')
#Threshold pour obtenir les contours
G_thresh = G>0.2
#Tranformée de Hough
accu, angles, distances = hough_line(G_thresh)
p_accu, p_angles, p_distances = hough_line_peaks(accu, angles, distances)
#Détermination de l'angle de rotation et rotation de l'image
rota = min(abs(p_angles))*360/(2*m.pi)
image_rota = rotate(image, angle=rota)
#Affichage pour vérifier que tout se passe bien
plt.figure(2)
plt.subplot(2,2,1)
plt.imshow(G_thresh)
plt.title('Contours')
plt.subplot(2,2,2)
plt.imshow(accu)
plt.title("matrice d'accumulation")
plt.subplot(2,2,3)
plt.imshow(image_rota)
plt.title('Image pivotée')
# --- Caractérisation du labyrinthe --- #
# Identifier les murs, les trous et les points de départ et d'arrivée et les #